State estimation with Kalman filters

A robot never knows its true state. Sensors are noisy, motion is uncertain, and no single measurement is trustworthy on its own. The Kalman filter is the principled way to fuse a motion model with noisy measurements into a best estimate, along with how confident you should be in it.

Predict and update

The filter cycles between two steps. Predict rolls the state forward with a motion model and grows the uncertainty. Update folds in a new measurement, shrinking the uncertainty in proportion to how reliable that measurement is. The Kalman gain is the knob that balances the two: trust the prediction or trust the measurement.

For a linear system with Gaussian noise, this is provably the optimal estimator. The state is carried as a mean and a covariance, and both are propagated every step.

The extended Kalman filter

Real robots are not linear, a drone's orientation and dynamics are anything but. The extended Kalman filter handles this by linearizing the motion and measurement models around the current estimate using their Jacobians, then applying the standard equations. It is an approximation, so it can diverge if the system is highly nonlinear or the initial guess is poor.

Sensor fusion

The filter shines when sensors are complementary. An IMU updates fast but drifts over time; vision or GPS is slower but absolute. Fusing them in an EKF gives a pose estimate that is both smooth and drift-free, which is exactly what you need for navigation when GPS drops out.

Tuning and failure modes

• Process noise (Q) says how much to trust the motion model; too small and the filter ignores reality.
• Measurement noise (R) says how much to trust each sensor; mis-set it and the estimate gets jumpy or sluggish.
• Divergence is the classic failure: bad linearization or a wrong model and the covariance stops reflecting the true error.

In short, the filter is always weighing "what I expected" against "what I just measured," and the covariance is its running estimate of how wrong it might be.

Step by step

1. Hold the state as a mean x and a covariance P.
2. Predict: push the state through the motion model F and grow uncertainty by Q.
3. Update: compare the measurement z to the prediction (the innovation).
4. Compute the Kalman gain K from the innovation covariance S.
5. Correct the state by K times the innovation and shrink P.
6. For nonlinear systems, the EKF replaces F and H with their Jacobians.

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R, x0, P0):
        self.F, self.H, self.Q, self.R = F, H, Q, R   # models + noise
        self.x, self.P = x0, P0                        # state mean + covariance

    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x

    def update(self, z):
        y = z - self.H @ self.x                         # innovation
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)        # Kalman gain
        self.x = self.x + K @ y
        self.P = (np.eye(len(self.P)) - K @ self.H) @ self.P
        return self.x

References

• Kalman, A New Approach to Linear Filtering and Prediction Problems (1960).
• Thrun, Burgard & Fox, Probabilistic Robotics (MIT Press, 2005).