Kalman Filter

The algorithm operates in a continuous two-step loop. The predict step uses a system's dynamic model to project the current state and its uncertainty forward in time. The update step (or correct step) then incorporates a new, noisy measurement to refine this prediction, producing a statistically optimal estimate that balances model prediction with sensor observation.

The Kalman filter provides the minimum mean square error (MMSE) estimate, meaning it is statistically optimal, under two key assumptions: all process and measurement noise is Gaussian (normally distributed) and the system dynamics are linear. This optimality makes it the best possible linear estimator for tracking and sensor fusion tasks where these conditions hold.

A core strength is its maintenance of a covariance matrix, which quantifies the estimated uncertainty (error) in the state. This matrix is propagated and updated alongside the state estimate itself. This allows the filter to:

• Weight sensor readings based on their known accuracy.
• Fuse multiple sensors optimally, trusting more certain estimates.
• Provide a confidence metric for its predictions, crucial for safety-critical systems.

Despite its mathematical sophistication, the Kalman filter is computationally lightweight. Its operations are primarily matrix multiplications and inversions on small matrices (for typical state vectors). This fixed, predictable computational footprint makes it ideal for real-time execution on microcontrollers with limited memory and CPU, unlike more complex non-linear filters.

The standard Kalman filter assumes linear system dynamics. For real-world non-linear systems (e.g., robotics, aerospace), it serves as the foundation for powerful extensions:

• Extended Kalman Filter (EKF): Linearizes the system model around the current estimate.
• Unscented Kalman Filter (UKF): Uses a deterministic sampling technique to handle non-linearities more accurately and stably than the EKF.
• Ensemble Kalman Filter (EnKF): Uses a Monte Carlo approach, suitable for very high-dimensional states.

The Kalman filter's architecture is inherently designed for multi-sensor data fusion. It can seamlessly integrate asynchronous measurements from different sensors (e.g., GPS, IMU, wheel encoders), each with its own noise characteristics and update rates. By modeling each sensor's error in the measurement covariance matrix, it produces a unified, more accurate, and reliable state estimate than any single sensor could provide.

Sensor fusion is the broader process of combining data from multiple, often heterogeneous, sensors to produce estimates with greater accuracy, reliability, and completeness than is possible from a single source. The Kalman filter is a premier algorithm for performing statistical sensor fusion, optimally merging data streams (e.g., from an IMU and GPS) by weighting each source according to its estimated uncertainty.

• Types: Includes complementary, competitive, and cooperative fusion.
• Key Benefit: Reduces uncertainty and provides robustness against single-sensor failure.
• Common Framework: The Kalman filter provides a recursive, probabilistic framework for fusing sequential measurements.

The Kalman filter is a specific, optimal implementation of recursive Bayesian estimation for linear Gaussian systems. This general framework describes how to update the probability distribution for a system's state as new evidence (measurements) arrives.

• Core Principle: It recursively applies Bayes' theorem to combine a prior belief (prediction) with a likelihood (measurement).
• Prediction Step: Uses a system model to project the state forward.
• Update Step: Incorporates a new measurement to correct the prediction.
• Kalman Filter's Role: It provides closed-form, computationally efficient solutions when all noise is Gaussian and models are linear.

The Extended Kalman Filter (EKF) is the most common nonlinear extension of the standard Kalman filter. It linearizes the nonlinear system and measurement models around the current state estimate using a first-order Taylor series expansion.

• Application: Used for systems with nonlinear dynamics or measurement functions (e.g., robotics, aerospace).
• Process: At each step, the EKF calculates local Jacobian matrices (partial derivatives) to approximate the nonlinear functions as linear for that operating point.
• Limitation: Performance degrades with high nonlinearity, as the linearization may be a poor approximation.

The Unscented Kalman Filter (UKF) is a more modern alternative to the EKF for nonlinear estimation. Instead of linearizing, it uses a deterministic sampling technique called the Unscented Transform to propagate a minimal set of carefully chosen sample points (sigma points) through the true nonlinear functions.

• Advantage over EKF: Often provides better accuracy for highly nonlinear systems and is easier to implement as it doesn't require deriving Jacobian matrices.
• Mechanism: The mean and covariance of the state are reconstructed from the transformed sigma points.
• Computational Cost: Similar to the EKF, making it suitable for many real-time applications.

A Particle Filter is a sequential Monte Carlo method used for Bayesian estimation in nonlinear, non-Gaussian systems. It represents the state's probability distribution using a set of random samples called particles, each with an associated weight.

• Process: Particles are propagated through the system model, then re-weighted based on the likelihood of the new measurement. Resampling is performed to avoid degeneracy.
• Strength: Can handle complex multi-modal distributions (tracking multiple hypotheses) where Kalman filters fail.
• Cost: Computationally intensive, as accuracy depends on the number of particles, making it less suitable for highly resource-constrained devices compared to EKF/UKF.

An Inertial Measurement Unit (IMU) is a key sensor package often fused using a Kalman filter. It typically contains a 3-axis accelerometer and a 3-axis gyroscope, and sometimes a magnetometer.

• Role in Filtering: IMUs provide high-frequency, short-term motion data but suffer from integration drift (bias and noise accumulate when calculating position from acceleration).
• Sensor Fusion Example: A Kalman filter fuses noisy, high-rate IMU data with absolute but lower-rate positional data from GPS or visual odometry. The filter uses the IMU for smooth prediction and the absolute source for periodic correction, yielding a robust, high-frequency pose estimate.
• Critical Parameters: Filter performance heavily depends on accurately modeled IMU noise characteristics (measurement covariance).