Sensor Fusion with Kalman Filters

calendarDec 12, 2024 · 3 min read · filters kalman sensor-fusion imu gps robotics  ·
Share on: linkedincopy

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.

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

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