Extended Kalman Filter (EKF)

The core mechanism of the EKF is the linearization of nonlinear functions via a first-order Taylor expansion. For a nonlinear state transition function f(x) and measurement function h(x), the EKF computes Jacobian matrices F and H. These matrices, which contain the partial derivatives of the functions with respect to the state, are evaluated at the current state estimate (x̂). This linear approximation allows the filter to propagate the Gaussian uncertainty through the nonlinear models, a process the standard Kalman Filter cannot perform.

• State Transition Jacobian (F): Fₖ = ∂f/∂x |ₓ=ₓ̂ₖ₋₁
• Measurement Jacobian (H): Hₖ = ∂h/∂x |ₓ=ₓ̂ₖ₋

The EKF operates in a recursive two-step cycle, maintaining a state estimate x̂ and its associated error covariance matrix P.

1. Prediction Step:

• Projects the state forward using the nonlinear model: x̂ₖ₋ = f(x̂ₖ₋₁, uₖ).
• Projects the covariance forward using the linearized model: Pₖ₋ = Fₖ Pₖ₋₁ Fₖᵀ + Qₖ. Q is the process noise covariance.

2. Update (Correction) Step:

• Computes the Kalman Gain Kₖ, which weights the confidence between the prediction and the new measurement zₖ.
• Updates the state estimate by incorporating the measurement residual: x̂ₖ = x̂ₖ₋ + Kₖ (zₖ - h(x̂ₖ₋)).
• Updates the covariance to reflect the reduced uncertainty: Pₖ = (I - Kₖ Hₖ) Pₖ₋.

The EKF is a foundational algorithm in Simultaneous Localization and Mapping (SLAM) and robotic state estimation. Its ability to fuse asynchronous, multi-modal sensor data into a single consistent state estimate is critical for autonomy.

• Visual-Inertial Odometry (VIO): Fuses camera features with IMU data to estimate 6-DOF pose.
• LiDAR Odometry: Estimates robot motion by matching consecutive LiDAR scans.
• Sensor Fusion: Integrates GPS, wheel encoders (odometry), and IMU data for robust localization.
• Landmark-based SLAM (EKF-SLAM): Maintains a joint state vector containing both the robot pose and the estimated positions of observed landmarks in the map, updating them simultaneously.

While powerful, the EKF has well-understood limitations that engineers must design around.

• Linearization Error: The first-order approximation is only valid locally. For highly nonlinear systems or large uncertainties, the linearization can introduce significant error, causing filter divergence.
• Computational Cost: The need to compute Jacobians analytically or via numerical differentiation at each step adds overhead. The covariance update is O(n²) in the state dimension n.
• Gaussian Assumption: The EKF assumes all uncertainties are Gaussian. It cannot represent multi-modal distributions, which can occur during data association ambiguity or symmetric environments.
• Susceptibility to Outliers: The filter relies on correct data association. Incorrect matching of sensor observations to map landmarks can catastrophically corrupt the state estimate.

The EKF sits within a family of Bayesian filters, each with different trade-offs.

• vs. Kalman Filter (KF): The KF is optimal for linear Gaussian systems. The EKF is a heuristic but effective extension for nonlinear systems.
• vs. Unscented Kalman Filter (UKF): The UKF uses a deterministic sampling approach (the unscented transform) to propagate uncertainty through nonlinear functions, often providing better accuracy than the EKF's Jacobian-based linearization, especially for strong nonlinearities.
• vs. Particle Filter: Particle filters represent the state distribution with a set of samples, making them capable of modeling non-Gaussian, multi-modal distributions. However, they are typically far more computationally expensive than the EKF, making the EKF preferable for high-frequency, embedded real-time systems where the Gaussian assumption holds.

Successfully deploying an EKF requires careful engineering beyond the core algorithm.

• Jacobian Computation: Use automatic differentiation (e.g., via ceres-solver or jax) for reliability and to avoid manual derivation errors.
• Tuning Noise Covariances (Q, R): The process noise Q and measurement noise R matrices are critical tuning parameters. They are often determined empirically or via system identification.
• Numerical Stability: Use the Joseph form for the covariance update (Pₖ = (I - Kₖ Hₖ) Pₖ₋ (I - Kₖ Hₖ)ᵀ + Kₖ Rₖ Kₖᵀ) to ensure P remains symmetric positive definite.
• Initialization: The filter requires a good initial state estimate x̂₀ and covariance P₀. Poor initialization can lead to slow convergence or divergence.

The Kalman Filter is the foundational recursive algorithm for optimal state estimation in linear dynamic systems with Gaussian noise. It operates in a two-step predict-update cycle: predicting the next state using a motion model, then correcting it with a new measurement. Its optimality is mathematically proven for linear systems, making it the benchmark for sensor fusion and tracking. The EKF is a direct nonlinear extension of this core algorithm.

The Unscented Kalman Filter is an alternative to the EKF for handling nonlinearities. Instead of linearizing models with a Jacobian, the UKF uses a deterministic sampling technique called the Unscented Transform. It selects a minimal set of sigma points that capture the mean and covariance of the state distribution, propagates these points through the nonlinear functions, and then reconstructs a new Gaussian estimate. This often provides more accurate covariance estimates than the EKF, especially for highly nonlinear systems, without the need to compute Jacobians.

A Particle Filter is a sequential Monte Carlo method for state estimation that does not assume Gaussian distributions or linear models. It represents the posterior probability distribution with a set of discrete samples called particles, each with an associated weight. It is exceptionally powerful for multi-modal distributions (e.g., global localization) and highly nonlinear/non-Gaussian problems. However, it is computationally more expensive than Kalman-type filters. The trade-off is flexibility vs. computational cost.

Sensor Fusion is the broader process of combining data from multiple, often heterogeneous, sensors to produce a state estimate that is more accurate, complete, and reliable than could be obtained from any single sensor. The EKF is a primary algorithmic tool for probabilistic sensor fusion. It fuses measurements by weighting them according to their covariance matrices (uncertainty). Common fusion pairs in robotics include Visual-Inertial Odometry (VIO), which uses cameras and IMUs, and LiDAR-inertial systems.

The Jacobian Matrix is the mathematical linchpin of the EKF. It is the matrix of all first-order partial derivatives of a vector-valued function. In the EKF:

• The process model Jacobian linearizes the motion model around the current state estimate.
• The observation model Jacobian linearizes the measurement model. These Jacobians are recalculated at every filter step to create a local linear approximation, which is then used in the standard Kalman filter equations. Computing accurate Jacobians is critical for EKF performance.

The Covariance Matrix is the core representation of uncertainty within the Kalman filter framework. In the EKF, this matrix quantifies the estimated error covariance of the state. It is propagated through the linearized models during the prediction step and updated during the correction step. The matrix's diagonals represent the variance (uncertainty) of each state variable (e.g., x-position, yaw), while off-diagonals represent the correlations between state variables (e.g., how an error in x relates to an error in velocity).