Kalman Filter

The Kalman filter is a recursive algorithm, meaning it processes measurements sequentially and updates its estimate as new data arrives. It does not require storing the entire history of past measurements. The current state estimate is a function of only the previous estimate and the latest measurement. This makes it extremely memory-efficient and suitable for real-time, streaming applications like tracking a moving vehicle or processing sensor data from a robot.

The algorithm operates in a strict, repeating two-step cycle:

• Prediction Step (Time Update): Uses the system's process model (e.g., equations of motion) to project the current state and its uncertainty (covariance matrix) forward in time. This yields an a priori (predicted) estimate.
• Update Step (Measurement Update): Incorporates a new, noisy sensor measurement. It computes the Kalman Gain, a weighting factor that balances the confidence in the prediction versus the confidence in the new measurement, to produce an optimal a posteriori (corrected) estimate. This cycle continuously refines the state estimate.

Under specific assumptions, the Kalman filter provides the minimum mean square error (MMSE) estimate. This optimality is guaranteed when:

• The system dynamics (process model) are linear.
• The observation model (how measurements relate to state) is linear.
• The process noise (modeling error) and measurement noise (sensor error) are zero-mean, white, and Gaussian (normally distributed). Within this framework, no other linear algorithm can produce a more accurate estimate on average. This makes it the benchmark for linear state estimation problems.

A defining feature is its maintenance of a covariance matrix, P. This matrix quantifies the estimated uncertainty (error) of the state vector. It is propagated through the prediction step (uncertainty grows) and reduced in the update step (uncertainty shrinks with new information). The Kalman Gain is derived directly from these covariance matrices. This provides a rigorous, probabilistic confidence interval for every estimate, which is critical for safety-critical systems like aviation or autonomous driving.

The Kalman Gain, K, is the engine of the update step. It is a matrix that optimally weights the difference between the new sensor measurement and the predicted measurement (the innovation or residual).

• If the sensor measurement is very noisy (high measurement covariance R), K is small, and the filter trusts the prediction more.
• If the prediction is very uncertain (high predicted state covariance P), K is large, and the filter trusts the new measurement more. This dynamic weighting is what allows the filter to smoothly fuse data from sources of varying reliability.

The standard Kalman filter's requirement for linearity is a major limitation for real-world systems. This led to the development of nonlinear variants that preserve its core recursive, probabilistic structure:

• Extended Kalman Filter (EKF): Linearizes the nonlinear process and observation models around the current state estimate using a Jacobian matrix. It is the most widely used nonlinear variant.
• Unscented Kalman Filter (UKF): Uses a deterministic sampling technique (the unscented transform) to propagate a set of points through the true nonlinear functions, often providing better accuracy and stability than the EKF for strong nonlinearities. These extensions demonstrate the Kalman filter's foundational role in estimation theory.

The Extended Kalman Filter is the nonlinear extension of the standard Kalman filter. It handles nonlinear process and observation models by linearizing them around the current state estimate using a first-order Taylor expansion (the Jacobian matrix).

• Core Mechanism: At each prediction and update step, the EKF computes Jacobians to approximate the nonlinear functions as linear for covariance propagation.
• Primary Use Case: Ubiquitous in robotics for tasks like visual-inertial odometry and GPS/INS navigation, where sensor models are inherently nonlinear.
• Limitation: Linearization errors can cause filter divergence if the system is highly nonlinear or the initial estimate is poor.

The Unscented Kalman Filter is a more advanced nonlinear estimator that uses a deterministic sampling approach called the unscented transform.

• Core Mechanism: Instead of linearizing, the UKF selects a minimal set of sigma points that capture the mean and covariance of the state distribution. These points are propagated through the true nonlinear functions, and a new mean and covariance are estimated from the transformed points.
• Advantage over EKF: Often provides more accurate estimates for highly nonlinear systems because it better approximates the posterior distribution, avoiding Jacobian calculations.
• Typical Application: Preferred for systems with severe nonlinearities, such as attitude estimation using quaternions or certain radar tracking problems.

A Particle Filter is a sequential Monte Carlo method used for state estimation in nonlinear and non-Gaussian systems. It represents the posterior probability distribution with a set of random samples (particles) and their associated weights.

• Core Mechanism: Operates in a predict-update-resample cycle. Particles are propagated through the motion model, then weighted by their likelihood given a new measurement. Resampling prevents degeneracy by focusing particles on high-probability regions.
• Key Strength: Makes no Gaussian assumption, making it ideal for multi-modal distributions (e.g., tracking an object that may be in one of several distinct locations).
• Computational Cost: Can be resource-intensive, as accuracy scales with the number of particles. Often used for global localization in robotics.

Bayesian Filtering is the overarching probabilistic framework that provides the theoretical foundation for the Kalman, Particle, and other filters. It formulates state estimation as a recursive application of Bayes' theorem.

• Mathematical Foundation: The goal is to compute the posterior belief bel(x_t) = P(x_t | z_{1:t}, u_{1:t}), where x is the state, z are observations, and u are control inputs.
• Two-Step Recursion:
  • Prediction: bel̄(x_t) = ∫ P(x_t | x_{t-1}, u_t) * bel(x_{t-1}) dx_{t-1}
  • Update: bel(x_t) = η * P(z_t | x_t) * bel̄(x_t)
• Unifying View: The Kalman filter is the optimal closed-form solution when motion and sensor models are linear and noise is Gaussian. The Particle Filter is a sampling-based approximation for the general, nonlinear case.

A Factor Graph is a bipartite graph used to represent the factorization of a complex probability distribution. It is a central data structure in modern graph-based optimization approaches to sensor fusion and SLAM.

• Graph Structure: Contains two node types:
  • Variable Nodes: Represent unknown states (e.g., robot poses, landmark positions).
  • Factor Nodes: Represent probabilistic constraints (e.g., sensor measurements, motion models) that connect variables.
• Connection to Filtering: While filters are recursive, factor graphs are often used for batch optimization over a history of states. Smoothing algorithms like iSAM2 incrementally solve the graph.
• Primary Advantage: Provides a highly flexible and sparse representation, making it efficient for solving large-scale Simultaneous Localization and Mapping (SLAM) problems by performing inference over the entire graph.

The Interacting Multiple Model algorithm is an adaptive estimator designed for systems whose dynamic behavior can switch between different modes or models.

• Core Mechanism: Runs a bank of parallel filters (e.g., multiple KFs or EKFs), each matched to a different hypothesized system model (e.g., "constant velocity," "coordinated turn").
• Interaction & Fusion: At each time step, the estimates and covariances from all filters are mixed based on the estimated probability of each model being correct. The filters then run independently before their outputs are combined into a final state estimate.
• Typical Application: Essential for target tracking of maneuvering vehicles (e.g., aircraft, cars), where a single motion model is insufficient. It provides robust performance when an object's dynamics change unpredictably.