Groves, Chapter 3

The Kalman Filter

State estimation, system propagation, measurement update, gain computation, EKF, and filter tuning.

Prerequisites: Chapter 2 (coordinate frames), basic probability (mean, variance, covariance).

Chapters

Simulation

Quizzes

Chapter 0: Why the Kalman Filter?

You have a GPS receiver giving noisy position measurements every second. You have an INS giving smooth but drifting position estimates 50 times per second. How do you combine them optimally?

The Kalman filter (1960, Rudolf Kalman) is the answer. It is a recursive algorithm that maintains an estimate of a system's state and its uncertainty, optimally combining predictions from a model with new measurements.

The core idea: The Kalman filter alternates between two phases. Prediction: use a model of how the system evolves to predict the current state. Update: use a new measurement to correct the prediction, weighted by how much you trust each source.

Predict (Propagate)

Project state and uncertainty forward using system model

↓ new measurement arrives

Update (Correct)

Blend prediction with measurement, weighted by uncertainties

↻ repeat

Check: What are the two phases of the Kalman filter?

Training and inference System propagation (predict) and measurement update (correct) Forward pass and backward pass

Chapter 1: The State Vector

The state vector x contains everything the filter estimates. In navigation, typical states include position error, velocity error, attitude error, accelerometer biases, gyro biases, and receiver clock errors.

The error covariance matrix P describes the uncertainty in each state estimate and the correlations between them. Diagonal elements are variances (squared uncertainties); off-diagonal elements capture how errors in one state relate to errors in another.

P = E[δx · δx^T]

where δx = x̂ − x is the difference between the estimated and true state vectors.

Symbol	Meaning
x	True state vector
x̂	State estimate
δx	State error (x̂ − x)
P	Error covariance matrix
Φ	State transition matrix
Q	System noise covariance

Check: What does the error covariance matrix P represent?

The uncertainty in each state estimate and correlations between states The measurement noise The system dynamics

Chapter 2: System Propagation

Between measurements, the filter predicts how the state evolves. The linear system model is:

ẋ(t) = F(t) x(t) + G(t) w_s(t)

where F is the system matrix (how states evolve), G distributes the system noise w_s across states.

For discrete-time processing, the state propagation is:

x̂_k⁻ = Φ_k−1 x̂_k−1⁺

and the covariance propagation:

P_k⁻ = Φ_k−1 P_k−1⁺ Φ_k−1^T + Q_k−1

The transition matrix Φ propagates states one time step. The system noise Q adds uncertainty to account for unmodeled effects (bumps, gusts, sensor drift).

Key insight: Without measurements, uncertainty always grows. The Q term ensures the filter "remembers" that its predictions become less reliable over time. A filter with Q = 0 would become overconfident and stop responding to measurements.

Check: What happens to uncertainty during propagation (no measurements)?

It always grows (Q adds noise, no measurements to correct) It always decreases It stays constant

Chapter 3: Measurement Update

When a measurement z arrives, the filter corrects its prediction. The measurement model:

z_k = H_k x_k + w_m,k

where H is the measurement matrix (how measurements relate to states) and w_m is measurement noise with covariance R.

The measurement innovation is the surprise — the difference between what you measured and what you predicted:

δz_k⁻ = z_k − H_k x̂_k⁻

The state update blends the prediction with the innovation:

x̂_k⁺ = x̂_k⁻ + K_k δz_k⁻

And the covariance shrinks:

P_k⁺ = (I − K_k H_k) P_k⁻

The innovation is everything: If the measurement matches the prediction (δz ≈ 0), nothing changes. If there's a big surprise, the filter adjusts. The Kalman gain K controls how much adjustment to make.

Check: What is the measurement innovation?

The difference between the actual measurement and the predicted measurement The measurement noise covariance The new state estimate

Chapter 4: The Kalman Gain

The Kalman gain K is the heart of the filter. It balances trust between the prediction and the measurement:

K_k = P_k⁻ H_k^T (H_k P_k⁻ H_k^T + R_k)⁻¹

Think of it as a trust slider:

Condition	K approaches	Meaning
P large, R small	H⁻¹ (trust measurement)	Prediction uncertain, measurement accurate
P small, R large	0 (trust prediction)	Prediction confident, measurement noisy

Key insight: The gain is the ratio P/(P+R). When your prediction uncertainty P is large relative to measurement noise R, the gain is high and you trust the measurement. When P is small, the gain is low and you stick with the prediction. This is optimal in the minimum-variance sense.

