Kalman Filter

The Kalman filter is an optimal estimator in the minimum mean-square error sense for linear systems with Gaussian noise. It is recursive, meaning it processes measurements sequentially, updating its state estimate with each new observation while discarding old data. This makes it extremely memory-efficient and suitable for real-time applications. The algorithm maintains two key quantities: the state estimate (the best guess of the system's true state) and the error covariance (a measure of the estimate's uncertainty).

The algorithm operates in a continuous, two-phase cycle:

• Prediction (Time Update): Uses the system's process model to project the current state and its uncertainty forward in time. This predicts where the system should be before new sensor data arrives.
• Update (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. The state and covariance are then corrected, producing a fused estimate that is more accurate than either source alone.

The filter represents all quantities—state, measurements, and noise—as probability distributions. The core assumption is that process and measurement noise are zero-mean Gaussian (white noise). The state is represented by a Gaussian distribution defined by a mean (the estimate) and a covariance matrix (the uncertainty). This probabilistic representation allows the filter to rigorously quantify uncertainty at every step and propagate it through the system dynamics, a critical feature for safety-critical systems like autonomous vehicles.

The Kalman filter is explicitly designed to function with imperfect sensors. The measurement model describes how sensors observe the true state, incorporating known sensor noise. It can also estimate unobserved states. For example, using only position measurements from a GPS, a Kalman filter can accurately estimate velocity and acceleration (the system's hidden states) by leveraging the dynamics defined in the process model. This ability to infer more than is directly measured is a key advantage.

The standard Kalman filter is optimal for linear dynamic systems. Most real-world systems (like robotics) are nonlinear. This led to the development of two primary nonlinear extensions:

• Extended Kalman Filter (EKF): Linearizes the nonlinear process and measurement models around the current state estimate using a first-order Taylor series expansion. 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 highly nonlinear systems.

The Kalman filter is the fundamental engine for tightly-coupled sensor fusion. It provides the mathematical framework to combine raw, heterogeneous data streams (e.g., IMU accelerations, GPS coordinates, LiDAR points, visual features) into a single, consistent state estimate. Its variants form the core of critical navigation systems like Visual-Inertial Odometry (VIO) and LiDAR-Inertial Odometry (LIO). In these systems, the filter's ability to model cross-correlations between different state variables (via the covariance matrix) is essential for accuracy.

Bayesian filtering is the overarching probabilistic framework for recursively estimating the state of a dynamic system. It applies Bayes' theorem to update a belief (the posterior probability) about the state as new measurements arrive. The Kalman filter is a specific, optimal implementation of a Bayesian filter under the assumptions of linear dynamics and Gaussian noise. Other algorithms, like the particle filter, are also Bayesian filters designed for different problem characteristics.

The Extended Kalman Filter (EKF) is the most common nonlinear extension of the standard Kalman filter. It handles nonlinear process and measurement models by linearizing them around the current state estimate using a first-order Taylor series expansion. The core Kalman filter equations are then applied to this local linear approximation. While widely used in robotics (e.g., for visual-inertial odometry), the EKF can introduce significant error for highly nonlinear systems due to the approximation.

The Unscented Kalman Filter (UKF) is a more sophisticated nonlinear estimator that avoids the analytical linearization of the EKF. Instead, it uses a deterministic sampling technique called the unscented transform. A minimal set of carefully chosen sample points (sigma points) are propagated through the true nonlinear functions. The mean and covariance of the transformed state are then estimated from these propagated points. The UKF often provides better performance and stability than the EKF for strongly nonlinear systems.

A particle filter is a sequential Monte Carlo method for Bayesian state estimation. It represents the posterior probability distribution using a large set of random samples called particles, each with an associated weight. This approach makes no assumptions of linearity or Gaussian noise. It is exceptionally powerful for:

• Highly nonlinear dynamics
• Non-Gaussian noise distributions
• Multi-modal posteriors (tracking multiple possible states) The primary computational cost is the need for a large number of particles to accurately represent complex distributions.

A factor graph is a bipartite graphical model that represents the factorization of a complex probability distribution. In state estimation, it provides a unifying framework for structuring problems like SLAM and bundle adjustment. Factors represent probabilistic constraints (from measurements or motion models) connecting variable nodes (states to be estimated). Solving the graph via nonlinear optimization (e.g., Maximum A Posteriori estimation) provides the optimal state estimate. This framework is the backbone of modern, smoothing-based estimation libraries.

The Error State Kalman Filter (ESKF) is a variant that estimates the error in a nominal state rather than the full state itself. The nominal state is propagated using a straightforward integration (e.g., of IMU data), while the filter maintains a small error state that is assumed to remain in a linear regime. This separation offers key advantages:

• Improved numerical stability, especially for attitude estimation on manifolds like SO(3).
• The error state dynamics are often simpler and more linear than the full state dynamics.
• It cleanly separates high-frequency sensor updates from low-frequency nominal state propagation.