The Problem: Why Is One Sensor Not Enough?
Every robot sensor has its own limitations:
- Wheel encoder: Accurate short-term, but drifts over time due to wheel slip and uneven terrain
- IMU (Inertial Measurement Unit): Precise acceleration and angular velocity, but double-integration drift causes errors (cm → m in just a few minutes)
- GPS: Provides absolute position, but very noisy (2-5m error outdoor), doesn't work indoors, slow update rate (1-10 Hz)
- LiDAR SLAM: Accurate, but computationally expensive and easily fails in featureless environments
Kalman Filter solves this by combining all sensors, taking the strengths of each and compensating for weaknesses. Result: more accurate position estimates than any single sensor can provide.
Kalman Filter — Intuition
Imagine you're estimating the position of a robot moving down a corridor:
- Predict: "Based on current velocity, I think the robot will be at position X after 0.1 seconds" (using the motion model)
- Update: "But the GPS sensor says the robot is at position Y" (using the measurement)
- Fuse: "The model says X, the GPS says Y. The model is better short-term, so I'll trust it 70%, GPS 30% → estimated position Z"
The Kalman Filter automatically calculates this trust ratio (Kalman Gain) based on the uncertainty (covariance) of each information source. The sensor with less noise gets more weight.
Kalman Filter Mathematics
State Vector
For a 2D robot, the state vector typically includes:
x = [x, y, theta, v, omega]^T
- (x, y): position
- theta: heading angle
- v: linear velocity
- omega: angular velocity
Predict Step
x_predicted = F * x_prev + B * u
P_predicted = F * P_prev * F^T + Q
- F: State transition matrix (motion model)
- B: Control input matrix
- u: Control input (e.g., motor commands)
- P: State covariance matrix (uncertainty)
- Q: Process noise covariance (model imperfection)
Update Step
y = z - H * x_predicted # Innovation (measurement residual)
S = H * P_predicted * H^T + R # Innovation covariance
K = P_predicted * H^T * S^(-1) # Kalman Gain
x_updated = x_predicted + K * y # Updated state
P_updated = (I - K * H) * P_predicted # Updated covariance
- z: Measurement vector
- H: Observation matrix (maps state → measurement space)
- R: Measurement noise covariance (sensor noise)
- K: Kalman Gain — the "trust ratio" between model and sensors
Understanding Kalman Gain
- K near 0: Trust the model more than sensors (sensor very noisy, or model very good)
- K near 1: Trust sensors more than model (high model uncertainty, sensor is accurate)
Python Implementation — Linear Kalman Filter
import numpy as np
import matplotlib.pyplot as plt
class KalmanFilter:
"""Linear Kalman Filter for robot localization."""
def __init__(self, dim_state: int, dim_measurement: int, dim_control: int = 0):
self.n = dim_state
self.m = dim_measurement
# State estimate and covariance
self.x = np.zeros((self.n, 1)) # State vector
self.P = np.eye(self.n) * 1000.0 # High initial uncertainty
# System matrices
self.F = np.eye(self.n) # State transition
self.H = np.zeros((self.m, self.n)) # Observation
self.Q = np.eye(self.n) * 0.01 # Process noise
self.R = np.eye(self.m) * 1.0 # Measurement noise
self.B = np.zeros((self.n, dim_control)) if dim_control > 0 else None
def predict(self, u: np.ndarray = None):
"""Predict step."""
if u is not None and self.B is not None:
self.x = self.F @ self.x + self.B @ u
else:
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
return self.x.copy()
def update(self, z: np.ndarray):
"""Update step with measurement."""
z = z.reshape(-1, 1)
y = 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
self.x = self.x + K @ y
I = np.eye(self.n)
self.P = (I - K @ self.H) @ self.P
return self.x.copy()
def get_state(self) -> np.ndarray:
return self.x.flatten()
def get_covariance(self) -> np.ndarray:
return self.P.copy()
Example: 1D Robot with Encoder + GPS
# Simulate robot moving at constant velocity
np.random.seed(42)
dt = 0.1 # 10 Hz
duration = 30.0 # 30 seconds
velocity = 1.0 # 1 m/s constant velocity
# Ground truth
times = np.arange(0, duration, dt)
true_positions = velocity * times
# Noisy measurements
encoder_noise_std = 0.1 # Encoder: low noise, but drifts
gps_noise_std = 3.0 # GPS: high noise, no drift
gps_rate = 1.0 # GPS at 1 Hz
encoder_readings = true_positions + np.cumsum(
np.random.normal(0, encoder_noise_std * dt, len(times))
)
gps_readings = true_positions + np.random.normal(0, gps_noise_std, len(times))
# Kalman Filter setup
kf = KalmanFilter(dim_state=2, dim_measurement=1)
# State: [position, velocity]
kf.F = np.array([[1, dt],
[0, 1]]) # Constant velocity model
kf.H = np.array([[1, 0]]) # We measure position only
kf.Q = np.array([[0.01, 0],
[0, 0.01]]) # Process noise
kf.R = np.array([[gps_noise_std**2]]) # GPS measurement noise
kf.x = np.array([[0], [velocity]]) # Initial state
# Run filter
kf_positions = []
kf_velocities = []
uncertainties = []
for i, t in enumerate(times):
# Predict (every step)
kf.predict()
# Update with GPS (every 1 second = every 10 steps)
if i % int(gps_rate / dt) == 0:
kf.update(np.array([gps_readings[i]]))
state = kf.get_state()
kf_positions.append(state[0])
kf_velocities.append(state[1])
uncertainties.append(np.sqrt(kf.P[0, 0]))
# Plot results
fig, axes = plt.subplots(3, 1, figsize=(12, 10))
axes[0].plot(times, true_positions, 'g-', label='Ground Truth', linewidth=2)
axes[0].plot(times, encoder_readings, 'b.', label='Encoder (drift)', alpha=0.3, markersize=2)
axes[0].scatter(times[::10], gps_readings[::10], c='r', s=20, label='GPS (noisy)', alpha=0.5)
axes[0].plot(times, kf_positions, 'k-', label='Kalman Filter', linewidth=1.5)
axes[0].set_ylabel('Position (m)')
axes[0].set_title('Sensor Fusion: Encoder + GPS')
axes[0].legend()
axes[1].plot(times, np.array(kf_positions) - true_positions, 'k-', label='KF Error')
axes[1].fill_between(times,
-2 * np.array(uncertainties),
2 * np.array(uncertainties),
alpha=0.2, color='blue', label='2-sigma uncertainty')
axes[1].set_ylabel('Error (m)')
axes[1].set_title('Estimation Error with Uncertainty Bounds')
axes[1].legend()
axes[2].plot(times, kf_velocities, 'k-', label='Estimated Velocity')
axes[2].axhline(y=velocity, color='g', linestyle='--', label='True Velocity')
axes[2].set_ylabel('Velocity (m/s)')
axes[2].set_xlabel('Time (s)')
axes[2].set_title('Velocity Estimation')
axes[2].legend()
plt.tight_layout()
plt.savefig('kalman_filter_demo.png', dpi=150)
plt.show()
Extended Kalman Filter (EKF) for Non-Linear Systems
Linear Kalman Filter assumes the system is linear. But real robots are nonlinear — turning the heading angle theta changes x, y through sin/cos.
EKF handles this by linearizing (computing linear approximation) at the operating point, replacing F with a Jacobian matrix.
EKF for Differential-Drive Robot
class ExtendedKalmanFilter:
"""EKF for differential-drive robot localization."""
def __init__(self):
# State: [x, y, theta]
self.x = np.zeros((3, 1))
self.P = np.eye(3) * 0.1
# Noise
self.Q = np.diag([0.1, 0.1, 0.05]) # Process noise
self.R_gps = np.diag([2.0, 2.0]) # GPS noise
self.R_imu = np.array([[0.01]]) # IMU heading noise
def predict(self, v: float, omega: float, dt: float):
"""
Predict using motion model.
v: linear velocity (from wheel encoders)
omega: angular velocity (from gyroscope)
"""
theta = self.x[2, 0]
# Nonlinear motion model
# x_new = x + v * cos(theta) * dt
# y_new = y + v * sin(theta) * dt
# theta_new = theta + omega * dt
dx = np.array([
[v * np.cos(theta) * dt],
[v * np.sin(theta) * dt],
[omega * dt]
])
self.x = self.x + dx
# Normalize theta to [-pi, pi]
self.x[2, 0] = (self.x[2, 0] + np.pi) % (2 * np.pi) - np.pi
# Jacobian of motion model (F = df/dx)
F = np.array([
[1, 0, -v * np.sin(theta) * dt],
[0, 1, v * np.cos(theta) * dt],
[0, 0, 1]
])
self.P = F @ self.P @ F.T + self.Q
return self.x.copy()
def update_gps(self, gps_x: float, gps_y: float):
"""Update with GPS measurement (x, y)."""
z = np.array([[gps_x], [gps_y]])
H = np.array([
[1, 0, 0],
[0, 1, 0]
])
self._update(z, H, self.R_gps)
def update_imu_heading(self, heading: float):
"""Update with IMU compass heading."""
z = np.array([[heading]])
H = np.array([[0, 0, 1]])
# Handle angle wrapping in innovation
y = z - H @ self.x
y[0, 0] = (y[0, 0] + np.pi) % (2 * np.pi) - np.pi
S = H @ self.P @ H.T + self.R_imu
K = self.P @ H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.x[2, 0] = (self.x[2, 0] + np.pi) % (2 * np.pi) - np.pi
self.P = (np.eye(3) - K @ H) @ self.P
def _update(self, z, H, R):
"""Generic update step."""
y = z - H @ self.x # Innovation
S = H @ self.P @ H.T + R # Innovation covariance
K = self.P @ H.T @ np.linalg.inv(S) # Kalman Gain
self.x = self.x + K @ y
self.P = (np.eye(3) - K @ H) @ self.P
def get_state(self):
return self.x.flatten() # [x, y, theta]
Simulation: EKF Sensor Fusion (Encoder + IMU + GPS)
# Simulate robot traveling in a circle
np.random.seed(42)
dt = 0.05 # 20 Hz control
duration = 60 # 60 seconds
# Robot parameters
v_true = 0.5 # 0.5 m/s
omega_true = 0.1 # Turning rate (rad/s) → circular path
times = np.arange(0, duration, dt)
n_steps = len(times)
# Ground truth trajectory (circle)
true_x, true_y, true_theta = [0.0], [0.0], [0.0]
for i in range(1, n_steps):
true_theta.append(true_theta[-1] + omega_true * dt)
true_x.append(true_x[-1] + v_true * np.cos(true_theta[-1]) * dt)
true_y.append(true_y[-1] + v_true * np.sin(true_theta[-1]) * dt)
true_x, true_y, true_theta = np.array(true_x), np.array(true_y), np.array(true_theta)
# Noisy sensor readings
encoder_v = v_true + np.random.normal(0, 0.05, n_steps) # Encoder velocity
imu_omega = omega_true + np.random.normal(0, 0.02, n_steps) # Gyroscope
gps_x = true_x + np.random.normal(0, 1.5, n_steps) # GPS x
gps_y = true_y + np.random.normal(0, 1.5, n_steps) # GPS y
imu_heading = true_theta + np.random.normal(0, 0.1, n_steps) # Compass
# Run EKF
ekf = ExtendedKalmanFilter()
est_x, est_y, est_theta = [], [], []
for i in range(n_steps):
# Predict with encoder + gyro (every step, 20 Hz)
ekf.predict(encoder_v[i], imu_omega[i], dt)
# Update with GPS (every 1 second = every 20 steps)
if i % 20 == 0:
ekf.update_gps(gps_x[i], gps_y[i])
# Update with IMU heading (every 5 steps = 4 Hz)
if i % 5 == 0:
ekf.update_imu_heading(imu_heading[i])
state = ekf.get_state()
est_x.append(state[0])
est_y.append(state[1])
est_theta.append(state[2])
# Plot
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# Trajectory
axes[0].plot(true_x, true_y, 'g-', linewidth=2, label='Ground Truth')
axes[0].scatter(gps_x[::20], gps_y[::20], c='r', s=15, alpha=0.5, label='GPS')
axes[0].plot(est_x, est_y, 'b-', linewidth=1.5, label='EKF Estimate')
axes[0].set_xlabel('X (m)')
axes[0].set_ylabel('Y (m)')
axes[0].set_title('Robot Trajectory -- EKF Sensor Fusion')
axes[0].legend()
axes[0].set_aspect('equal')
axes[0].grid(True, alpha=0.3)
# Error over time
error = np.sqrt((np.array(est_x) - true_x)**2 + (np.array(est_y) - true_y)**2)
axes[1].plot(times, error, 'b-', label='Position Error (EKF)')
axes[1].set_xlabel('Time (s)')
axes[1].set_ylabel('Error (m)')
axes[1].set_title('Localization Error Over Time')
axes[1].legend()
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('ekf_sensor_fusion.png', dpi=150)
plt.show()
ROS 2 robot_localization Package
In practice, you don't need to implement EKF from scratch. ROS 2 provides the robot_localization package with ready-made EKF and UKF nodes.
Configuring robot_localization
# ekf.yaml -- Configuration for robot_localization EKF node
ekf_filter_node:
ros__parameters:
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: true # 2D robot (ground robot)
# World frame
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# Sensor 1: Wheel odometry
odom0: /wheel_odom
odom0_config: [true, true, false, # x, y, z
false, false, true, # roll, pitch, yaw
true, true, false, # vx, vy, vz
false, false, true, # vroll, vpitch, vyaw
false, false, false] # ax, ay, az
# Sensor 2: IMU
imu0: /imu/data
imu0_config: [false, false, false, # x, y, z
false, false, true, # roll, pitch, yaw
false, false, false, # vx, vy, vz
false, false, true, # vroll, vpitch, vyaw
true, true, false] # ax, ay, az
imu0_differential: true # Use differential mode for heading
# Sensor 3: GPS (optional, requires navsat_transform)
# odom1: /gps/odom
# odom1_config: [true, true, false, ...]
# Process noise
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
Launch File
# ekf_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
config = os.path.join(
get_package_share_directory('my_robot_localization'),
'config', 'ekf.yaml'
)
return LaunchDescription([
Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[config],
remappings=[
('odometry/filtered', '/odom/filtered'),
]
),
])
Comparing KF, EKF, and UKF
| Feature | Kalman Filter | Extended KF (EKF) | Unscented KF (UKF) |
|---|---|---|---|
| System type | Linear only | Nonlinear (linearized) | Nonlinear (sigma points) |
| Accuracy | Optimal (for linear) | First-order approximation | Second-order approximation |
| Jacobian needed | No | Yes (manual or autodiff) | No |
| Speed | Fastest | Fast | ~2x slower |
| When to use | Linear systems | "Light" nonlinear systems | Highly nonlinear systems |
| ROS 2 support | robot_localization | robot_localization | robot_localization |
In practice, EKF is the most popular choice for robot localization because it balances accuracy and computational cost well. UKF is only needed when nonlinearity is very strong (e.g., underwater robots with complex dynamics).
References
- Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press -- classic reference on robot localization
- arXiv:2410.10409 -- SMART-TRACK: Kalman Filter-Guided Sensor Fusion for UAV Object Tracking
- arXiv:2509.22693 -- Mobile Robot Localization via Indoor Positioning System and Odometry Fusion
Related Articles
- PID Control for Robots: From Theory to Practice -- Control fundamentals before sensor fusion
- SLAM Navigation for Robots -- Next step after localization
- Nav2 Series: SLAM for Mobile Robots -- Detailed SLAM in ROS 2
- LiDAR 3D Mapping for Robots -- LiDAR sensors and mapping
- Python for Robots: From Basics to ROS 2 -- Robot programming with Python
