Sensor Fusion with Kalman Filters

calendar Dec 12, 2024 · 4 min read · filters kalman sensor-fusion imu gps robotics  ·
Share on: linkedin copy

Comprehensive guide to sensor fusion using Kalman filters for combining data from multiple sensors.

What is Sensor Fusion?

Sensor Fusion: Combining data from multiple sensors to produce more accurate, reliable, and complete information than any single sensor alone.

graph TD
    A[GPS<br/>Low rate, High accuracy] --> D[Kalman Filter]
    B[IMU<br/>High rate, Drift] --> D
    C[Magnetometer<br/>Heading] --> D
    D --> E[Fused Estimate<br/>High rate + High accuracy]
    
    style A fill:#ffcccc
    style B fill:#ccccff
    style C fill:#ccffcc
    style E fill:#ffffcc

Why Fuse Sensors?

Individual Sensor Limitations

SensorAdvantagesDisadvantages
GPSAbsolute position, No driftLow rate (1-10 Hz), Noisy, Dropouts
IMUHigh rate (100+ Hz), SmoothDrift over time, No absolute reference
MagnetometerAbsolute headingMagnetic interference
BarometerAltitudeAffected by weather

Fusion Benefits

  • Complementary strengths: Each sensor compensates for others' weaknesses
  • Increased accuracy: Statistical combination reduces noise
  • Robustness: System works even if one sensor fails
  • Higher update rate: Fast sensors fill gaps from slow sensors

GPS + IMU Fusion

Most common sensor fusion application: combining GPS (slow, accurate) with IMU (fast, drifting).

State Vector

$$ \mathbf{x} = \begin{bmatrix} p_x \\ p_y \\ p_z \\ v_x \\ v_y \\ v_z \\ \phi \\ \theta \\ \psi \end{bmatrix} \quad \begin{aligned} &\text{Position} \\ &\text{Velocity} \\ &\text{Orientation (roll, pitch, yaw)} \end{aligned} $$

Prediction (IMU)

Use accelerometer and gyroscope at high rate (100-1000 Hz):

$$ \begin{aligned} \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2}\mathbf{a}_k \Delta t^2 \\ \mathbf{v}_{k+1} &= \mathbf{v}_k + \mathbf{a}_k \Delta t \\ \boldsymbol{\theta}_{k+1} &= \boldsymbol{\theta}_k + \boldsymbol{\omega}_k \Delta t \end{aligned} $$

Update (GPS)

Correct with GPS at low rate (1-10 Hz):

$$ \mathbf{z}_{GPS} = \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} $$

Interactive GPS+IMU Fusion

Multi-Sensor Fusion Architecture

graph TD
    A[Accelerometer<br/>100 Hz] --> E[IMU Integration]
    B[Gyroscope<br/>100 Hz] --> E
    C[Magnetometer<br/>50 Hz] --> F[Heading Correction]
    D[GPS<br/>10 Hz] --> G[Position Correction]
    
    E --> H[Extended Kalman Filter]
    F --> H
    G --> H
    
    H --> I[Fused State<br/>Position, Velocity<br/>Orientation]
    
    style E fill:#ffcccc
    style H fill:#ccffcc
    style I fill:#ffffcc

Extended Kalman Filter (EKF)

For non-linear systems (like orientation), use EKF:

Non-Linear State Transition

$$ \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k $$

Linearization

$$ F_k = \frac{\partial f}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_k} $$

EKF Prediction

$$ \begin{aligned} \hat{\mathbf{x}}_{k|k-1} &= f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k) \\ P_{k|k-1} &= F_k P_{k-1|k-1} F_k^T + Q_k \end{aligned} $$

EKF Update

Same as standard Kalman filter, but with linearized observation model.

Complementary Filter (Simplified Alternative)

For simple applications, a complementary filter can work well:

$$ \hat{\theta} = \alpha \cdot (\hat{\theta} + \omega \Delta t) + (1 - \alpha) \cdot \theta_{accel} $$
  • $\alpha \approx 0.98$: Trust gyroscope (high frequency)
  • $1 - \alpha \approx 0.02$: Trust accelerometer (low frequency, corrects drift)
 1import numpy as np
 2
 3class ComplementaryFilter:
 4    def __init__(self, alpha=0.98):
 5        self.alpha = alpha
 6        self.angle = 0.0
 7    
 8    def update(self, gyro, accel, dt):
 9        """
10        gyro: angular velocity (rad/s)
11        accel: acceleration vector (for angle estimation)
12        dt: time step
13        """
14        # Integrate gyroscope
15        self.angle = self.angle + gyro * dt
16        
17        # Estimate angle from accelerometer
18        accel_angle = np.arctan2(accel[1], accel[0])
19        
20        # Complementary filter
21        self.angle = self.alpha * self.angle + (1 - self.alpha) * accel_angle
22        
23        return self.angle

Sensor Fusion Best Practices

  1. Understand sensor characteristics

    • Update rates
    • Noise levels
    • Drift characteristics
    • Failure modes

  2. Tune process and measurement noise

    • $Q$: How much you trust the model
    • $R$: How much you trust the sensors
    • Higher $Q$ → trust sensors more
    • Higher $R$ → trust model more

  3. Handle outliers

    • Check innovation (measurement - prediction)
    • Reject measurements with large innovation
    • Use robust estimation (M-estimators)

  4. Synchronization

    • Time-stamp all measurements
    • Interpolate/extrapolate when needed
    • Handle variable update rates

  5. Initialization

    • Start with reasonable initial state
    • Use large initial covariance for uncertainty
    • Allow filter to converge

Common Applications

1. Drone/UAV Navigation

  • GPS + IMU + Barometer + Magnetometer
  • High-rate attitude estimation
  • Robust to GPS dropouts

2. Autonomous Vehicles

  • GPS + IMU + Wheel odometry + LIDAR
  • Lane keeping and navigation
  • Redundancy for safety

3. Smartphones

  • Accelerometer + Gyroscope + Magnetometer
  • Screen rotation
  • Step counting
  • Augmented reality

4. Robotics

  • Wheel encoders + IMU + Camera
  • SLAM (Simultaneous Localization and Mapping)
  • Path following

Further Reading

Related Snippets