Kalman Gain Demo

Adjust the prediction uncertainty (P) and measurement noise (R) to see how the Kalman gain changes.

P50

R50

Check: When the Kalman gain K is close to 1, it means...

The filter trusts the measurement more than the prediction The filter ignores the measurement The system noise is zero

Chapter 5: System Model

The system model defines how states evolve over time. In continuous time: ẋ = Fx + Gw_s. The system matrix F captures the physics. For a simple position-velocity system:

F = [0, 1; 0, 0] — velocity drives position change

The transition matrix Φ is the matrix exponential of Fτ_s (where τ_s is the time step). For simple systems, Φ ≈ I + Fτ_s.

The system noise covariance Q models unmodeled forces. For the position-velocity example, if acceleration is white noise with PSD n_a²:

Q = [0, 0; 0, n_a² τ_s]

Check: What does the system matrix F describe?

How the states evolve over time (the physics of the system) The measurement noise statistics The initial state estimate

Chapter 6: Closed-Loop Kalman Filter

In many navigation applications, the system model isn't truly linear — the differential of x depends on higher-order terms. The closed-loop implementation feeds back the estimated errors to correct the system itself, zeroing the filter states.

This keeps the filter states small, minimizing linearization errors. In contrast, the open-loop implementation lets states grow, which can degrade linearity assumptions.

Navigation application: A closed-loop INS/GNSS filter estimates INS position and velocity errors, then feeds them back to correct the INS solution every cycle. The filter always starts each cycle near zero, keeping the linear approximation valid. This is essential for low-grade IMUs where errors can grow rapidly.

Check: Why is closed-loop operation preferred for low-grade INS?

Feeding back corrections keeps states small, maintaining linearity assumptions It requires less processing power It eliminates the need for a measurement model

Chapter 7: Tuning

Tuning means selecting values for three matrices: initial covariance P₀, system noise Q, and measurement noise R. The critical parameter is the ratio P/R.

P/R too small	P/R optimal	P/R too large
Gain too low, slow convergence	Good tracking, stable estimates	Gain too high, noisy/unstable estimates

In practice, the filter model is always an approximation. System noise must overbound reality — "make the round hole big enough for the square peg." Typical 1σ uncertainties are 2–3× the actual error standard deviations to maintain stability.

Tuning philosophy: Fix P₀ and Q from sensor specs. Vary R by trial and error to find the smallest value that gives stable estimates. Tuning is a tradeoff between convergence speed and stability.

Check: What happens if the ratio P/R is set too large?

The Kalman gain becomes too large, making state estimates noisy and potentially unstable The filter converges too slowly The filter ignores all measurements

Chapter 8: The Extended Kalman Filter

The standard Kalman filter requires linear models. Real navigation systems are nonlinear — GNSS ranging equations involve square roots and trig functions. The Extended Kalman Filter (EKF) handles this by linearizing about the current state estimate.

The EKF replaces the linear system model Fx with a nonlinear function f(x), and the linear measurement model Hx with h(x). The Jacobians of these functions are used for covariance propagation and gain computation.

State: ẋ = f(x) + Gw_s
Measurement: z = h(x) + w_m

The EKF assumes the error is small relative to the nonlinearity. Closed-loop operation helps maintain this assumption by keeping the state estimate close to truth.

Other extensions: Schmidt-Kalman filter handles time-correlated noise. Adaptive Kalman filter automatically adjusts noise parameters. Multiple-hypothesis filtering handles ambiguous measurements. Kalman smoothing uses future data (post-processing only) to improve past estimates.

Check: How does the EKF handle nonlinear models?

By linearizing (computing Jacobians) about the current state estimate By using neural networks By ignoring the nonlinear terms

Chapter 9: Summary

Kalman filter essentials:
• Two phases: propagate (predict) and update (correct)
• State vector x, covariance P, gain K
• Gain K = P/(P+R) — the optimal trust slider
• System model: Φ (transition), Q (system noise)
• Measurement model: H (measurement matrix), R (noise covariance)
• Innovation δz = z − Hx̂ drives the correction
• Closed-loop: feed back corrections to keep states small
• Tuning: balance convergence speed vs. stability via P/R ratio
• EKF: linearize nonlinear models about current estimate

Check: In the Kalman gain formula K = PH^T(HPH^T + R)⁻¹, which matrix represents measurement noise?

P H R