Kalman Filters Explained: The Gold Standard for Sensor Fusion

A Kalman filter estimates the hidden state of a dynamic system by recursively blending a physics-based prediction with noisy sensor measurements. It weights each source by its uncertainty, so a drifting IMU and a jittery GPS or wheel encoder combine into an estimate better than either alone. This guide covers the math, a working NumPy implementation, ROS2 integration with robot_localization, tuning of the Q and R matrices, and the trade-offs between KF, EKF, and UKF.

Quick Takeaways

• The filter runs two steps per cycle: predict (propagate state with a motion model) and update (correct with a measurement).
• The Kalman gain K sets how much the filter trusts the measurement versus the prediction.
• Tuning comes down to two matrices: process noise Q and measurement noise R.
• Use a linear KF for linear systems, an EKF for mildly nonlinear robots, and a UKF for strongly nonlinear dynamics.

Why Kalman Filters Dominate Sensor Fusion

Every sensor lies in its own way. A MEMS IMU sampled at 200Hz gives smooth short-term data but integrates into unbounded drift. A GNSS receiver at 5Hz has bounded error but jumps by meters. Wheel odometry accumulates slip error. A Kalman filter exploits these complementary error profiles.

It is the optimal linear estimator under three assumptions:

1. The system model is linear.
2. Process and measurement noise are zero-mean, white, and Gaussian.
3. Noise covariances are known.

When these hold, no other estimator achieves lower mean squared error. In practice they hold approximately, which is why the filter works so well on real hardware.

Mathematical Foundations

State-Space Model

The filter assumes the system evolves as:

x_k = F x_{k-1} + B u_k + w_k      w_k ~ N(0, Q)   (process model)
z_k = H x_k + v_k                  v_k ~ N(0, R)   (measurement model)

The Predict Step

x_pred = F x + B u
P_pred = F P F^T + Q

Uncertainty grows here. Every prediction without a measurement inflates P by Q.

The Update Step

y = z - H x_pred                    (innovation / residual)
S = H P_pred H^T + R                (innovation covariance)
K = P_pred H^T S^-1                 (Kalman gain)
x = x_pred + K y                    (state correction)
P = (I - K H) P_pred                (covariance correction)

For numerical robustness, use the Joseph form for the covariance update:

P = (I - K H) P_pred (I - K H)^T + K R K^T

This keeps P symmetric and positive semi-definite despite floating-point error.

Intuition for the Gain

In one dimension, the gain reduces to:

K = P_pred / (P_pred + R)

• R → 0 (perfect sensor): K → 1, the filter trusts the measurement.
• P_pred → 0 (perfect model): K → 0, the filter ignores the measurement.

Implementation: 1D Constant-Velocity Tracker

This tracks position and velocity from noisy position-only measurements, such as a wheel encoder or ultrasonic range sensor.

import numpy as np

class KalmanFilter:
    def __init__(self, dt, q_var, r_var):
        # State: [position, velocity]^T
        self.x = np.zeros((2, 1))

        # Transition matrix: pos += vel * dt, vel unchanged
        self.F = np.array([[1.0, dt],
                           [0.0, 1.0]])

        # Observation matrix: we only measure position
        self.H = np.array([[1.0, 0.0]])

        # Process noise (discrete white-noise acceleration model)
        self.Q = q_var * np.array([[dt**4 / 4, dt**3 / 2],
                                   [dt**3 / 2, dt**2]])

        # Measurement noise: sensor variance (m^2)
        self.R = np.array([[r_var]])

        # Initial covariance: high uncertainty
        self.P = np.eye(2) * 500.0

    def predict(self):
        self.x = self.F @ self.x                    # propagate state
        self.P = self.F @ self.P @ self.F.T + self.Q  # inflate uncertainty
        return self.x

    def update(self, z):
        y = np.array([[z]]) - self.H @ self.x       # innovation
        S = self.H @ self.P @ self.H.T + self.R     # innovation covariance
        K = self.P @ self.H.T @ np.linalg.inv(S)    # Kalman gain (2x1)
        self.x = self.x + K @ y                     # corrected state

        # Joseph form covariance update for numerical stability
        I_KH = np.eye(2) - K @ self.H
        self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T
        return self.x, y, S                         # y, S feed consistency checks

