1271 Kalman Filters for Sensor Fusion

Learning Objectives

After completing this chapter, you will be able to:

Understand Kalman filter theory and state-space models
Implement the predict-update cycle
Apply Kalman filters to sensor fusion problems
Tune filter parameters (Q, R) for optimal performance
Handle multi-dimensional state estimation

Understanding State Estimation

Core Concept: State estimation infers the true value of a system’s hidden state (position, velocity, temperature) from noisy, incomplete sensor measurements by combining a predictive model of how the state evolves with observations that constrain what the state can be.

Why It Matters: Real sensors never report the true state directly. A GPS chip reports a position estimate with 3-5 meter uncertainty; an accelerometer measures gravity plus linear acceleration plus noise. State estimation algorithms like Kalman filters maintain a running best-guess of the true state plus a measure of confidence (covariance), enabling downstream systems to make informed decisions even when individual measurements are unreliable.

Key Takeaway: Think of state estimation as maintaining a belief about reality rather than trusting any single measurement. The belief gets updated by two mechanisms: prediction (what physics says should happen) and correction (what sensors actually observed). Good estimators balance these based on how much you trust each.

1271.1 Introduction to Kalman Filtering

The Kalman filter is the optimal linear estimator for systems with Gaussian noise. It’s widely used for sensor fusion in navigation, robotics, and tracking.

1271.1.1 State Space Model

State equation: \(x_k = F x_{k-1} + B u_k + w_k\) (process model)

Measurement equation: \(z_k = H x_k + v_k\) (sensor model)

Where:

\(x_k\): State vector at time \(k\)
\(F\): State transition matrix
\(B\): Control input matrix
\(u_k\): Control input
\(H\): Measurement matrix
\(w_k\): Process noise ~ \(N(0, Q)\)
\(v_k\): Measurement noise ~ \(N(0, R)\)

1271.2 The Predict-Update Cycle

The Kalman filter operates in two alternating phases:

%%{init: {'theme': 'base', 'themeVariables': { 'primaryColor': '#2C3E50', 'primaryTextColor': '#fff', 'primaryBorderColor': '#16A085', 'lineColor': '#16A085', 'secondaryColor': '#E67E22', 'tertiaryColor': '#ecf0f1'}}}%%
flowchart LR
    subgraph Predict["PREDICT"]
        P1["State: x_pred = F*x"]
        P2["Covariance: P_pred = F*P*F' + Q"]
    end

    subgraph Update["UPDATE"]
        U1["Gain: K = P*H'/(H*P*H' + R)"]
        U2["State: x = x + K*(z - H*x)"]
        U3["Covariance: P = (I - K*H)*P"]
    end

    Init["Initial x, P"] --> Predict
    Predict --> Update
    Update --> Sensor["New Measurement z"]
    Sensor --> Predict

    style Predict fill:#16A085,stroke:#2C3E50,color:#fff
    style Update fill:#E67E22,stroke:#2C3E50,color:#fff

1271.2.1 Step 1: Initialize

Set initial estimate and uncertainty based on prior knowledge:

Initial state \(\hat{x}_0\)
Initial covariance \(P_0\)

1271.2.2 Step 2: Predict

Project state forward using motion model. Uncertainty grows:

\[\hat{x}_{k|k-1} = F \hat{x}_{k-1|k-1} + B u_k\]

\[P_{k|k-1} = F P_{k-1|k-1} F^T + Q\]

1271.2.3 Step 3: Compute Kalman Gain

Balance prediction vs measurement trust. \(K \in [0,1]\):

\[K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}\]

1271.2.4 Step 4: Update

Correct prediction using measurement. Uncertainty shrinks:

\[\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1})\]

\[P_{k|k} = (I - K_k H) P_{k|k-1}\]

Key Insight: Why Uncertainty Shrinks

When you multiply two Gaussian distributions (prediction x measurement), the result is always narrower than either input. This is the mathematical magic of sensor fusion - combining imperfect information sources produces a better estimate than any single source alone.

The math: If prediction has variance P and measurement has variance R, the fused variance is: \[\sigma_{fused}^2 = \frac{1}{\frac{1}{P} + \frac{1}{R}} = \frac{PR}{P+R}\]

For example: P = 100 m^2 (10m uncertainty), R = 25 m^2 (5m uncertainty) \[\sigma_{fused}^2 = \frac{100 \times 25}{100 + 25} = \frac{2500}{125} = 20 \text{ m}^2 \approx (4.5\text{m})^2\]

The fused uncertainty (4.5m) is smaller than both inputs (10m and 5m)!

1271.3 Worked Example: Temperature Sensor Tracking

Scenario: Track room temperature using a noisy sensor (+/-1.0C accuracy)

System Parameters:

State: Temperature at time \(k\) (scalar, 1D case)
State transition: \(A = 1\) (temperature doesn’t change much between samples)
Measurement matrix: \(H = 1\) (we measure temperature directly)
Process noise: \(Q = 0.1\) (variance in temperature change per time step)
Measurement noise: \(R = 1.0\) (sensor variance)
Initial conditions: \(\hat{X}_0 = 20.0\)C, \(P_0 = 1.0\)

