A Kalman filter is a recursive state estimation algorithm that combines a mathematical model of how a system moves with noisy sensor measurements to produce the best estimate of the system’s current state and its uncertainty. In robotics control systems, the Kalman filter supplies the position, velocity, orientation or other state variables that feedback controllers need but cannot measure directly or cleanly.
For a linear system with Gaussian noise, the Kalman filter is provably optimal: no other estimator achieves a lower mean squared error. Robots are rarely perfectly linear, so practical systems often use its nonlinear extensions, but the core idea stays the same: predict with the model, then correct with the measurement, weighting each by how much it can be trusted.
How the Kalman filter predicts and corrects
The Kalman filter runs a two-step cycle at every time step. In the prediction step, it uses a process model and the known control input to project the previous state estimate forward in time, and it increases the estimate’s uncertainty because the model is imperfect. In the update step, it compares a new measurement with the predicted measurement and shifts the estimate toward the observation.
The size of that shift is set by the Kalman gain. When the sensor is precise relative to the prediction, the gain is high and the estimate follows the measurement closely; when the sensor is noisy, the gain is low and the filter relies mostly on the model. The filter tracks this balance through the covariance matrix, which describes the uncertainty of every state variable and the correlations between them.
| Symbol | Name | Role |
|---|---|---|
| x | State vector | Quantities being estimated, such as position and velocity |
| P | State covariance | Uncertainty of the current estimate |
| F | State transition model | How the state evolves between steps |
| B and u | Control model and input | Effect of commanded motion |
| H | Observation model | How the state maps to sensor readings |
| Q | Process noise covariance | Trust placed in the motion model |
| R | Measurement noise covariance | Trust placed in each sensor |
| K | Kalman gain | Weight given to the new measurement |
Variants for nonlinear robot systems
Robot motion and many sensors are nonlinear: a wheeled robot’s position depends on the sine and cosine of its heading, and a range-bearing sensor measures distances and angles rather than coordinates. Several variants extend the Kalman filter to these cases, trading accuracy against computation.
| Variant | How it handles nonlinearity | Typical use | Main limitation |
|---|---|---|---|
| Linear Kalman filter | Assumes linear models | Constant-velocity tracking, simple axes | Fails on strong nonlinearity |
| Extended Kalman filter (EKF) | Linearizes with Jacobians at each step | Odometry and IMU fusion, GNSS fusion | Can diverge if linearization is poor |
| Error-state Kalman filter | Estimates small errors around a nominal state | Inertial navigation, attitude estimation | More complex to derive |
| Unscented Kalman filter (UKF) | Propagates sigma points through the true model | Strongly nonlinear dynamics | Higher computational cost |
| Particle filter | Represents the distribution with samples | Multimodal problems such as global localization | Not a Kalman filter, costly in high dimensions |
In practice, the EKF remains the workhorse of mobile robot state estimation. Widely used ROS 2 packages, such as robot_localization, fuse wheel odometry, inertial measurement units and GNSS with EKF or UKF nodes.
The Kalman filter within feedback control
In a feedback control loop, the Kalman filter sits between the sensors and the controller. The controller, whether a PID loop, an LQR or a model predictive controller, compares the estimated state with the setpoint and computes the actuator command. That command is then fed back into the filter’s prediction step as the control input.
Combining a Kalman filter with a linear quadratic regulator gives the linear quadratic Gaussian (LQG) controller. Under linear-Gaussian assumptions, the separation principle states that the estimator and the controller can be designed independently and still produce the optimal combined result, although LQG designs can have poor robustness margins and need verification.
| Estimator | Uses a model | Adapts weighting | Typical robotics use |
|---|---|---|---|
| Moving average | No | No | Smoothing slow sensor readings |
| Complementary filter | Implicitly, via frequency split | No, fixed blend | Low-cost attitude from gyro and accelerometer |
| Fixed-gain observer | Yes | No | Embedded estimation with known dynamics |
| Kalman filter | Yes | Yes, through covariance | Multi-sensor state estimation |
Tuning a Kalman filter
The filter is only as good as its noise covariances Q and R. The measurement noise R can often be estimated from sensor datasheets or by recording a stationary sensor, while the process noise Q captures unmodeled effects, such as wheel slip or unknown accelerations, and usually has to be tuned empirically.
- Q too small: the filter trusts the model too much, responds sluggishly and may ignore real changes.
- Q too large: the estimate follows every noisy measurement and loses its smoothing benefit.
- R too small: sensor noise passes directly into the estimate and into the controller.
- Check the innovation: the difference between actual and predicted measurements should look like zero-mean noise with the predicted variance.
Normalized innovation squared tests offer a statistical consistency check. If innovations are consistently larger than the covariance predicts, the filter is overconfident and Q or R should be increased.
Common pitfalls in robot applications
Most Kalman filter failures in robotics come from modeling and bookkeeping errors rather than from the algorithm itself.
- Wrong time stamps: fusing measurements with delayed or misaligned time stamps introduces systematic errors.
- Unit and frame mix-ups: radians versus degrees, or sensor frames versus the body frame, corrupt the update silently.
- Angle wrapping: heading innovations must be wrapped to the range of minus pi to pi, otherwise a small turn looks like a full rotation.
- Outliers: a single bad GNSS fix can pull the estimate far away, so gating rejects measurements with improbably large innovations.
- Covariance losing symmetry: numerical errors can make P invalid, which the Joseph form of the update helps prevent.
A non-technical view
A robot never knows exactly where it is. Its wheels slip, and its sensors give slightly different readings every time. A Kalman filter combines what the robot expects with what its sensors report, and it trusts whichever source is more reliable at that moment.
It resembles walking through a dark room: you count your steps to guess your position, and when you touch a familiar table you correct that guess. The result is a steadier, more accurate sense of position than either method alone.
A concrete example
An outdoor delivery robot estimates its position from wheel odometry at 50 Hz, an inertial measurement unit at 100 Hz and a GNSS receiver at 5 Hz. Odometry is smooth but drifts by several percent of distance traveled, and GNSS is drift-free but jumps by a meter or more near buildings.
The engineering team runs an extended Kalman filter that predicts motion from odometry and the IMU and corrects it with GNSS fixes, rejecting fixes whose innovation exceeds a gating threshold. After tuning Q and R on recorded drives, the fused position stays within the sidewalk width along the whole route, and the path-following controller no longer reacts to GNSS jumps with sudden steering corrections.