Kalman Filter

The Kalman filter is a recursive algorithm, meaning it processes measurements sequentially and updates its estimate as new data arrives. It does not require storing the entire history of measurements, only the current state estimate and its error covariance. This makes it extremely memory-efficient and suitable for real-time systems where data streams continuously, such as in visual-inertial odometry for AR headsets.

• Process: For each new measurement, it performs a predict step (based on the system model) and an update step (fusing the prediction with the measurement).
• Efficiency: Computational complexity is O(n³) in the state dimension, but for many problems (like tracking position and velocity), n is small, enabling microsecond-level updates.

At its core, the Kalman filter is an optimal estimator under the assumptions of linear dynamics and Gaussian noise. It fuses a predicted state (from a motion model) with a noisy measurement by computing the Kalman Gain. This gain is a weighting factor that determines how much to trust the new measurement versus the prior prediction.

• Kalman Gain Calculation: K = (Prediction Uncertainty) / (Prediction Uncertainty + Measurement Uncertainty). A high measurement uncertainty results in a low gain, trusting the prediction more.
• Result: The fused estimate has a lower mean squared error than any estimate based solely on the prediction or the measurement alone. This is critical for smoothing jittery sensor data, like fusing a camera's drift-prone pose with an IMU's high-frequency but drifting accelerometer data.

The filter operates within a rigorous probabilistic framework. All quantities—the state, predictions, and measurements—are treated as Gaussian distributions (defined by a mean and a covariance matrix), not as single points. This allows the filter to explicitly model and propagate uncertainty.

• State Representation: x ~ N(μ, P), where μ is the state mean (e.g., position, velocity) and P is the covariance matrix representing estimation uncertainty.
• Model Components: It requires two precisely defined models:
  • Process Model: Describes how the state evolves over time (e.g., x_{k} = F * x_{k-1} + w, where w is process noise).
  • Measurement Model: Describes how measurements relate to the state (e.g., z_{k} = H * x_{k} + v, where v is measurement noise).

The algorithm executes in a strict, two-phase cycle that is repeated for each new timestep. This cycle is the engine of all Kalman filter variants.

1. Predict Step (Time Update):

• Projects the current state and its uncertainty forward in time using the process model.
• Predicted State = F * Previous State
• Predicted Covariance = F * Previous Covariance * Fᵀ + Q (where Q is process noise covariance).

2. Update Step (Measurement Update):

• Incorporates the new sensor measurement to correct the prediction.
• Computes the Kalman Gain K.
• Fuses prediction and measurement: Updated State = Predicted State + K * (Measurement - H*Predicted State).
• Reduces the state uncertainty: Updated Covariance = (I - K*H) * Predicted Covariance.

This cycle enables real-time tracking where the state is constantly refined.

The filter's performance is defined by its explicit modeling of noise covariance matrices. Tuning these matrices is essential for real-world application.

• Process Noise Covariance (Q): Models the uncertainty in the system's motion model. A larger Q tells the filter that the model is unreliable, making it trust measurements more. In SLAM, this accounts for unmodeled dynamics.
• Measurement Noise Covariance (R): Models the uncertainty of the sensors. A larger R tells the filter that the sensor is noisy, making it trust the internal prediction more. For a camera, R might increase in low-light conditions.
• Adaptive Filtering: Advanced variants can estimate Q and R online, allowing the filter to adapt to changing sensor reliability or dynamic conditions.

The classic Kalman filter assumes linear models. Most real-world systems in robotics and AR (like camera rotation) are non-linear. Two primary extensions address this:

• Extended Kalman Filter (EKF): The workhorse of non-linear estimation. It linearizes the non-linear process and measurement models around the current state estimate using a first-order Taylor expansion (the Jacobian matrix). It then applies the standard Kalman equations to this linear approximation. Used extensively in visual-inertial odometry.
• Unscented Kalman Filter (UKF): Uses a deterministic sampling technique called the Unscented Transform. It propagates a carefully chosen set of "sigma points" through the true non-linear functions and then computes the mean and covariance of the transformed points. Often more accurate and stable than the EKF for highly non-linear systems, though computationally heavier.

Both maintain the recursive, two-step structure of the original filter.

Sensor fusion is the overarching process of combining data from multiple, often heterogeneous, sensors to produce a unified state estimate that is more accurate and reliable than any single source. The Kalman filter is a foundational algorithm for this task.

• Core Principle: It statistically weights data from different sensors (e.g., camera, IMU, LiDAR) based on their estimated uncertainty.
• Example: In Visual Inertial Odometry (VIO), a Kalman filter fuses high-frequency, drift-prone IMU data with lower-frequency, absolute but sometimes lost visual data from a camera.

The Extended Kalman Filter (EKF) is the non-linear version of the standard Kalman filter, which is designed for linear systems. It linearizes the system's non-linear prediction and measurement models around the current state estimate using a Jacobian matrix.

• Use Case: Essential in robotics and SLAM, where motion models and camera projection are inherently non-linear.
• Limitation: Linearization errors can accumulate, potentially causing divergence if the initial estimate is poor or the system is highly non-linear.

The Unscented Kalman Filter (UKF) is an alternative to the EKF for non-linear systems. Instead of linearizing, it uses a deterministic sampling technique (the unscented transform) to propagate a set of carefully chosen sample points through the non-linear functions.

• Advantage: Often provides more accurate estimates than the EKF for strong non-linearities, as it better approximates the true mean and covariance.
• Computational Cost: Typically similar to the EKF, making it a popular choice for modern sensor fusion pipelines.

A Particle Filter is a sequential Monte Carlo method used for state estimation in non-linear, non-Gaussian systems. It represents the probability distribution of the state using a set of random samples (particles) with associated weights.

• Comparison to Kalman: Unlike the Kalman filter, which assumes Gaussian noise, particle filters can model arbitrary distributions, making them powerful for multi-modal situations (e.g., global robot localization).
• Drawback: Computationally expensive, as accuracy requires a large number of particles, making it less common for high-frequency, on-device pose estimation compared to EKF/UKF.

Visual Inertial Odometry (VIO) is a specific, high-performance application of sensor fusion that combines visual data from a camera with inertial data from an IMU to track a device's 6-degree-of-freedom pose. Kalman filters (EKF) or optimization-based methods are its core.

• Role of Kalman Filter: It provides a recursive framework to predict motion via the IMU and correct drift using visual feature measurements.
• Key Benefit: Enables robust tracking during rapid motion or visual texture loss, which is critical for mobile AR and robotics.

The covariance matrix is the fundamental mathematical object within the Kalman filter that quantifies the estimated uncertainty of the state variables and the correlations between them.

• Prediction Step: The covariance matrix grows, reflecting increased uncertainty after motion.
• Update Step: The covariance matrix shrinks when a measurement is incorporated, reflecting increased confidence in the state estimate.
• Interpretation: Its diagonal elements are the variances (squared uncertainty) of each state variable (e.g., position, velocity).