Table of Contents
The Extended Kalman Filter (EKF) is a widely used algorithm in robot localization. It estimates the robot’s position and orientation by combining sensor data with a mathematical model of the robot’s motion. Understanding the mathematical foundations of EKF is essential for effective implementation and tuning.
State Representation and Prediction
The core of EKF involves representing the robot’s state as a vector, typically including position and orientation. The prediction step uses a nonlinear motion model to project the current state forward in time. This involves calculating the Jacobian matrix of the motion model to linearize the nonlinear equations around the current estimate.
Measurement Update and Linearization
Sensor measurements are incorporated through the update step. Since measurements are often nonlinear functions of the state, EKF linearizes these functions using their Jacobians. This process adjusts the predicted state based on the difference between expected and actual sensor readings.
Mathematical Equations
The prediction equations are:
State prediction:
x̂ₖ₊₁⁻ = f(x̂ₖ, uₖ)
Covariance prediction:
Pₖ₊₁⁻ = Fₖ Pₖ Fₖᵗ + Qₖ
where f is the nonlinear motion model, Fₖ is its Jacobian, P is the covariance matrix, and Q is the process noise covariance.
The update equations are:
Kalman gain:
Kₖ = Pₖ⁻ Hₖᵗ (Hₖ Pₖ⁻ Hₖᵗ + Rₖ)⁻¹
State update:
x̂ₖ = x̂ₖ⁻ + Kₖ (zₖ – h(x̂ₖ⁻))
Covariance update:
Pₖ = (I – Kₖ Hₖ) Pₖ⁻
Here, h is the nonlinear measurement model, Hₖ is its Jacobian, Rₖ is the measurement noise covariance, and zₖ is the actual sensor measurement.