UAV Multi-Sensor Tracker: Camera + Range Fusion via EKF

Fuses a camera-derived bearing measurement (with a calibrated distortion model) and a range sensor (LiDAR/radar-style) into a single, continuous 3D UAV track using an Extended Kalman Filter. Built to address the state estimation, sensor fusion, and camera modelling problems described in Augur's Robotics Engineer role.

Problem

A single sensor gives an incomplete or noisy picture of a target's position: a camera gives good bearing but no reliable depth; a range sensor gives depth but no bearing; both drop out under real conditions (occlusion, lighting, sensor failure). This project fuses both into one filtered estimate that stays continuous through dropout frames.

Approach

1. Camera model (python/camera_model.py) — pinhole projection with radial distortion coefficients (k1, k2). Detections are undistorted before being converted to a bearing angle, so the fusion step is not corrupted by lens distortion near the image edges.
2. Detector (python/detector.py) — YOLOv5 wrapper for real footage. For the demo, python/sensor_sim.py simulates a UAV trajectory with realistic camera and range noise plus 8% random detection dropout, so the pipeline runs without a live camera or GPU.
3. State estimation (python/ekf.py, mirrored in cpp/ekf.cpp) — a 6-state EKF (position + velocity, constant-velocity model) fusing nonlinear bearing/range measurements. The C++ version (cpp/ekf.cpp + cpp/test_ekf.cpp) is a standalone, dependency-light implementation using Eigen, built and tested independently of the Python demo.
4. Evaluation (python/fusion_pipeline.py, python/evaluate.py) — RMSE of raw detections vs EKF-filtered track, and error during detector dropout frames (predict-only) vs normal tracking frames.

Results (simulated trajectory, 150 frames, 8% dropout)

Metric	Raw detections	EKF-filtered
RMSE vs ground truth	0.767 m	0.465 m
Improvement	—	39.3%

During dropout frames the EKF predicts through the gap (mean error 0.59 m) instead of losing the track entirely, which the raw detection stream cannot do by definition — there is no raw reading on a dropout frame.

Run it

pip install -r requirements.txt
cd python
python fusion_pipeline.py   # runs sim + EKF, saves plot, prints RMSE
python evaluate.py          # dropout vs tracked error breakdown

C++ EKF standalone build and test:

cd cpp
mkdir build && cd build
cmake .. && make
./test_ekf

Why EKF over the geometric fusion used in my MSc dissertation

My dissertation (LiDAR + YOLOv5 counter-UAS prototype) used a geometric sensor model to convert detections directly into world-frame coordinates. That works per-frame but has no principled way to handle measurement noise over time, missed detections, or velocity estimation. The EKF here treats the problem properly: it maintains a probability distribution over the state, weights new measurements against prediction uncertainty, and degrades gracefully when a sensor drops out.

Limitations / next steps

• Constant-velocity motion model; a manoeuvring target (sharp turns) would need a higher-order model or an IMM (interacting multiple model) filter.
• Single-target only. Multi-target would need data association (nearest neighbour or JPDA) before the fusion step.
• EKF linearises around the current estimate; a UKF or particle filter would handle stronger nonlinearity if the sensor geometry gets more extreme (e.g. near-vertical elevation).
• Real camera intrinsics/distortion coefficients from a calibration checkerboard would replace the assumed values in camera_model.py.

Tech

Python, C++ (Eigen), OpenCV (camera model), YOLOv5 (detector, optional real video path), NumPy, Matplotlib.