Kalman Filter

The Kalman filter provides the optimal (minimum mean-square error) estimate of a system's state under two strict conditions: the system dynamics must be linear, and all process and measurement noises must be Gaussian (normally distributed). This means no other algorithm can produce a more accurate estimate on average given the same model and data. In practice, many real-world systems are approximately linear and Gaussian over short intervals, making the Kalman filter exceptionally effective.

• Optimality Proof: Derived from Bayesian inference and orthogonal projection theory.
• Practical Implication: For sensor fusion in robotics (e.g., fusing IMU and GPS), it is the benchmark against which other filters are compared.

The algorithm operates recursively, requiring only the previous state estimate and the new measurement to compute the current estimate. It does not need to store or reprocess the entire history of measurements. This is implemented in a two-step predict-update cycle:

• Predict Step: Projects the previous state forward in time using the system model (the state transition matrix). It also projects the uncertainty forward (the covariance prediction).
• Update Step: Incorporates the new sensor measurement by computing the Kalman Gain, which optimally weights the prediction against the new data based on their respective uncertainties.

This recursive nature makes it computationally efficient and suitable for real-time applications on embedded systems.

A defining feature is its maintenance of a covariance matrix (P) that explicitly represents the estimated uncertainty (error) in the state. This is not a single number but a full matrix capturing uncertainties and correlations between all state variables (e.g., between position and velocity).

• Predicted Covariance (Pₖ₋): Grows during the predict step, reflecting increased uncertainty due to process noise and imperfect model.
• Updated Covariance (Pₖ): Shrinks during the update step when a measurement reduces overall uncertainty.
• Kalman Gain Calculation: This matrix is directly used to compute the Kalman Gain (K), which determines how much to trust the new measurement versus the model prediction.

The Kalman Gain (K) is the engine of the update step. It is a matrix that optimally balances the confidence in the model's prediction against the confidence in the incoming sensor measurement.

• High Measurement Noise: If the sensor is very noisy (large measurement covariance R), the Kalman Gain is small. The filter trusts the model prediction more and only makes a small adjustment based on the measurement.
• Low Measurement Noise: If the sensor is very accurate (small R), the Kalman Gain is large. The filter trusts the new measurement more and corrects the prediction significantly.
• Mathematically: Kₖ = Pₖ₋ Hᵀ (H Pₖ₋ Hᵀ + R)⁻¹, where H is the measurement matrix. It minimizes the posterior error covariance.

The filter's framework naturally extends to multisensor fusion. Measurements from different sensors (e.g., IMU, camera, LiDAR) can be incorporated sequentially or simultaneously within the update step, provided each has a defined measurement model (H matrix) and noise characteristic (R matrix).

• Sequential Update: Process one sensor measurement at a time within the same timestep, updating the state and covariance after each. This is computationally simple.
• Batch Update: Combine all sensor measurements into a single vector and update once. This is mathematically equivalent but requires matrix operations on larger arrays.
• Asynchronous Sensors: The filter can handle measurements arriving at different, irregular rates by applying the update step only when a measurement is available, while the predict step runs at a fixed clock.

For non-linear systems (which are the norm in robotics), the core Kalman filter properties are preserved through approximations:

• Extended Kalman Filter (EKF): Linearizes the non-linear system and measurement models around the current state estimate using a first-order Taylor series expansion (the Jacobian). It propagates the Gaussian uncertainty through this linear approximation.
• Unscented Kalman Filter (UKF): Uses a deterministic sampling technique (the unscented transform). It selects a minimal set of sample points (sigma points) around the mean, propagates them through the true non-linear functions, and then recomputes the mean and covariance. The UKF often handles strong non-linearities more accurately than the EKF without requiring Jacobian calculations.

Both maintain the recursive predict-update structure and explicit uncertainty tracking of the original Kalman filter.

Sensor fusion is the computational process of combining data from multiple disparate sensors (e.g., IMU, camera, LiDAR) to produce a more accurate, complete, and reliable estimate of a system's state than any single sensor could provide. A Kalman filter is a foundational algorithm for probabilistic sensor fusion.

• Types: Includes complementary, competitive, and cooperative fusion.
• Goal: Overcome individual sensor limitations like noise, drift, and occlusion.
• Example: Fusing a GPS (accurate but slow) with an IMU (fast but drifting) for smooth, high-frequency pose estimation.

The Extended Kalman Filter (EKF) is the non-linear version of the standard Kalman filter. It linearizes the system's non-linear process and measurement models around the current state estimate using a first-order Taylor expansion (the Jacobian).

• Application: Essential for robotics where motion and sensor models are inherently non-linear (e.g., wheeled robot kinematics, camera projection).
• Limitation: Can diverge with highly non-linear systems or poor initial estimates due to linearization errors.
• Core Step: Computes Jacobian matrices at each timestep to approximate the covariance propagation.

The Unscented Kalman Filter (UKF) is a more robust 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 sigma points through the true non-linear functions.

• Advantage: Often more accurate and stable than the EKF for strong non-linearities, as it captures the posterior mean and covariance to the 3rd order.
• Process: Selects a minimal set of sample points that capture the true mean and covariance of the state distribution.
• Use Case: Preferred for systems with severe non-linearities in both process and measurement models.

A PID (Proportional-Integral-Derivative) controller is a ubiquitous feedback control loop mechanism. While a Kalman filter estimates state, a PID controller acts on state error to drive a system to a desired setpoint.

• Components: Proportional term for present error, Integral for accumulated past error, Derivative for predicted future error.
• Synergy: A Kalman filter's clean state estimate (e.g., velocity) is often the perfect input to a PID controller's derivative term, replacing noisy direct differentiation.
• Application: Found in everything from motor speed control to drone attitude stabilization.

An Inertial Measurement Unit (IMU) is a key sensor fused in Kalman filters. It typically contains a 3-axis accelerometer and a 3-axis gyroscope, providing high-frequency proprioceptive data on linear acceleration and angular velocity.

• Role in Fusion: Provides fast, short-term motion data but suffers from integration drift. A Kalman filter fuses IMU data with absolute but slower sensors (like GPS or vision) to correct this drift.
• Simulation: IMU simulation models include injecting realistic Gaussian noise, bias instability, and scale factor errors to test filter robustness.
• Output: Raw IMU data is integrated to estimate pose, a process where error accumulates rapidly without corrective fusion.

These are the core statistical models that define a Kalman filter's behavior. Process noise (Q) models uncertainty in the system's motion model. Measurement noise (R) models uncertainty in sensor observations.

• Process Noise Covariance (Q): Represents unmodeled dynamics, disturbances, and actuator imperfections. A higher Q makes the filter trust new measurements more.
• Measurement Noise Covariance (R): Represents sensor inaccuracy and noise. A higher R makes the filter trust its internal prediction more.
• Tuning: The performance of a Kalman filter is critically dependent on accurately modeling or empirically tuning these covariance matrices.