Simulated Measurements:

Time \(k\)	True Temp	Measurement \(Z_k\)	Noise
1	21.0C	21.5C	+0.5C
2	21.5C	20.8C	-0.7C
3	22.0C	22.3C	+0.3C
4	22.3C	21.9C	-0.4C
5	22.5C	22.6C	+0.1C

1271.3.1 Step-by-Step Calculations

k=1: First measurement Z1 = 21.5C

Predict: \[\hat{X}_{1|0} = 1 \times 20.0 = 20.0°C\] \[P_{1|0} = 1 \times 1.0 \times 1 + 0.1 = 1.1\]

Update: \[y_1 = 21.5 - 20.0 = 1.5°C\] \[K_1 = \frac{1.1}{1.1 + 1.0} = 0.524\] \[\hat{X}_{1|1} = 20.0 + 0.524 \times 1.5 = 20.786°C\] \[P_{1|1} = (1 - 0.524) \times 1.1 = 0.524\]

k=2 through k=5: Continue the predict-update cycle…

Summary Table:

k	Measurement	Kalman Gain	Updated Estimate	Uncertainty	Error
0	-	-	20.000	1.000	0.000
1	21.5	0.524	20.786	0.524	-0.214
2	20.8	0.384	20.791	0.384	-0.709
3	22.3	0.326	21.283	0.326	-0.717
4	21.9	0.299	21.467	0.299	-0.833
5	22.6	0.285	21.790	0.285	-0.710

Key Observations:

Uncertainty Decreases: \(P\) reduces from 1.000 to 0.285 (71.5% reduction)
Kalman Gain Stabilizes: \(K\) converges from 0.524 to 0.285
Filter smooths noise while tracking trend

1271.4 Worked Example: GPS + Accelerometer Fusion

This example demonstrates how a Kalman filter combines two complementary sensors for drone position tracking.

Real-World Application: Delivery Drone Navigation

Delivery drones face a fundamental challenge: GPS provides absolute position (no drift) but updates slowly (1 Hz) with moderate noise (several meters). The accelerometer provides high-frequency motion updates (100 Hz) but suffers from integration drift. The Kalman filter optimally fuses these complementary sensors.

%%{init: {'theme': 'base', 'themeVariables': { 'primaryColor': '#2C3E50', 'primaryTextColor': '#fff', 'primaryBorderColor': '#16A085', 'lineColor': '#E67E22', 'secondaryColor': '#7F8C8D', 'tertiaryColor': '#fff'}}}%%
flowchart TB
    subgraph Input["SENSOR INPUTS"]
        GPS["GPS Receiver<br/>Absolute position<br/>R = 16 m2<br/>Updates: 1 Hz"]
        Accel["Accelerometer<br/>Velocity integration<br/>Updates: 100 Hz"]
    end

    subgraph Predict["PREDICTION"]
        P1["Position: x = x + v*dt"]
        P2["Uncertainty: P = P + Q"]
    end

    subgraph Update["UPDATE"]
        K["Kalman Gain K"]
        Est["Fused Estimate"]
    end

    subgraph Output["OUTPUT"]
        Result["Optimal Position<br/>Lower uncertainty<br/>than either sensor!"]
    end

    GPS --> Update
    Accel --> Predict
    Predict --> Update
    Update --> Output

    style GPS fill:#16A085,stroke:#2C3E50,color:#fff
    style Accel fill:#E67E22,stroke:#2C3E50,color:#fff
    style Result fill:#27AE60,stroke:#2C3E50,color:#fff

1271.4.1 Problem Setup

Given Parameters:

Parameter	Value	Physical Meaning
Initial position	100 m	Starting position
Initial uncertainty	25 m2	Moderately uncertain
Velocity	2 m/s	Constant velocity
Time step	1 s	Time between GPS updates
GPS measurement	105 m	What GPS reports
GPS noise variance R	16 m2	Typical consumer GPS
Process noise Q	1 m2	Wind, vibration

1271.4.2 Step-by-Step Solution

Step 1: Prediction

\[\hat{x}^- = 100 + 2 \times 1 = 102\,m\] \[P^- = 25 + 1 = 26\,m^2\]

Step 2: Kalman Gain

\[K = \frac{26}{26 + 16} = 0.619\]

The Kalman gain tells us:

Trust in GPS: K = 0.619 (62%)
Trust in prediction: 1-K = 0.381 (38%)

Step 3: Update

\[y = 105 - 102 = 3\,m\] \[\hat{x} = 102 + 0.619 \times 3 = 103.86\,m\] \[P = 0.381 \times 26 = 9.9\,m^2\]

1271.4.3 Results Summary

Stage	Position	Uncertainty (P)	Std Dev
Initial	100 m	25 m2	5.0 m
After Prediction	102 m	26 m2	5.1 m
After GPS Update	103.86 m	9.9 m2	3.1 m

The Power of Sensor Fusion

Uncertainty DECREASED from 5.0m to 3.1m by fusing two noisy sensors!

Even more remarkably:

Prediction alone: 5.1m
GPS alone: 4.0m
Fused result: 3.1m (better than either sensor!)

