State Estimation for UAV Navigation

Overview

For KTH EL2320 Applied Estimation, I implemented and compared an Extended Kalman Filter (EKF), a particle filter with augmented Monte Carlo localization (MCL), and a heuristic hybrid EKF-PF in an automated Unity drone simulation.

The project asks how local tracking accuracy, recovery from a sudden position jump, and particle count affect estimator behavior. Automated flight routes and synchronized logs support repeatable comparisons under controlled sensor-noise settings.

All figures below are extracted directly from my course report, Comparative Analysis of EKF and Particle Filter for UAV Localization via Automated Unity Simulation (January 29, 2026).

Original Unity screenshot with the drone, feature rays, ground-truth and estimated trajectories, and particle cloud. — Figure 1 from the report (p. 3): runtime visualization in Unity. Green rays show feature measurements, white and blue lines show ground truth and estimated trajectories, and red dots represent particles.

Simulation Pipeline

The simulation builds on the map and drone dynamics provided for KTH DD2438. I added sensor simulation, estimator implementations, and an automated ExperimentManager for the EL2320 study. Reference routes use Dijkstra’s algorithm and Bezier smoothing. Noisy GPS-like observations and eight ray-cast LiDAR beams provide position and map-feature measurements.

Ground-truth and estimated poses are recorded at 50 Hz and exported to synchronized CSV logs. The planar state contains position, heading, and velocity:

x = [px, pz, theta, vx, vz]^T

Here, px and pz are ground-plane positions, theta is heading, and vx and vz are planar velocities. The implementation follows Unity’s Y-up, Z-forward convention and uses C# with MathNet.Numerics for matrix operations.

Estimator Comparison

Extended Kalman Filter

The EKF propagates a Gaussian state estimate and uses analytical Jacobians for nonlinear motion and observation updates. In the tested low-noise setting it provides smooth local tracking, while a large position discontinuity can invalidate its local approximation.

Augmented Monte Carlo Localization

MCL represents uncertainty using weighted particles. Slow and fast averages of observation likelihood determine when to inject random particles. After localization failure, this mechanism can introduce hypotheses beyond the current local belief.

Original Unity screenshot of particles dispersed across the map during augmented MCL recovery. — Figure 2 from the report (p. 4): particle dispersion during recovery from a kidnapped-robot event. Global particle injection allows the estimator to search beyond its previous location.

Hybrid EKF-PF

Each particle maintains a local EKF estimate. A weighted combination of the particle state and its EKF correction guides hypotheses toward likely states. This is a heuristic fusion rule, rather than an exact Bayesian or Rao-Blackwellized formulation. The experiments test its sample efficiency and sensitivity to tuning.

Experimental Results

The report compares three scenarios using trajectory overlays and Euclidean position error over time. RMSE is defined as an accuracy metric; recovery is defined by the error returning below 5 m after a position jump. The figures below are the original experimental plots, including their legends, axes, and transient errors.

Low-noise baseline

With low LiDAR noise (approximately 0.1 m), the EKF tracks smoothly. The 50-particle PF exhibits stochastic jitter, while the 50-particle hybrid shows pronounced error spikes. Local EKF refinement does not consistently improve the particle estimator in this setting.

Original low-noise trajectory and error plots comparing EKF, PF with 50 particles, and hybrid filtering with 50 particles. — Figure 3 from the report (p. 6): low-noise baseline. The hybrid has substantial error spikes despite the smooth EKF estimate; the two panels show trajectories and position error over time.

Kidnapped-robot recovery

A scripted position jump at approximately 10 s tests recovery after the estimate becomes inconsistent with the true state. In the reported run, the EKF remains divergent and oscillatory. PF and hybrid estimates eventually return to low error through globally injected hypotheses, with substantial transients during recovery.

Original kidnapped-robot trajectory and error plots showing EKF divergence and eventual recovery of the PF and hybrid estimates. — Figure 4 from the report (p. 6): robustness to a position discontinuity. The full curves retain the recovery transients and show the different failure and recovery behaviors.

High noise and particle count

The final scenario compares high-noise estimation and a hybrid configuration reduced to 20 particles. After an initial warm-up period, the reduced-particle hybrid maintains tracking in the tested run. This supports investigating sample efficiency, while the baseline spikes show that tuning and stability remain concerns.

Original high-noise trajectory and error plots including the hybrid estimator with 20 particles. — Figure 5 from the report (p. 7): high-noise and sample-efficiency experiment. The 20-particle hybrid has a large initial transient before settling into tracking.

My Contribution

I developed the automated evaluation pipeline and sensor simulation on the course’s Unity environment, implemented the three estimators, and carried out the controlled comparisons and report analysis. The work includes deterministic route generation, 50 Hz CSV logging, augmented-MCL recovery, heuristic per-particle EKF refinement, and runtime visualization.

Findings and Limitations

The experiments reveal a trade-off between smooth local tracking, global recovery, and particle count. The EKF is effective in the low-noise tracking scenario but fails after the tested position jump. Augmented MCL supports recovery with more jitter. The hybrid can track with fewer particles in the high-noise test, but exhibits instability in the low-noise baseline.

These are results from controlled Unity experiments. The report does not establish that the hybrid is universally better, provide a theoretically exact fusion guarantee, or demonstrate deployment on a physical UAV. Lower particle count alone also does not establish a proportional runtime improvement when each particle runs an EKF.