Kalman Filter

The Kalman filter is optimal 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 as new data arrives without reprocessing the entire history. This makes it computationally efficient and suitable for real-time applications.

• Optimality: Minimizes the estimated error covariance.
• Recursion: Requires only the previous state estimate and the new measurement.
• Efficiency: Computational complexity is O(n³) for matrix inversions, but often n is small (e.g., for pose estimation, n=6).

The algorithm operates in a strict, repeating two-step cycle that separates model-based prediction from measurement-based correction.

1. Prediction (Time Update):

• Projects the current state forward in time using the system's dynamic model.
• Increases the uncertainty (covariance) to account for model inaccuracy and process noise.

2. Update (Measurement Update):

• Compares the prediction with the actual sensor measurement.
• Computes the Kalman Gain, which optimally weights the prediction against the new observation.
• Fuses them to produce a corrected state estimate with reduced uncertainty.

The filter represents all quantities—state, measurements, and noise—as probability distributions, specifically Gaussian (normal) distributions. This provides a rigorous mathematical foundation.

• State as Gaussian: Represented by a mean vector (the best estimate) and a covariance matrix (the uncertainty).
• Noise Modeling: Process noise (Q) and measurement noise (R) are modeled as zero-mean Gaussian white noise with known covariances.
• Uncertainty Propagation: The covariance matrix is explicitly propagated through the predict and update steps, providing a direct measure of estimation confidence.

The classic Kalman filter requires linear system dynamics and measurement models. These are expressed with matrices:

• State Transition Matrix (F): Describes how the state evolves from time k to k+1 without noise or control input (e.g., constant velocity model: x_{k+1} = x_k + v_k * dt).
• Control Input Matrix (B): (If applicable) relates control inputs to state changes.
• Measurement Matrix (H): Maps the true state to the expected measurement (e.g., a sensor that reads position directly would have H = [1, 0] for a state vector [position, velocity]).

For nonlinear systems, the Extended Kalman Filter (EKF) linearizes these models around the current estimate.

The Kalman gain (K) is the mathematical engine of sensor fusion. It dynamically determines how much to trust the new measurement versus the model's prediction.

• High Measurement Noise (large R): Kalman gain is small. The filter trusts the model prediction more.
• Low Measurement Noise (small R): Kalman gain is large. The filter trusts the new sensor reading more.
• Optimal Weighting: K is calculated to minimize the final error covariance: K = (Predicted Covariance * Hᵀ) * (H * Predicted Covariance * Hᵀ + R)⁻¹.

This allows seamless fusion of heterogeneous sensors (e.g., GPS, IMU, wheel encoders) with different noise characteristics and update rates.

The core principles of the Kalman filter have been extended to handle more complex, real-world scenarios, creating a family of related algorithms.

• Extended Kalman Filter (EKF): Handles nonlinear systems by linearizing the dynamics and measurement models around the current estimate using a Jacobian.
• Unscented Kalman Filter (UKF): Uses a deterministic sampling technique (the unscented transform) to propagate uncertainty through nonlinear functions, often more accurate and stable than the EKF.
• Ensemble Kalman Filter (EnKF): Uses a Monte Carlo approach (an ensemble of state vectors) to approximate the covariance, making it scalable for very high-dimensional systems like weather forecasting.
• Information Filter: An algebraically equivalent formulation that propagates the inverse of the covariance matrix, useful in some multi-sensor fusion architectures.

The Extended Kalman Filter (EKF) is the nonlinear extension of the standard Kalman filter. It linearizes the system's nonlinear dynamics and measurement models around the current state estimate using a first-order Taylor expansion (the Jacobian matrix). This allows it to handle systems where the relationships between states and measurements are not strictly linear.

• Core Mechanism: Propagates uncertainty through linearized models.
• Primary Limitation: Can diverge if the system is highly nonlinear or the initial estimate is poor, as the linearization error becomes significant.
• Common Use Case: Sensor fusion for robot localization using GPS, IMU, and wheel odometry, where motion and sensor models are nonlinear.

The Unscented Kalman Filter (UKF) 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: Selects a minimal set of sigma points around the mean, propagates these points through the true nonlinear functions, and then recomputes the mean and covariance of the transformed points.
• Key Advantage: Often provides more accurate estimates than the EKF for strong nonlinearities, as it captures the posterior mean and covariance accurately to the third order (Taylor series expansion).
• Typical Application: Attitude estimation for spacecraft or aircraft, where trigonometric functions in orientation dynamics are highly nonlinear.

A Particle Filter is a sequential Monte Carlo method used for state estimation in nonlinear, 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. Particles are propagated through the system model, then weighted by their likelihood given new measurements. Low-weight particles are discarded.
• Key Strength: Can represent arbitrary, multi-modal distributions (e.g., tracking an object that could be in one of several distinct locations).
• Computational Cost: Significantly higher than Kalman filters, scaling with the number of particles. Used when Gaussian assumptions fail, such as in global robot localization (the "kidnapped robot" problem).

Sensor Fusion is the overarching process of combining sensory data from disparate sources (e.g., LiDAR, camera, IMU, GPS) to produce estimates that are more accurate, complete, and reliable than those provided by any single sensor. Kalman filters are a primary algorithmic tool for this task.

• Key Principles: Complementarity (sensors cover different weaknesses) and Redundancy (multiple sensors provide the same data for robustness).
• Architectures:
  • Centralized: All raw data feeds into a single estimator (e.g., one Kalman filter).
  • Decentralized: Local estimators process sensor data separately, with fused results combined later.
• Robotics Example: An autonomous vehicle fusing camera-based lane detection (high accuracy, low reliability in fog) with LiDAR-based road boundary detection (works in fog, lower accuracy) and IMU data for ego-motion.

The Linear Quadratic Estimator (LQE), or Kalman Filter in its fundamental form, is the optimal state observer for a linear dynamic system with additive Gaussian white noise. Its design is the dual of the Linear Quadratic Regulator (LQR) control problem.

• Optimality Criterion: Minimizes the mean-squared estimation error.
• Mathematical Foundation: Solves a Riccati equation to compute the steady-state Kalman gain, which optimally balances trust between the system model prediction and the incoming sensor measurements.
• Separation Principle: In Linear Quadratic Gaussian (LQG) control, the optimal controller is designed separately (using LQR) from the optimal estimator (using LQE), and their combination remains optimal.

Simultaneous Localization and Mapping (SLAM) is the computational challenge where a mobile robot builds a map of an unknown environment while simultaneously estimating its own position within that map. Kalman filter variants (like EKF-SLAM) are foundational historical approaches.

• Core Challenge: The data association problem—correctly matching new sensor observations to existing map features—is critical. Errors here can cause filter divergence.
• EKF-SLAM: Represents the robot pose and all map feature locations in a single, large state vector. The covariance matrix captures correlations between the robot and every landmark.
• Modern Evolution: While EKF-SLAM is conceptually clear, scalability is an issue. Modern systems often use graph-based optimization (e.g., pose-graph SLAM) or particle filter approaches (FastSLAM) for large-scale environments.