Extended Kalman Filter

Introduction

The Extended Kalman Filter (EKF) is a popular algorithm used in robotics for state estimation. It is an extension of the Kalman Filter, designed to handle nonlinear systems, which are common in robotic applications such as localization, sensor fusion, and SLAM (Simultaneous Localization and Mapping).

Why Use EKF in Robotics?

In robotics, many models of motion and sensors are nonlinear. EKF allows robots to:

  • Estimate their position and velocity from noisy sensor data
  • Fuse data from multiple sensors like GPS, IMU, and wheel encoders
  • Handle real-world uncertainty and noise in dynamic environments

EKF Algorithm Overview

1. Prediction Step

Uses the motion model to predict the new state and covariance: 

x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ)
Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Q
  • f(): nonlinear motion model
  • Fₖ: Jacobian of f() with respect to state
  • Q: process noise covariance

2. Update Step

Uses the measurement to correct the prediction: 

Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + R)⁻¹
x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - h(x̂ₖ⁻))
Pₖ = (I - KₖHₖ)Pₖ⁻
  • h(): nonlinear measurement model
  • Hₖ: Jacobian of h() with respect to state
  • R: measurement noise covariance

Applications in Robotics

1. Mobile Robot Localization

EKF helps a robot determine its position and orientation (pose) by fusing odometry and GPS/IMU data.

2. SLAM (Simultaneous Localization and Mapping)

EKF is widely used in SLAM to estimate both the robot's pose and the map of the environment using landmarks.

3. Sensor Fusion

EKF combines data from multiple sensors such as cameras, LiDAR, IMU, and GPS to produce accurate state estimates.

Advantages

  • Efficient for real-time applications
  • Can handle nonlinear systems through linear approximations
  • Well-understood and widely used in robotics

Limitations

  • Linearization errors can lead to divergence in highly nonlinear systems
  • Requires accurate models and Jacobians
  • Not optimal for all types of non-Gaussian noise

Conclusion

The Extended Kalman Filter remains a fundamental tool in the robotics toolbox for real-time state estimation and sensor fusion. Despite the rise of more advanced methods like the Unscented Kalman Filter (UKF) and Particle Filter, EKF remains a practical and widely-used solution for many robotic systems.