Kalman Filter

The Kalman filter is an optimal recursive estimator for linear dynamic systems with Gaussian noise. It processes measurements sequentially, updating its state estimate with each new observation. This is in contrast to batch estimators that reprocess all past data. The algorithm is optimal in the minimum mean-square error (MMSE) sense, meaning it provides the statistically best estimate given the model and noise assumptions. Its recursive nature makes it computationally efficient and suitable for real-time applications like robotic perception, where data arrives continuously.

The algorithm operates through a strict two-phase cycle:

• Prediction Step: Uses the system's dynamic model to project the current state and its uncertainty (covariance) forward in time.
• Update/Correction Step: Incorporates a new sensor measurement to correct the prediction. The key mechanism is the Kalman Gain, a weighting factor that balances trust in the model's prediction versus the new measurement based on their respective uncertainties. This cycle continuously refines the state estimate, converging toward the true state over time.

A defining feature is its explicit probabilistic handling of uncertainty. The filter maintains not just a state estimate (e.g., position, velocity) but also a covariance matrix representing the uncertainty in that estimate. This covariance is propagated through the prediction step (uncertainty grows) and reduced in the update step (uncertainty shrinks). This allows the filter to quantify the confidence of its estimates, a critical requirement for safety-critical systems like autonomous vehicles making navigation decisions.

The Kalman filter is a fundamental sensor fusion engine. It can seamlessly combine data from multiple, heterogeneous sensors (e.g., camera, IMU, LiDAR) with different noise characteristics and update rates into a single, consistent state estimate. In the update step, measurements from any sensor can be incorporated as long as a measurement model exists. This fusion produces an estimate that is more accurate, complete, and reliable than any single sensor could provide, which is essential for robust robotic perception in dynamic environments.

The classic Kalman filter's optimality is guaranteed under two key assumptions:

• Linearity: The system dynamics and measurement models must be linear functions of the state.
• Gaussian Noise: Process noise (model inaccuracy) and measurement noise must be additive white Gaussian noise. These assumptions are often violated in real-world robotics (e.g., camera models are nonlinear). This limitation led to the development of nonlinear variants like the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF), which linearize or approximate the models to handle nonlinearities.

For a state vector of dimension n, the computational complexity is O(n³), primarily due to matrix inversions in the Kalman gain calculation. However, for many practical problems in robotics (e.g., tracking a robot's 2D pose), n is small (e.g., 3), making the filter extremely fast and suitable for real-time operating systems (RTOS) on embedded hardware. Its fixed, predictable computation per cycle enables deterministic execution, which is a non-negotiable requirement for control loops in autonomous systems.

Sensor fusion is the algorithmic process of combining data from multiple disparate sensors (e.g., camera, LiDAR, IMU, radar) to produce a more accurate, complete, and reliable estimate of the environment's state than is possible with any single sensor alone. The Kalman filter is a foundational probabilistic framework for sensor fusion.

• Purpose: Overcome individual sensor limitations like noise, drift, and occlusion.
• Levels: Data can be fused at the raw, feature, or decision level.
• Example: Fusing a camera's object detection with LiDAR's precise depth measurement to create a confident 3D track.

The Extended Kalman Filter is the nonlinear version of the standard Kalman filter. It handles systems where the dynamics or measurement models are nonlinear by linearizing the models around the current state estimate using a first-order Taylor expansion (the Jacobian matrix).

• Core Mechanism: Applies the standard Kalman filter equations to a locally linear approximation of the system.
• Use Case: Ubiquitous in robotics for tasks like visual-inertial odometry, where motion and projection models are inherently nonlinear.
• Limitation: Can diverge if the linearization is poor for highly nonlinear systems or large uncertainties.

A particle filter is a sequential Monte Carlo method used for state estimation in highly nonlinear and non-Gaussian systems. It represents the posterior probability distribution using a set of random samples called particles, each with an associated weight.

• Core Mechanism: Uses importance sampling and resampling to propagate a cloud of hypotheses about the system's state.
• Advantage over KF: Makes no Gaussian assumption; can model multi-modal distributions (e.g., tracking an object that may be behind one of several obstacles).
• Trade-off: Computationally more expensive than Kalman filters, scaling with the number of particles.

SLAM is the computational problem where an agent must construct a map of an unknown environment while simultaneously estimating its own location within that map. Kalman filters (and EKFs) were used in early filter-based SLAM approaches.

• Kalman Filter SLAM: The state vector contains both the robot pose and the positions of all mapped landmarks. The covariance matrix grows quadratically, limiting scalability.
• Modern Approaches: Graph-based optimization (e.g., pose-graph SLAM) and particle filters (FastSLAM) are now more common for large-scale mapping.
• Foundation: The Kalman filter's predict-update cycle is the conceptual backbone for online state and map estimation.

VIO is a specific sensor fusion technique that combines visual data from a camera with inertial data from an Inertial Measurement Unit to estimate the 6-degree-of-freedom pose and velocity of a platform. The Extended Kalman Filter is a standard workhorse algorithm for VIO.

• Sensor Synergy: The camera provides absolute but sometimes slow or erratic scale/pose; the IMU provides high-frequency, smooth motion data but drifts over time. The EKF optimally fuses them.
• Process: Visual features provide measurements to correct the state predicted by the IMU's motion model.
• Application: Essential for autonomous drones, AR/VR headsets, and mobile robots operating in GPS-denied environments.

Mahalanobis distance is a statistical measure of the distance between a point and a distribution, which accounts for the correlations between variables via the covariance matrix. It is a critical tool within Kalman filtering for data association.

• Formula: ( D_M = \sqrt{(z - H\hat{x})^T S^{-1} (z - H\hat{x})} ), where ( S ) is the innovation covariance.
• Use in Gating: Before a sensor measurement is used to update the Kalman filter state, its Mahalanobis distance is computed. If it exceeds a threshold (e.g., based on a chi-squared distribution), it is rejected as an outlier or irrelevant observation.
• Significance: Provides a probabilistically meaningful distance metric, far superior to Euclidean distance for correlated, multi-dimensional state spaces.