Extended Kalman Filter Sensor Fusion

15-state EKF optimally combining INS, DVL, LBL, and depth sensors with real-time covariance evolution

About This Demo

The Extended Kalman Filter is the heart of the navigation system, fusing all sensor measurements into an optimal state estimate. Toggle sensors on/off to see how the filter adapts. Watch covariance evolution, Kalman gain dynamics, and innovation sequences in real-time. The 15-state error vector includes position, velocity, attitude errors plus gyro and accelerometer biases.

Sensor Configuration

Inertial Navigation System (INS)
ACTIVE
Doppler Velocity Log (DVL)
ACTIVE
Long Baseline Acoustic (LBL)
ACTIVE
Pressure Depth Sensor
ACTIVE

Position Error & Uncertainty Evolution

Kalman Gain Dynamics

Covariance Trace (Position States)

Measurement Innovation Sequence

EKF Theory

State Vector (15 states)

δx = [δpN δpE δpD δvN δvE δvD δψN δψE δψD bgx bgy bgz bax bay baz]T

Prediction (Time Update)

δx̂-k = Φk-1 δx̂k-1
P-k = Φk-1 Pk-1 ΦTk-1 + Qk-1

Correction (Measurement Update)

Kk = P-k HTk (Hk P-k HTk + Rk)-1
δx̂k = δx̂-k + Kk (zk - Hk δx̂-k)
Pk = (I - Kk Hk) P-k