Robot Localization & Sensor Fusion: Encoders, IMU, GPS, LiDAR and Kalman Filters

Core Engineering Takeaways

•Every individual robotic sensor has fatal blind spots: wheel encoders slip on slick floors, IMU gyro integration drifts exponentially over time, GPS fails indoors, and LiDAR degenerates in open fields.
•Sensor fusion mathematically combines high-rate, smooth relative sensors (encoders, IMU) with low-rate, absolute drift-free sensors (GPS, LiDAR SLAM) to yield an optimal state estimate.
•The Kalman Filter operates in two stages: Predict (propagating state forward via a physics motion model) and Update (correcting the prediction with new noisy sensor measurements).
•The Kalman Gain (K) acts as an optimal weighting factor: if sensor noise covariance R is large, the filter trusts its prediction; if process noise covariance Q is large, the filter aggressively trusts the sensor measurement.
•The Extended Kalman Filter (EKF) linearizes non-linear robot kinematics using first-order Taylor series Jacobian matrices (F_j and H_j).
•The Unscented Kalman Filter (UKF) propagates deterministic "sigma points" directly through non-linear functions, capturing mean and covariance to 3rd-order accuracy without deriving Jacobians.
•Production ROS 2 robots run two independent EKF instances: a continuous, smooth local filter (odom -> base_link) for high-rate motor controllers, and a global filter (map -> odom) that handles discontinuous GPS/SLAM updates.

Prerequisites

Matrix algebra (matrix multiplication, transpose, inversion)
Probability fundamentals (mean, variance, Gaussian normal distributions, covariance)

1. Why No Single Sensor is Enough: The Failure Modes of Robotics Sensors

If you ask an autonomous robot "Where are you right now?", no single sensor onboard can tell the absolute truth. Every sensing modality suffers from physical and environmental limitations:

1. Wheel Encoders (Odometry)

Strengths: High frequency (100 Hz+), zero latency, smooth, low cost.
Fatal Flaw: Accumulates error with distance traveled. If a wheel loses traction on tile or wet grass for half a second, the encoder counts rotations while the chassis remains stationary. Over a 100-meter run, wheel slip and tire compression introduce meters of drift.

2. Inertial Measurement Units (IMU)

Strengths: Captures 6-axis accelerations and angular rates directly from internal MEMS proof-masses, immune to wheel slippage.
Fatal Flaw: Measuring position requires double integration of linear acceleration:

p(t) = \iint a(t) \, dt^2

Even a tiny accelerometer zero-bias offset of $0.05\text{ m/s}^2$ compounds quadratically into a catastrophic 90-meter position error after just 60 seconds! Gyroscope integration bias similarly causes heading drift ( $1^\circ\text{ to }5^\circ\text{ per minute}$ ).

3. GNSS / GPS

Strengths: Absolute global coordinates anywhere on Earth with zero drift over hours or days.
Fatal Flaw: Low update rate (1-10 Hz), high latency, multi-path reflections in urban canyons, and complete blackout inside warehouses, tunnels, and forests.

4. LiDAR Scan Matching

Strengths: Precise millimeter relative localization in structured rooms.
Fatal Flaw: Degenerates in featureless straight corridors (no longitudinal features) or open fields without obstacles.

2. The Mathematics of Uncertainty: State Vectors & Covariance Matrices

To combine these conflicting sensors, we represent the robot's physical condition as a State Vector $\mathbf{x}$ and our confidence in that state as an Error Covariance Matrix $\mathbf{P}$ .

The 2D Robot State Vector

\mathbf{x} = \begin{bmatrix} x \ y \ \theta \ \dot{x} \ \dot{y} \ \dot{\theta} \end{bmatrix} \quad \begin{aligned} &\text{X position} \ &\text{Y position} \ &\text{Heading angle} \ &\text{Linear velocity X} \ &\text{Linear velocity Y} \ &\text{Angular yaw rate} \end{aligned}

The Covariance Matrix $\mathbf{P}$

The covariance matrix $\mathbf{P}$ is a symmetric positive-definite matrix where:

Diagonal elements represent the variance (uncertainty) of each state variable (e.g., $\sigma_x^2, \sigma_y^2, \sigma_\theta^2$ ).
Off-diagonal elements represent the correlation between variables (e.g., $\sigma_{xy}$ indicates how uncertainty in $X$ affects uncertainty in $Y$ ).

When the robot is first powered on at an unknown position, diagonal elements of $\mathbf{P}$ are set to large numbers ( $10^3$ ). As accurate sensors provide measurements, the elements shrink, representing rising confidence.

3. The Linear Kalman Filter (KF): The Predict-Update Cycle

Invented by Rudolf E. Kálmán in 1960 for the Apollo space program, the Kalman Filter is the mathematically optimal recursive algorithm for estimating the state of a linear dynamical system disturbed by white Gaussian noise.

