Optimal State Estimation & Sensor Fusion

Interactive Kalman Filter & Sensor Fusion Sandbox

Master linear Kalman filtering and optimal recursive estimation. Fuse noisy odometry with GPS measurements, observe Kalman Gain $K(t)$ convergence, and explore dynamic 2D error covariance uncertainty ellipses.

POSITION ESTIMATION (TRUE vs MEASURED vs FILTERED) ±2σ Confidence Band

KALMAN GAIN K(t) & ESTIMATION RESIDUAL (INNOVATION) Steady-State Gain K_∞

Kalman Gain K

0.000

Estimation Uncertainty (√P)

±0.00 m

Filter RMSE (Accuracy)

0.00 m

Noise Reduction vs Raw

0.0%

True Physical State

Noisy Raw Sensor (GPS)

Kalman Filter Estimate

2σ Uncertainty Envelope

🔍 Live Equation State (Frame 0)

1. Predict State: x̂⁻ = F · x̂ + B · u = 0.00

2. Predict Covariance: P⁻ = F · P · Fᵀ + Q = 0.00

3. Kalman Gain: K = P⁻ · Hᵀ / (H · P⁻ · Hᵀ + R) = 0.000

4. Update State: x̂ = x̂⁻ + K · (z - H · x̂⁻) = 0.00

5. Update Covariance: P = (I - K · H) · P⁻ = 0.00

🎛️ Covariance Matrices (Q & R)

Process Noise Covariance (Q) 0.050

System physics uncertainty / model inaccuracy

Measurement Noise Covariance (R) 1.50

Sensor measurement variance σ_z² (noise power)

Initial Estimate Covariance (P₀) 5.0

Confidence in initial robot pose

🚗 Physical Dynamics & Trajectory

2D Target Trajectory Pattern

Simulation Speed 1.0x

💡 Tuning Scenarios

📐 The 5 Golden Kalman Equations & ROS 2 Sensor Fusion

1. Time Update (Predict)

Project state forward in time using internal physical kinematic model:

\hat{x}_k^- = F_k \hat{x}_{k-1} + B_k u_k
P_k^- = F_k P_{k-1} F_k^T + Q_k

The process noise covariance $Q$ adds uncertainty to the prior error covariance $P^-$ at every timestep. In dead-reckoning without external sensor updates, $P^-$ grows indefinitely.

2. Measurement Update (Correct)

Compute optimal Kalman Gain $K_k$ and correct the state estimate:

K_k = P_k^- H_k^T \left(H_k P_k^- H_k^T + R_k\right)^{-1}
\hat{x}_k = \hat{x}_k^- + K_k \left(z_k - H_k \hat{x}_k^-\right)
P_k = (I - K_k H_k) P_k^-

When sensor measurement noise $R \to 0$, $K \to 1$ (complete belief in sensor). When $R \to \infty$, $K \to 0$ (sensor ignored; belief stays with physical model).

3. ROS 2 robot_localization Integration

In modern autonomous mobile robotics (ROS 2), the robot_localization package runs dual Extended Kalman Filters (EKF):

Local EKF: Fuses continuous high-frequency wheel odometry (encoders) with IMU angular velocity at 50Hz to publish a smooth, drift-free odom -> base_link transform.
Global EKF: Fuses local odometry with low-frequency, noisy RTK-GPS or visual SLAM landmarks to publish absolute georeferenced map -> odom poses without jumps.