Kalman Filter and State Estimation

Core Idea

The Kalman filter estimates the hidden state of a dynamic system (e.g., robot position and velocity) given noisy measurements and a model of how the system evolves. It is optimal for linear systems with Gaussian noise, minimizing mean-squared error. The algorithm alternates between prediction (using the motion model) and measurement update (using sensor readings). The filter maintains a Gaussian belief: state estimate plus covariance matrix representing uncertainty. As measurements are incorporated, uncertainty shrinks. The Extended Kalman Filter (EKF) handles nonlinear systems by linearizing around the current state. The Unscented Kalman Filter (UKF) avoids linearization by sampling the Gaussian. Variants enable sensor fusion (combining multiple noisy sensors) and SLAM (simultaneous localization and mapping).

How It's Best Learned

Implement a scalar Kalman filter by hand: estimate a single quantity (e.g., temperature) from noisy measurements. Compute prediction covariance, measurement update, and observation residuals. Verify that uncertainty decreases as you incorporate measurements. Extend to 2D position estimation: predict motion using a constant-velocity model, update using GPS measurements. Observe how the filter balances model predictions (which drift over time) with measurements (which are noisy). Code an EKF for a nonlinear system (e.g., robot with nonlinear odometry). Compare with UKF. Experiment with tuning process and measurement noise covariances.

Common Misconceptions

• The Kalman filter smooths measurement noise away; it optimally trades model predictions against noisy measurements, but it cannot create information not present in either source.
• Higher measurement frequency always improves estimates; if measurements are very noisy, lower frequency with more averaging is better. The filter's Kalman gain balances these.
• The Kalman filter is guaranteed to produce correct estimates; it is optimal only if the model is accurate and noise assumptions (Gaussian, white) are satisfied. Model errors cause divergence.
• Extended Kalman Filter linearization is always accurate; for highly nonlinear systems, the EKF linearization can diverge, leading to inconsistent estimates.

Explainer

A robot moving through the environment needs to know its location. Sensors provide noisy measurements: GPS has meter-level error, wheel odometry drifts over time, IMU accelerometers are biased. The challenge: fuse these imperfect measurements into a good estimate of the true state (position, velocity, orientation). The Kalman filter is the optimal solution for linear systems with Gaussian noise.

The filter maintains a belief: a Gaussian distribution over the state, characterized by a mean estimate and a covariance matrix representing uncertainty. It operates in two phases:

Prediction Phase: Using the motion model, predict the state at the next time step:

• x_pred = f(x_est, u), where u is the control input
• P_pred = F·P·F^T + Q, where F is the Jacobian of f, and Q is the process noise covariance

The motion model predicts where the robot should be. The covariance increases (uncertainty grows) because the model is imperfect (Q accounts for this unmodeled error).

Update (Measurement) Phase: When a measurement z arrives, correct the estimate:

• residual: y = z - H·x_pred, where H maps state to measurement space
• Kalman gain: K = P_pred·H^T / (H·P_pred·H^T + R), where R is measurement noise covariance
• update: x_est = x_pred + K·y
• update covariance: P = (I - K·H)·P_pred

The Kalman gain K is the key: it balances the prediction and the measurement. If the prediction is very certain (P is small), K is small and the measurement is largely ignored. If the prediction is uncertain (P is large), K is large and the measurement has strong influence. Similarly, if the measurement is very noisy (R is large), K is small; if the measurement is clean (R is small), K is large.

Optimal property: Under linear system dynamics and Gaussian noise, the Kalman filter minimizes the mean-squared error of the estimate. This is a remarkable result: the filter automatically weights predictions and measurements optimally given their respective uncertainties.

Extended Kalman Filter (EKF) extends this to nonlinear systems by linearizing the motion and measurement models around the current estimate using Jacobians. The linearization introduces approximation error, which can cause divergence for highly nonlinear systems. The EKF is widely used in robotics because many systems (robot kinematics, sensor models) are only mildly nonlinear, making linear approximations reasonable.

Unscented Kalman Filter (UKF) avoids explicit linearization by using a clever sampling strategy: the "unscented transform" generates sigma points whose statistics match the current Gaussian belief, propagates them through the true nonlinear model, and recomputes the Gaussian from the transformed points. This often provides better accuracy than EKF for strongly nonlinear systems without requiring Jacobians.

Practical considerations:

• Tuning: Q and R must be chosen appropriately. Too low Q causes the filter to trust inaccurate predictions; too high Q causes divergence. Too low R causes sensor noise to dominate; too high R ignores good measurements. Tuning often requires empirical testing or adaptive methods.
• Initialization: The initial state estimate and covariance must be reasonable. A poor initial state can cause slow convergence or divergence.
• Sensor dropout: If a sensor fails, the Kalman filter gracefully degrades: it relies more on predictions until the sensor recovers.
• Non-Gaussian noise: Real measurements often have non-Gaussian tails (outliers). Robust estimation (outlier rejection, huber loss) is sometimes applied before feeding data to the filter.

Applications: GPS-denied navigation (using IMU and odometry), SLAM (fusing odometry and landmarks), multi-sensor fusion in autonomous vehicles, target tracking, and any scenario requiring state estimation from noisy measurements. The Kalman filter is fundamental to modern robotics and control systems.