The filter alternates perpetually between two steps:

Stage 1: The Predict Step (Time Propagation)

Using our physical kinematic motion model and control input $\mathbf{u}_k$ (e.g., commanded motor speeds), propagate the state and uncertainty forward by $\Delta t$ :

\hat{\mathbf{x}}_k^- = \mathbf{F}_k \hat{\mathbf{x}}_{k-1} + \mathbf{B}_k \mathbf{u}_k

\mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^T + \mathbf{Q}_k

$\mathbf{F}_k$ : State transition matrix (physics model).
$\mathbf{Q}_k$ : Process noise covariance (accounts for unmodeled physical effects like wind, motor jitter, or carpet friction). Notice that prediction *increases* uncertainty ( $\mathbf{P}$ grows larger).

Stage 2: The Update Step (Measurement Correction)

When a sensor reading $\mathbf{z}_k$ arrives (e.g., GPS or LiDAR):

1
Measurement Innovation $\mathbf{y}_k$ : Compute the discrepancy between physical sensor reading $\mathbf{z}_k$ and predicted measurement $\mathbf{H}_k \hat{\mathbf{x}}_k^-$ :

\mathbf{y}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-

2
Kalman Gain $\mathbf{K}_k$ : Calculate the optimal weighting factor:

\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1}

3
Correct State and Covariance:

\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \mathbf{y}_k

\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^-

$\mathbf{R}_k$ : Measurement noise covariance (the inherent sensor noise from the datasheet).

The Kalman Gain (The Dynamic Trust Arbiter)

\mathbf{K} = \mathbf{P}^- \mathbf{H}^T (\mathbf{H} \mathbf{P}^- \mathbf{H}^T + \mathbf{R})^{-1}

In Simple Words: The Kalman gain weighs whether to trust the physics prediction (small P) or the noisy incoming sensor measurement (small R).

Variables Breakdown:

\mathbf{K}

Kalman Gain matrix (weights innovation)Unit: dimensionless

\mathbf{P}^-

A priori predicted state covariance matrixUnit: state units squared

\mathbf{H}

Measurement transition matrix mapping state to sensor spaceUnit: dimensionless

\mathbf{R}

Sensor measurement noise covariance matrixUnit: measurement units squared

Worked Example:

If sensor noise R → ∞ (broken sensor), K → 0: the filter completely ignores the sensor. If R → 0 (perfect sensor), K → H^-1: the state snaps instantly to the measurement.

4. The Extended Kalman Filter (EKF): Handling Non-Linear Robot Kinematics

Standard Kalman Filters require linear equations. However, mobile robots rotate using non-linear trigonometry:

x_{k+1} = x_k + v_k \Delta t \cos(\theta_k)

y_{k+1} = y_k + v_k \Delta t \sin(\theta_k)

Passing a Gaussian probability distribution through a non-linear trigonometric function deforms the distribution into a complex non-Gaussian shape, breaking linear KF math.

First-Order Taylor Series Linearization

The Extended Kalman Filter (EKF) resolves this by calculating the Jacobian matrix (partial derivatives) of the non-linear motion function $f(\mathbf{x}, \mathbf{u})$ and measurement function $h(\mathbf{x})$ evaluated at the current state estimate:

\mathbf{F}_J = \left. \frac{\partial f}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_k} = \begin{bmatrix} 1 & 0 & -v \Delta t \sin\theta \ 0 & 1 & v \Delta t \cos\theta \ 0 & 0 & 1 \end{bmatrix}

The EKF uses the true non-linear functions to propagate the state vector, but uses the linearized Jacobian matrices $\mathbf{F}_J$ and $\mathbf{H}_J$ to propagate the covariance matrices $\mathbf{P}$ .

5. The Unscented Kalman Filter (UKF): Sigma Points Without Analytical Jacobians

While EKF is widely used, it has two drawbacks:

1
Analytical Jacobians are difficult to derive for complex 15-state 3D systems.
2
If the system is highly non-linear or initial uncertainty is large, first-order Taylor linearization introduces substantial error, causing filter divergence.

The Unscented Transform Intuition

*"It is easier to approximate a probability distribution than it is to approximate an arbitrary non-linear function."* — Julier & Uhlmann.

Instead of linearizing the equations, the Unscented Kalman Filter (UKF) picks a small, deterministic set of sample points called Sigma Points located around the mean along the axes of the covariance ellipse:

For an $n$ -dimensional state vector, it computes $2n + 1$ sigma points.
Each sigma point is propagated through the *actual, exact non-linear function* without any linearization.
The new mean and covariance are calculated from the transformed sigma points using statistical weights.

