State Estimation and Filtering

Every sensor lies, a little. An accelerometer measuring a robot's forward motion returns not the true acceleration but the true acceleration plus noise. An encoder counting wheel rotations accumulates small errors that grow with distance. A GPS receiver places the robot within a circle of uncertainty that expands in urban canyons. No single measurement tells the robot exactly where it is or what it is doing. Yet robots act. They hold trajectories, track targets, balance on two wheels, and land on moving platforms. The mechanism that makes this possible is state estimation: the principled combination of uncertain measurements over time to maintain a best estimate of the robot's state.

The State and the Estimation Problem

The state of a robot is a vector that completely describes the robot's current relevant condition for the purpose of control and planning. A wheeled robot's state might be (x, y, theta) — position and heading. A quadrotor's state might be (x, y, z, roll, pitch, yaw, x_dot, y_dot, z_dot, roll_rate, pitch_rate, yaw_rate) — position, orientation, and their derivatives. A robot arm's state might be the angles and angular velocities of all joints. The state is never directly observable. Sensors measure projections of the state, corrupted by noise. A wheel encoder measures (approximately) the angle each wheel has turned — but not the robot's absolute position. A camera gives pixel intensities — not object distances. An IMU gives accelerations and rotation rates — not absolute velocity or position. State estimation is the problem of inferring the hidden state from a sequence of noisy, indirect measurements. It is fundamentally a Bayesian reasoning problem: maintain a probability distribution over the state, update it with each new measurement, and propagate it forward as the robot moves.

The Bayesian filter is the theoretical foundation. It has two alternating steps. Prediction step: given the current belief and the control command just executed, predict the next state distribution using a motion model. If the robot commanded 'move forward 1 meter,' the motion model predicts how the state distribution shifts. Because motion has uncertainty (wheel slip, surface irregularity, timing error), the prediction spreads the distribution — uncertainty grows. Update step: incorporate the latest sensor measurement to sharpen the estimate. The measurement model describes the probability of observing a given measurement given a hypothesized state. Bayes' rule combines the predicted distribution with this measurement likelihood to produce the posterior: the updated belief. If the measurement is highly informative, it sharply narrows the distribution; if it is noisy, it narrows it less. Repeat: move (predict, uncertainty grows) → observe (update, uncertainty shrinks) → move → observe. The filter constantly oscillates between expanding and contracting uncertainty, maintaining a running estimate that is better than any single measurement or any dead-reckoned prediction alone.

The Kalman filter (Kalman, 1960) is the optimal Bayesian filter for the special case where the motion model is linear, the measurement model is linear, and both the process noise and measurement noise are Gaussian. Under these conditions, the belief is always a Gaussian distribution, fully characterized by its mean (the best estimate) and covariance (the uncertainty). The Kalman filter maintains these two quantities. The predict step advances the mean through the linear motion model and grows the covariance by adding the process noise covariance. The update step computes the Kalman gain — a matrix that balances how much to trust the prediction versus the measurement — then updates the mean and shrinks the covariance. Kalman gain intuition: if the measurement noise is very small (the sensor is accurate), the Kalman gain is large — the update step trusts the measurement and moves the estimate strongly toward it. If the measurement noise is large (the sensor is noisy), the gain is small — the estimate barely moves. The filter automatically and optimally weights sensors by their known reliability. For nonlinear systems (which describes most real robots), the Extended Kalman Filter (EKF) linearizes the motion and measurement models around the current estimate using Jacobians. The Unscented Kalman Filter (UKF) uses a deterministic set of sigma points to capture the distribution's statistics without requiring explicit linearization. Particle filters handle arbitrarily nonlinear, non-Gaussian problems by representing the distribution as a set of weighted samples.