Kalman Filtering

The Kalman filter operates through a continuous two-phase loop. The predict step projects the current state estimate and its uncertainty forward in time using a mathematical model of the system's dynamics. The update step then corrects this prediction by incorporating a new noisy sensor measurement, weighted by the Kalman gain. This gain optimally balances trust between the model prediction and the measurement based on their relative uncertainties. Because it is recursive, the filter only needs the previous state estimate and the new measurement—not the entire history—making it exceptionally memory-efficient.

The standard Kalman filter is provably the minimum mean squared error (MMSE) estimator for linear systems with additive white Gaussian noise. This means no other algorithm can produce a state estimate with a lower expected error under these conditions. The filter assumes that both process noise (unmodeled dynamics, disturbances) and measurement noise (sensor imprecision) follow zero-mean Gaussian distributions. When these assumptions hold, the filter's estimated covariance matrix faithfully represents the true uncertainty of the state estimate, a property called consistency.

The filter maintains two core data structures. The state vector contains the variables being estimated—for a tracked vehicle, this might include position, velocity, and acceleration in three dimensions. The covariance matrix quantifies the uncertainty in each state variable and the correlations between them. A key insight is that the filter tracks not just the best guess but also how confident it is in that guess. The covariance matrix shrinks when measurements confirm predictions and grows during prediction steps when process noise adds uncertainty.

The classical Kalman filter requires the system to be described by linear state transition and measurement models. The state transition matrix defines how the state evolves from one time step to the next without external input. The measurement matrix maps the true state to what the sensor actually observes. For systems with nonlinear dynamics—such as most real-world robotics and tracking problems—extensions like the Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) were developed to handle nonlinearity through linearization and sigma-point sampling, respectively.

The Kalman filter is inherently a sensor fusion algorithm. The measurement update step can naturally incorporate data from multiple heterogeneous sensors simultaneously. Each sensor contributes a measurement vector and its associated noise covariance. The filter automatically weights each sensor's contribution based on its precision:

• A high-precision LiDAR range measurement receives higher weight than a noisy ultrasonic reading
• An Inertial Measurement Unit (IMU) provides high-frequency predictions between slower absolute position fixes from GPS
• The fused estimate is statistically more accurate than any single sensor could provide independently

The Kalman filter's matrix operations scale polynomially with the state vector dimension, typically O(n³) for the covariance update where n is the number of state variables. For most practical applications with state vectors under 20 dimensions, this runs in microseconds on embedded processors. This efficiency made it the algorithm of choice for the Apollo guidance computer and continues to make it ideal for modern edge AI deployments in manufacturing where low-latency state estimation must run on resource-constrained industrial controllers without cloud connectivity.

The nonlinear extension of the standard Kalman filter that handles systems where state transitions or measurement models are not linear. The EKF linearizes the current mean and covariance estimates by computing Jacobian matrices of the nonlinear functions at each time step.

• Mechanism: First-order Taylor series approximation around the current estimate
• Limitation: Introduces errors in highly nonlinear systems; requires differentiable functions
• Use case: GPS/INS integration, robot localization with range-bearing sensors

A derivative-free alternative to the EKF that uses the unscented transform to propagate a minimal set of deterministically chosen sigma points through nonlinear functions. This captures the posterior mean and covariance accurately to the third order for Gaussian inputs.

• Advantage: No Jacobian computation required; superior accuracy for highly nonlinear systems
• Mechanism: 2n+1 sigma points sampled around the mean, propagated, then recombined
• Use case: Aircraft attitude estimation, ballistic target tracking

A sequential Monte Carlo method that represents the state probability distribution using a set of weighted random samples called particles. Unlike Kalman-based filters, particle filters can represent arbitrary non-Gaussian, multimodal distributions.

• Mechanism: Importance sampling with resampling to avoid particle degeneracy
• Trade-off: Computational cost scales with number of particles; handles any distribution shape
• Use case: Global localization (kidnapped robot problem), visual tracking with occlusions

A consistent fusion algorithm for combining state estimates when the cross-correlation between them is unknown or too expensive to track. It computes a weighted average that guarantees the fused covariance is never overconfident.

• Mechanism: Convex combination of inverse covariance matrices with an optimized weight parameter
• Critical property: Yields a consistent (not optimistic) estimate in decentralized networks
• Use case: Multi-robot SLAM, distributed sensor networks without a central node

A graphical model framework that represents state estimation as a bipartite graph of variable nodes (states) and factor nodes (probabilistic constraints). The maximum a posteriori estimate is found by solving a nonlinear least squares problem over the entire graph.

• Mechanism: Sparse bundle adjustment using incremental smoothing (iSAM2)
• Advantage: Handles loop closures and batch optimization naturally; outperforms filtering for SLAM
• Use case: Modern visual SLAM systems (ORB-SLAM), large-scale mapping

The computational process of determining which measurement originated from which object in multi-target tracking. Incorrect associations can catastrophically corrupt a Kalman filter's state estimate.

• Key algorithms: Nearest Neighbor, Joint Probabilistic Data Association (JPDA), Multiple Hypothesis Tracking (MHT)
• Challenge: Combinatorial explosion in cluttered environments
• Use case: Autonomous vehicle perception, air traffic control radar tracking