1271.5 Parameter Tuning: Q and R

Tuning Guidelines

Process Noise Q:

Higher Q = less trust in motion model (tracks measurements more)
Lower Q = more trust in model (smoother but slower to respond)
Set based on physical system dynamics

Measurement Noise R:

From sensor datasheets or empirical testing
Measure variance of stationary readings
Higher R = noisier sensor, less trusted

Balance:

If Q large and R small: K approaches 1 (trust measurements)
If Q small and R large: K approaches 0 (trust prediction)

Quiz: Kalman Filter Parameters

Question: A Kalman filter with very small process noise Q will:

Explanation: Small Q means we trust our process model strongly. The filter becomes overconfident in predictions and responds slowly to actual changes in the system. This produces smooth estimates but can miss rapid dynamics. K will be small (trust prediction over measurements).

1271.6 Understanding Uncertainty Propagation

Understanding Uncertainty Propagation

Core Concept: Uncertainty propagation tracks how measurement errors and model imperfections accumulate, spread, and combine as data flows through a fusion system, represented mathematically by the covariance matrix P that grows during prediction and shrinks during measurement updates.

Why It Matters: Knowing the estimate is not enough; you must know how confident you are. A position estimate of “10 meters east” is useless without knowing if the uncertainty is 0.1 meters (safe for drone landing) or 50 meters (dangerous for navigation). The covariance matrix P captures not just individual variable uncertainties but also correlations between them.

Key Takeaway: Always propagate uncertainty alongside state estimates. Use covariance to weight sensor contributions (Kalman gain), detect anomalies (innovation monitoring), and communicate confidence to downstream systems.

1271.7 Gaussian Intuition: Spreading and Sharpening

%%{init: {'theme': 'base', 'themeVariables': { 'primaryColor': '#2C3E50', 'primaryTextColor': '#fff', 'primaryBorderColor': '#16A085', 'lineColor': '#E67E22', 'secondaryColor': '#7F8C8D', 'tertiaryColor': '#fff'}}}%%
graph LR
    A["Previous Estimate<br/>Narrow Gaussian"]
    B["PREDICT<br/>Gaussian SPREADS<br/>Wider"]
    C["Measurement<br/>Sensor Gaussian"]
    D["UPDATE<br/>Gaussians MULTIPLY<br/>Sharpened"]

    A -->|"+ Process Noise Q"| B
    B -->|"New Measurement"| C
    C -->|"Kalman Gain"| D
    D -.->|"Next Cycle"| A

    style A fill:#2196F3,stroke:#1565C0,color:#fff
    style B fill:#9C27B0,stroke:#6A1B9A,color:#fff
    style C fill:#4CAF50,stroke:#2E7D32,color:#fff
    style D fill:#F44336,stroke:#C62828,color:#fff

Key Insights:

Predict Step: Uncertainty GROWS (Gaussian spreads)
- Apply motion model but add process noise Q
- The further we predict, the less certain we are
Update Step: Uncertainty DECREASES (Gaussians multiply)
- Fuse prediction with measurement
- New information reduces uncertainty

1271.8 When Kalman Filters Fail

Situation	Problem	Alternative
Non-linear system	Linearization errors	Extended Kalman (EKF)
Multi-modal distribution	Single Gaussian assumption	Particle filter
Unknown noise parameters	Wrong Q or R	Adaptive Kalman

1271.9 Extended Kalman Filter (EKF)

For nonlinear systems, the Extended Kalman Filter linearizes around the current estimate using Jacobians:

Nonlinear state model: \(x_k = f(x_{k-1}, u_k) + w_k\)

Linearization: Compute Jacobian \(F = \frac{\partial f}{\partial x}\) at each time step

The EKF is commonly used for GPS-IMU fusion where the measurement model (range to satellites) is nonlinear.

Worked Example: Kalman Filter for Indoor Position Tracking

Scenario: Track forklift position in warehouse using wheel odometry + UWB beacons.

Given: - State vector: [x, y, vx, vy] (4 states) - Wheel odometry: 50 Hz, 2% drift - UWB beacons: 10 Hz, +/-15 cm accuracy - Target: <30 cm position error

Result: EKF achieves 18 cm RMS error (within 30 cm target). Pure odometry after 5 minutes: 3.2 m error. Pure UWB: 22 cm. Fusion provides 6x improvement over odometry.

Key Insight: The Kalman gain automatically weights each source based on current uncertainty - trusting odometry between UWB updates, then snapping back when UWB measurements arrive.

1271.10 Summary

The Kalman filter is the foundation of sensor fusion:

Predict-Update Cycle: Project forward with model, correct with measurements
Optimal Weighting: Kalman gain balances prediction vs measurement based on uncertainties
Uncertainty Tracking: Covariance matrix captures confidence in estimates
Fusion Benefit: Combined estimate better than any single sensor

1271.11 What’s Next

Complementary Filters & IMU - Simpler alternative for IMU fusion
Particle Filters - Non-linear, non-Gaussian systems
Fusion Architectures - System design patterns
Exercises - Practice implementing Kalman filters