# 50Hz loop, 0.5 m/s true velocity, sensor sigma = 0.1 m
kf = KalmanFilter(dt=0.02, q_var=0.05, r_var=0.01)
rng = np.random.default_rng(42)

for k in range(250):
    true_pos = 0.5 * k * 0.02
    z = true_pos + rng.normal(0, 0.1)               # noisy measurement
    kf.predict()
    state, _, _ = kf.update(z)

print(f"Estimated velocity: {state[1, 0]:.3f} m/s")  # converges near 0.5

The filter recovers velocity, which no sensor measured directly. This is the observability property at work: unmeasured states can be inferred through the model.

Extended Kalman Filter for Mobile Robots

Real robots are nonlinear. A differential-drive robot with state [x, y, θ] follows:

x_k     = x_{k-1} + v·cos(θ_{k-1})·dt
y_k     = y_{k-1} + v·sin(θ_{k-1})·dt
θ_k     = θ_{k-1} + ω·dt

The EKF linearizes by replacing F with the Jacobian of the motion function f:

        | 1   0   -v·sin(θ)·dt |
F_k  =  | 0   1    v·cos(θ)·dt |
        | 0   0    1           |

The predict and update equations keep the same structure. Only F and H are recomputed each cycle at the current estimate.

def ekf_predict(x, P, v, w, dt, Q):
    th = x[2, 0]
    # Nonlinear motion model f(x, u)
    x_pred = x + np.array([[v * np.cos(th) * dt],
                           [v * np.sin(th) * dt],
                           [w * dt]])
    x_pred[2, 0] = (x_pred[2, 0] + np.pi) % (2 * np.pi) - np.pi  # wrap angle

    # Jacobian of f with respect to the state
    F = np.array([[1, 0, -v * np.sin(th) * dt],
                  [0, 1,  v * np.cos(th) * dt],
                  [0, 0,  1]])
    P_pred = F @ P @ F.T + Q
    return x_pred, P_pred

Angle wrapping is the most common EKF bug. Always normalize θ to [-π, π] after prediction and after computing heading innovations. Skipping it produces a 2π jump in the residual and the estimate diverges.

Sensor Interfacing and Fusion Architecture

Fusion uses asynchronous updates. Run predict at the fastest sensor rate (the IMU), and apply an update whenever any slower sensor delivers a message. Timestamps matter: if a GNSS fix arrives 80ms late, the filter must apply it against the state at the measurement time, not the current state. Use a state history buffer and re-propagate, or compensate with latency-aware delay handling.

ROS2 Integration with robot_localization

The robot_localization package provides production-grade EKF and UKF nodes. This configuration fuses wheel odometry and an IMU:

# ekf.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 50.0                # filter output rate in Hz
    two_d_mode: true               # ignore z, roll, pitch for planar robots
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom              # odom-frame EKF; use map for the global one

    # Wheel odometry: fuse only vx, vy, vyaw
    odom0: /wheel/odometry
    # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]

    # IMU: fuse yaw and yaw rate, and forward acceleration
    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  true,  false, false]
    imu0_remove_gravitational_acceleration: true

    # Process noise covariance diagonal (15x15 in the full config)
    process_noise_covariance: [0.05, 0.05, 0.06, 0.03, 0.03, 0.06, 0.025,
                               0.025, 0.04, 0.01, 0.01, 0.02, 0.01, 0.01, 0.015]

Launch it:

# ekf.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=['config/ekf.yaml'],   # path relative to package share
            remappings=[('odometry/filtered', '/odom')],
        )
    ])

Verify the output with ros2 topic echo /odom --field pose.covariance. Covariance values that grow without bound indicate a missing update source or a Q that is too large.

Tuning Q and R

Most bad Kalman filters are bad tunings. Use this workflow:

