Extended Kalman Filter

What it did

Track a single moving vehicle from a mix of LIDAR (2D position only) and RADAR (range, bearing, range-rate) measurements, using an Extended Kalman Filter. Output a smoothed trajectory whose RMSE against ground truth had to clear the rubric thresholds:

• px ≤ 0.11, py ≤ 0.11
• vx ≤ 0.52, vy ≤ 0.52

Implemented in C++ talking to Udacity’s term-2 simulator over uWebSockets.

The math, abridged

The state is [px, py, vx, vy]. Constant-velocity motion model:

F = [[1, 0, Δt, 0],
     [0, 1, 0, Δt],
     [0, 0, 1,  0],
     [0, 0, 0,  1]]

x' = F · x
P' = F · P · Fᵀ + Q

LIDAR observes [px, py] directly — measurement function H is linear, plain Kalman update.

RADAR observes [ρ, φ, ρ̇] — measurement function is non-linear:

ρ  = √(px² + py²)
φ  = atan2(py, px)
ρ̇  = (px·vx + py·vy) / ρ

Hence “Extended” — linearize via the Jacobian H_j around the current estimate, then apply the standard update equations with H_j in place of H.

What was actually tricky

• Sensor timing. LIDAR and RADAR arrive interleaved at different rates; Δt between updates is variable. Forgetting that is the fastest way to a divergent filter.
• φ normalization. Bearing wraps at ±π. After the innovation step, y = z - h(x) can produce a y_φ outside [-π, π], which the update then over-corrects against. One while (y_φ > π) y_φ -= 2π fix saves the entire filter.
• Initialization from the first measurement. Doing it wrong means the filter takes many steps to converge — and the rubric’s RMSE is cumulative. Initializing position from the first LIDAR (or polar→ Cartesian from the first RADAR) before any update is non-obvious.

What I’d do differently with hindsight

• Use an Unscented Kalman Filter. The UKF skips the Jacobian entirely (deterministic sampling of sigma points through the non-linear function), is simpler to implement once you have the cholesky tooling, and tracks heading discontinuities better than the EKF does.
• Add yaw to the state. A constant-turn-rate-velocity (CTRV) model with state [px, py, v, ψ, ψ̇] matches real vehicle dynamics better than constant-velocity Cartesian. The Udacity UKF project ships with exactly this.
• Factor the math from the wire. The uWebSockets glue + the EKF math + the simulator-protocol parsing are three concerns smeared across one file. Splitting them into tracker/, transport/, and main.cpp makes the EKF unit-testable.

What it taught me

This was the first SDC project where the math wasn’t a hidden detail behind a framework call. Writing out P' = F P Fᵀ + Q by hand with Eigen — and watching the state covariance shrink as measurements roll in — is what made probabilistic state estimation feel concrete. Everything later in the nanodegree (the [UKF particle filter, the particle filter localizer, even the path-planning behavior layer) built on the same intuition.