Kalman Filter

The Kalman filter is optimal in the minimum mean-square error (MMSE) sense for linear systems with Gaussian noise. It is recursive, meaning it processes measurements sequentially, updating its estimate as new data arrives without reprocessing the entire history. This makes it computationally efficient for real-time applications like tracking and navigation. The algorithm maintains only the current state estimate and its error covariance, discarding old measurements after they are incorporated.

The algorithm operates in a continuous two-phase cycle:

• Prediction (Time Update): Uses the system's dynamic model to project the current state and its uncertainty forward in time. This yields a prior estimate.
• Update (Measurement Update): Corrects the prior estimate by incorporating a new sensor measurement. The correction is weighted by the Kalman Gain, which balances trust in the model's prediction versus the new measurement based on their respective uncertainties. This cycle fuses predictive modeling with observational data.

A defining feature is its explicit, quantitative representation of uncertainty. The filter maintains a covariance matrix for the state estimate, which encodes the uncertainty (variance) of each state variable and the correlations between them. All operations—prediction, correction, and fusion—propagate and update this covariance matrix. This allows the filter to correctly weight information from multiple, potentially noisy sensors and to quantify the confidence in its own estimates, which is critical for robust decision-making in autonomous systems.

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

• Linearity: Both the system dynamics (how the state evolves) and the measurement model (how sensors observe the state) must be linear functions.
• Gaussian Noise: All process noise (disturbances affecting the system) and measurement noise (sensor errors) are assumed to be zero-mean, white Gaussian noise. These assumptions are mathematically convenient, producing closed-form, efficient update equations. Real-world systems often violate these assumptions, leading to extensions like the Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF).

The Kalman filter provides a principled, probabilistic framework for sensor fusion. It can seamlessly integrate data from multiple, heterogeneous sensors (e.g., IMU, GPS, camera) with different noise characteristics and update rates. The algorithm automatically computes the optimal combination by considering the uncertainty (covariance) of each sensor's measurement relative to the current state uncertainty. This makes it a cornerstone of state estimation in robotics, aerospace, and autonomous vehicles, where combining inertial, visual, and positional data is essential.

For a state vector of dimension n, the computational complexity of one filter iteration is O(n³), dominated by matrix inversions in the Kalman gain calculation. However, for many practical problems, n is small (e.g., 6-16 for pose and velocity), making it suitable for real-time execution on embedded systems. Its efficiency stems from the recursive formulation, which avoids storing a history of measurements. This predictable, bounded computation time is a key reason for its widespread adoption in time-critical control and navigation loops.

The Extended Kalman Filter is the nonlinear extension of the standard Kalman filter. It handles nonlinear system dynamics and measurement 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 new Jacobian matrices to propagate the state covariance.
• Primary Use Case: Ubiquitous in robotics for fusing inertial measurement unit (IMU) data with other sensors, where motion models are inherently nonlinear.
• Limitation: The linearization can introduce significant error for highly nonlinear systems or with poor initial estimates, potentially leading to filter divergence.

The Unscented Kalman Filter is a derivative-free alternative to the EKF for nonlinear estimation. Instead of linearizing, it uses a deterministic sampling technique called the unscented transform.

• Core Mechanism: A small, carefully chosen set of sigma points are propagated through the true nonlinear functions. The mean and covariance of the transformed state are then estimated from these propagated points.
• Advantage over EKF: Often provides more accurate estimates than the EKF for strong nonlinearities, as it captures the posterior mean and covariance to the third order for Gaussian inputs.
• Common Application: Vehicle state estimation (e.g., for autonomous cars) where tire and motion models are highly nonlinear.

A Particle Filter is a sequential Monte Carlo method that represents the state's probability distribution using a set of random samples called particles, each with an associated weight.

• Core Mechanism: It does not assume a Gaussian distribution. Particles are propagated through the system model, then re-weighted based on sensor measurements. Resampling is performed to avoid degeneracy (where most particles have negligible weight).
• Strength: Excels at estimating states in highly nonlinear systems and multi-modal distributions (e.g., global localization where a robot could be in one of several distinct locations).
• Trade-off: Computational cost is significantly higher than Kalman filters, scaling with the number of particles.

Sensor Fusion is the overarching 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.

• Kalman Filter's Role: It is a premier algorithmic framework for probabilistic sensor fusion, optimally blending data based on modeled uncertainty.
• Complementary Sensors: A classic fusion example is Visual-Inertial Odometry (VIO), where a camera (accurate but suffers from scale ambiguity and motion blur) is fused with an IMU (provides high-frequency acceleration and rotation but drifts quickly).
• Architecture Levels: Fusion can occur at the raw data level, feature level, or decision level, with Kalman filters typically operating at the feature or state vector level.

The Covariance Matrix is a fundamental mathematical object in the Kalman filter and all probabilistic estimation. It quantifies the uncertainty in the state estimate and the correlations between different state variables.

• In the Kalman Filter: The state covariance matrix P is propagated and updated alongside the state vector x̂. Its diagonal elements represent the variance (squared uncertainty) of each state element (e.g., position, velocity). Off-diagonal elements represent how errors in one state are related to errors in another.
• Process Noise Q & Measurement Noise R: These are covariance matrices that model the uncertainty in the system dynamics and the sensors, respectively. Tuning these is critical for filter performance.
• Information Form: The inverse of the covariance matrix is the information matrix, which is useful in certain multi-sensor fusion and SLAM back-end optimization problems.

Recursive Bayesian Estimation is the general theoretical framework that underpins the Kalman filter, particle filter, and other state estimators. It provides a probabilistic method for updating a belief about an unknown state as new evidence (measurements) arrives.

• Two-Step Cycle:
  • Prediction (Chapman-Kolmogorov): Propagate the prior belief forward in time using the system model: bel(x_t) = ∫ p(x_t | x_{t-1}) * bel(x_{t-1}) dx_{t-1}.
  • Update (Bayes' Rule): Integrate the new measurement z_t to compute the posterior belief: bel(x_t) = η * p(z_t | x_t) * bel(x_t).
• Kalman Filter as a Special Case: The KF is the optimal closed-form solution to the recursive Bayesian estimation problem when the system is linear and all noise is additive white Gaussian. It efficiently computes the mean (state) and covariance of the belief.