Extended Kalman Filter (EKF)

The core mechanism of the EKF is the first-order Taylor expansion of nonlinear system and measurement models. It calculates Jacobian matrices—partial derivatives of the models with respect to the state and noise variables—at the current state estimate. This linearization approximates the nonlinear functions as linear for a single prediction-update cycle, making the Kalman filter's mathematics applicable.

• Process Model Jacobian (F): Linearizes the state transition function.
• Measurement Model Jacobian (H): Linearizes the function that maps the state to sensor observations.
• This approximation is valid only locally, around the current estimate, and must be recomputed at every time step.

The EKF maintains a Gaussian belief over the system state, characterized by a mean vector and a covariance matrix. It operates in a rigorous two-step recursive cycle:

1. Prediction (Time Update):

• Projects the state mean and covariance forward using the nonlinear process model.
• The covariance is propagated using the linearized Jacobian F to approximate how uncertainty grows due to system dynamics and process noise.

2. Update (Measurement Update):

• When a sensor measurement arrives, the EKF computes the Kalman Gain, which optimally weights the confidence in the prediction versus the new observation.
• The state and covariance are corrected using the innovation (the difference between the actual and predicted measurement), linearized via the Jacobian H.

This cycle runs continuously, fusing noisy sensor data with model predictions.

The EKF's primary utility is estimating state in systems where motion or observations are not straight-line relationships. Common robotics examples include:

• Robot Pose Estimation: Tracking [x, y, theta] where the motion model involves trigonometric functions (sin, cos) for rotation.
• Sensor Fusion: Combining data from inherently nonlinear sensors like a monocular camera (perspective projection), radar (range and bearing), or an IMU (orientation via quaternions or Euler angles).
• Landmark-Based Navigation: Updating a robot's position based on measuring the angle and distance to known landmarks, a nonlinear function of the robot's (x, y) pose.

The EKF provides a mathematically sound framework to unify these diverse, nonlinear data sources into a single consistent state estimate.

Despite its approximations, the EKF is favored in embedded and robotic systems for its predictable computational footprint. Its complexity is O(n^3) where n is the state dimension, which is often manageable for typical state vectors (e.g., 6-15 dimensions for a robot's pose, velocity, and sensor biases).

• Deterministic Runtime: The number of matrix operations is fixed per cycle, crucial for real-time operating systems (RTOS) where deadline guarantees are required.
• Comparison to Alternatives: It is significantly less computationally intensive than the Particle Filter, which requires maintaining and updating hundreds or thousands of samples, especially in high-dimensional state spaces.
• This efficiency enables deployment on onboard computers in drones, autonomous vehicles, and mobile robots.

The EKF's performance is bounded by its underlying assumptions. Violations can lead to divergence (the filter becoming wildly inaccurate). Key limitations include:

• First-Order Approximation Error: The Taylor expansion truncates higher-order terms. If system nonlinearities are severe or uncertainty is large, this linearization error can be significant, causing the filter to become inconsistent (underestimating its own error).
• Gaussian Noise Assumption: The EKF assumes all process and measurement noises are Gaussian. It struggles with multi-modal distributions or heavy-tailed noise.
• Susceptibility to Initialization: A poor initial guess can cause the filter to linearize around the wrong point, leading to divergence.
• These limitations motivate more advanced filters like the Unscented Kalman Filter (UKF) or Particle Filter for highly nonlinear or non-Gaussian problems.

The EKF is a workhorse algorithm in several foundational robotics perception and control stacks:

• Visual-Inertial Odometry (VIO): Fusing camera features with IMU data to track a robot's 6D pose and velocity. The IMU dynamics and camera projection models are nonlinear.
• Sensor Fusion for Localization: Combining wheel odometry (which drifts), IMU data, and occasional absolute measurements from GPS or LiDAR scan-matching within a single EKF framework.
• Simultaneous Localization and Mapping (EKF-SLAM): Maintaining a joint state vector containing both the robot's pose and the positions of observed landmarks in the map, with nonlinear relationships between them.
• Target Tracking: Estimating the state (position, velocity) of other moving objects from sensor data, using nonlinear motion models like coordinated turn.

The foundational optimal recursive estimator for linear systems. It operates in a two-step cycle:

• Prediction: Projects the current state forward using a linear motion model.
• Update (Correction): Fuses the prediction with a new, noisy measurement.

The Kalman filter is optimal for linear systems with Gaussian noise, providing the minimum mean square error estimate. It is the direct linear predecessor to the EKF, which linearizes nonlinear models to apply the same core recursive framework.

The algorithmic process of combining data from multiple, disparate sensors (e.g., camera, LiDAR, IMU, GPS) to produce a state estimate that is more accurate, complete, and reliable than any single sensor could provide. The EKF is a primary fusion engine within this paradigm. Key fusion levels include:

• Low-Level (Raw Data): Fusing signals before feature extraction.
• Feature-Level: Fusing extracted features like positions or velocities.
• Decision-Level: Fusing high-level object classifications or tracks. EKF excels at feature-level fusion, integrating heterogeneous asynchronous measurements into a unified state.

A sequential Monte Carlo method for state estimation that represents the posterior probability distribution using a set of random samples (particles). Unlike the EKF's single Gaussian approximation, a particle filter can model arbitrary, multi-modal distributions, making it robust for highly nonlinear systems or non-Gaussian noise. However, this comes at a high computational cost (particle degeneracy), often requiring thousands of samples versus the EKF's single state vector and covariance matrix. It is used when linearization fails or precise uncertainty shape is critical.

A specific sensor fusion application that tightly couples visual data from a camera with inertial data from an IMU to estimate a platform's 6-degree-of-freedom pose and velocity. The EKF (and its iterative variant, IEKF) is a classic algorithm for implementing VIO. The IMU provides high-frequency, drift-prone motion estimates, while the camera provides lower-frequency, absolute but sometimes unreliable visual constraints. The EKF's prediction step is driven by the IMU, and the update step occurs when visual features are successfully matched, correcting the accumulating IMU drift.

The core challenge of building a map of an unknown environment while simultaneously tracking the agent's location within it. EKF-SLAM was a seminal approach, where the filter's state vector contained both the robot pose and the estimated positions of all observed landmarks. The covariance matrix captured correlations between the robot and every landmark. While limited by computational complexity (O(n²) in landmarks), it established the probabilistic foundation for modern SLAM. It demonstrates the EKF's role in jointly estimating a large, correlated state from sparse, noisy observations.

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 Jacobian linearizes the nonlinear motion model around the current state estimate.
• The observation Jacobian linearizes the nonlinear measurement model. These Jacobians are recalculated at every filter step to produce the local linear approximation. The accuracy of this first-order Taylor expansion directly determines the filter's performance; large errors or high nonlinearity cause linearization error, which can lead to filter divergence. Understanding Jacobians is essential for implementing a correct EKF.