UKF achieves 3rd-order Taylor series accuracy for Gaussian inputs and eliminates the need to calculate any Jacobians.

6. Dual-Instance Localization Architecture: odom Frame vs map Frame

In production robotics (ROS 2 robot_localization package), running a single EKF for all sensors creates a major problem: discontinuous jumps.

If your robot drives indoors and momentarily regains GPS or triggers a SLAM loop closure, the global position might snap 2 meters to the left. If your high-rate motor velocity PID controller is tracking that frame, the sudden 2-meter jump commands infinite wheel acceleration, stripping gears or tipping the robot over.

The Standard Dual-EKF Architecture

1
Local EKF Instance (odom Frame):

*Inputs:* Wheel encoders + IMU angular velocity & linear acceleration.
*Output:* Publishes the odom $→$ base_link transform.
*Characteristics:* Smooth, monotonic, continuous, zero jumps, but slowly drifts over time. Used by local trajectory controllers.

2
Global EKF Instance (map Frame):

*Inputs:* Wheel encoders + IMU + GPS / SLAM pose.
*Output:* Publishes the map $→$ odom transform.
*Characteristics:* Drift-free in global coordinates. Any sensor correction jump is absorbed into the map $→$ odom offset, leaving the robot's local motion perfectly smooth.

simple_1d_kalman.py

python

import numpy as np

class KalmanFilter1D:
    def __init__(self, process_noise_q: float, sensor_noise_r: float):
        self.q = process_noise_q # Uncertainty in system model
        self.r = sensor_noise_r # Uncertainty in physical sensor
        self.x = 0.0            # State estimate
        self.p = 1.0            # Estimate uncertainty (variance)

    def predict(self, control_velocity: float, dt: float):
        # 1. Propagate state forward via motion model
        self.x += control_velocity * dt
        # 2. Uncertainty grows due to process noise
        self.p += self.q

    def update(self, measurement: float):
        # 3. Compute Kalman Gain: K = P / (P + R)
        k = self.p / (self.p + self.r)
        
        # 4. Correct state estimate with sensor innovation
        self.x += k * (measurement - self.x)
        
        # 5. Uncertainty decreases because we observed reality
        self.p *= (1.0 - k)
        return self.x

7. Engineering Matrix: Complementary Sensor Modalities

Review how different sensor technologies complement each other in an integrated robotics state estimator.

Table 1: State Estimation Sensor Modalities and Fusion Roles

Sensor Modality	Update Rate	Typical Noise / Drift	Best Role in Fusion Pipeline
Wheel Encoders	50 Hz - 200 Hz	High longitudinal slip, low short-term noise	High-rate relative linear velocity and local odometry dead-reckoning
MEMS IMU (Gyro/Accel)	100 Hz - 1000 Hz	High zero-rate drift, zero mechanical slip	High-rate angular yaw rate and tilt estimation; bridges encoder slippage
2D/3D LiDAR Scan Matcher	10 Hz - 20 Hz	Millimeter accuracy, fails in symmetric tunnels	Medium-rate planar correction; bounds orientation and XY position drift
Stereo Visual Odometry (VIO)	20 Hz - 60 Hz	Sensitive to lighting/texture, no mechanical slip	Full 6-DOF spatial velocity estimation for quadrupeds, drones, and arms
RTK GPS / GNSS	1 Hz - 10 Hz	Centimeter accuracy outdoors; zero drift over days	Low-rate global anchor; eliminates drift in long-distance outdoor robots

Frequently Asked Questions

Common Questions & Troubleshooting

Q:How do I tune the Q and R covariance matrices in practice?

Set measurement noise R directly from sensor datasheets or by taking 1,000 stationary sensor readings and calculating the sample variance (sigma^2). Tuning process noise Q requires empirical testing: start with Q small (trusting your physics model) and increase Q if the filter reacts too sluggishly during abrupt accelerations or turns.

Q:Why should I never fuse absolute yaw from a magnetic compass near high-current motors?

Brushless DC motors and high-current power cables generate fluctuating magnetic fields (B = μ0 · I / (2πr)) that completely overwhelm Earth’s faint magnetic field (~50 microteslas). Fusing an unshielded magnetometer reading into an EKF during motor acceleration will cause erratic heading jumps.

Q:What happens if a sensor measurement arrives late or out-of-order in ROS 2?

The ROS 2 robot_localization EKF maintains a historical buffer of states and covariances (typically 1-2 seconds). When a delayed measurement arrives, the filter rolls back its state to the timestamp of the late measurement, applies the update step, and then re-predicts all intervening steps forward to the present time.

Previous ArticleComputer Vision in Robotics: Cameras, OpenCV, Object Detection & Depth Next ArticleRobot Mechanics & Chassis Design: Wheels, Gears, Bearings, Torque & Materials