State estimation

Overview

No sensor measures a robot’s state directly. Encoders measure wheel rotation, not position. IMUs measure angular rate and specific force, not orientation. Cameras measure pixels. State estimation combines these measurements with a model of how the robot moves to produce the best current guess of position, velocity, orientation and whatever else the controller needs, together with how uncertain that guess is.

The uncertainty matters as much as the estimate. A planner that knows its position to 5 cm can pass through a doorway that one uncertain to 50 cm should not try. This hub is for engineers building or tuning a localisation or fusion stack, and for anyone whose robot’s pose estimate jumps, drifts or diverges.

Key ideas

The Bayes filter

Every method on this page approximates one recursion. The belief is a probability distribution over the state. In the prediction step, a motion model pushes the belief forward in time, and the uncertainty grows. In the update step, a measurement model scores how well each possible state explains the new measurement, and the belief sharpens. The recursion assumes the current state captures everything relevant from the past, the Markov assumption. When a filter misbehaves, the cause is usually that the models or their noise levels do not match the robot, not the algorithm.

The Kalman family

If the motion and measurement models are linear and the noise is Gaussian, the Bayes filter has an exact closed form: the Kalman filter, from Kalman’s 1960 paper. The belief is a mean and a covariance, and each step is a few matrix operations.

Real robots are nonlinear, because headings enter through sines and cosines. The extended Kalman filter (EKF) linearises the models around the current estimate using their Jacobians. It is cheap and works well when the estimate is close and the models are smooth. The unscented Kalman filter (UKF), described by Julier and Uhlmann (2004), propagates a small set of sigma points through the full nonlinear models instead. It needs no Jacobians and handles stronger nonlinearity at modest extra cost. For orientation, many practical filters estimate a small error state in the tangent space described in /topics/spatial-math/, which keeps quaternions normalised.

Tuning comes down to the process noise Q and the measurement noise R. A filter that is too confident ignores new measurements, and one that is too uncertain chases noise. Check consistency, not only accuracy: the innovations (measurement minus prediction) should look like zero-mean noise with the covariance the filter predicts.

Particle filters

A particle filter represents the belief by a set of sampled states, each with a weight. Predict moves every particle through the motion model with sampled noise. Update reweights them by the measurement likelihood, and resampling discards unlikely particles. It can represent several separate hypotheses at once, so it can localise a robot that starts with no idea where it is and recover when the robot is moved by hand. The cost grows quickly with the dimension of the state, which is why particle filters are common for 2D localisation in a known map and rare for full 6-DoF estimation. Nav2’s AMCL is a particle filter of this kind.

Sensor fusion and factor graphs

Fusion combines sensors with complementary errors: wheel odometry is smooth but drifts, an IMU is fast but drifts in position, GPS is absolute but noisy and intermittent. A filter fuses them one measurement at a time, and must handle measurements that arrive late and out of order.

Factor graphs take a different route. The unknowns (poses, landmarks, sensor biases) are variables. Each measurement becomes a factor that connects the variables it depends on. Instead of keeping only the latest state, the solver finds the assignment of all variables that best satisfies every factor, a nonlinear least-squares problem. Re-linearising past states makes this more accurate than an EKF, and the sparse structure keeps it fast. Incremental solvers such as iSAM2 update the solution as new factors arrive. Dellaert and Kaess (2017) is the standard introduction, and the same machinery underlies the SLAM back ends in /topics/slam-and-mapping/.

Where to start

/learn/beginner/06-sensors-and-noise/ builds the noise models a filter needs, and /learn/beginner/07-differential-drive/ gives you the motion model. Keep /reference/kalman-filter/ open while you implement one, then fuse odometry with landmark measurements in the project /projects/diff-drive-astar-kalman/.

Tools and platforms

Common mistakes

• Tuning Q and R until the plot looks good on one log. Check the innovations for consistency on a different log.
• Fusing two sensors that already share information, such as wheel odometry and an odometry topic computed from the same wheels, and counting it twice.
• Ignoring timestamps. A measurement applied at arrival time rather than measurement time biases the estimate when the robot moves.
• Treating angles as ordinary numbers. Wrap heading differences to the range from minus pi to pi.
• Publishing map to base_link directly instead of map to odom, which breaks the REP 105 tree.

Going deeper

• Sebastian Thrun, Wolfram Burgard and Dieter Fox, Probabilistic Robotics, MIT Press, 2005.
• Timothy D. Barfoot, State Estimation for Robotics, 2nd edition, Cambridge University Press, 2024. A draft PDF is on the author’s site.
• R. E. Kalman, A new approach to linear filtering and prediction problems, Journal of Basic Engineering 82(1), 1960.
• S. J. Julier and J. K. Uhlmann, Unscented filtering and nonlinear estimation, Proceedings of the IEEE 92(3), 2004.
• Frank Dellaert and Michael Kaess, Factor graphs for robot perception, Foundations and Trends in Robotics 6(1-2), 2017.
• M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard and F. Dellaert, iSAM2: incremental smoothing and mapping using the Bayes tree, International Journal of Robotics Research 31(2), 2012.