1. Set R from sensor data. Hold the robot still, log 60 seconds of measurements, and compute the sample variance. Never guess R when a datasheet or a static test can supply it.
2. Start Q small and diagonal. Increase it until the filter tracks real maneuvers without lag.
3. Check the innovation sequence. For a well-tuned filter, innovations y are zero-mean and white, and they stay within ±2√S about 95% of the time.
4. Run the NIS test. The Normalized Innovation Squared, NIS = y^T S^-1 y, should follow a chi-squared distribution with degrees of freedom equal to the measurement dimension. Consistently high NIS means the filter is overconfident (raise Q or R). Consistently low NIS means it is underconfident.

Real-World Workflow: Fusing IMU and Wheel Odometry on a Differential-Drive Robot

Scenario: A 24V warehouse AMR drifts 0.8m over a 20m straight run using wheel odometry alone, because of slip on a polished floor.

1. Characterize sensors. Log the IMU (gyro noise density 0.005 °/s/√Hz) and the encoders on a straight 10m run. Compute yaw-rate variance from the gyro and velocity variance from the encoders.
2. Calibrate the IMU. Remove gyro bias by averaging 30 seconds of stationary data, and subtract it from the readings.
3. Configure the EKF. Fuse encoder vx and vyaw, plus gyro yaw rate, with two_d_mode: true at 50Hz.
4. Inflate odometry covariance during wheel slip. Raise the encoder R when commanded acceleration exceeds the traction limit.
5. Validate. Drive a 20m square loop and measure the closure error. Heading drift drops sharply because the gyro dominates short-term yaw while the encoders bound velocity.

Expect closure error to fall from the encoder-only baseline to a fraction of it. Adding LiDAR scan matching or GNSS as a global correction then bounds the remaining position drift.

Choosing Between KF, EKF, and UKF

Use the EKF by default for ground robots. It costs little and robot_localization supports it directly. Switch to the UKF when the Jacobian derivation is error-prone or the nonlinearity is strong, such as aggressive quadrotor attitude. The UKF propagates 2n+1 sigma points through the true nonlinear function and needs no Jacobians, at roughly 2-3x the compute of the EKF. For multimodal problems such as the kidnapped-robot case, a Gaussian belief is the wrong model, and a particle filter (AMCL) is the right tool.

Common Failure Modes

• Unobservable states. If no sensor constrains a state, its covariance grows forever. A robot with only an IMU cannot hold absolute position.
• Correlated noise. The filter assumes white noise. Vibration-induced IMU noise is colored, so low-pass filter or model it.
• Outliers. One bad GNSS fix can corrupt the state. Gate measurements with a Mahalanobis test: reject if y^T S^-1 y exceeds the chi-squared threshold (for example, 9.21 for 2 DOF at 99%).
• Timestamp misalignment. A 50ms offset between IMU and odometry yields phantom heading errors during turns.

FAQ

What is the difference between a Kalman filter and a complementary filter?

A complementary filter blends signals with a fixed frequency-domain crossover, such as a gyro high-pass plus an accelerometer low-pass. A Kalman filter adapts its blend over time using the covariance P, so it weights sensors by their current uncertainty. The Kalman filter is more principled and handles multiple sensors, while the complementary filter needs less compute and tuning.

When should I use an EKF instead of a standard Kalman filter?

Use an EKF when the state transition or measurement function is nonlinear, which includes any robot whose model contains sin/cos of heading. A standard KF only applies when both models are linear, such as constant-velocity tracking in Cartesian coordinates.

How do I tune the process noise covariance Q?

Start with small diagonal values based on expected unmodeled acceleration, then raise them until the estimate follows real motion without lag. Validate with the NIS test: if innovations are consistently larger than predicted by S, Q is too small.

Why does my Kalman filter diverge?

The usual causes are a Q that is too small (the filter becomes overconfident and rejects measurements), unwrapped angle residuals, an incorrect R, or a loss of positive definiteness in P. Use the Joseph form update and check that innovations stay inside their predicted bounds.