State Estimation

State estimation computes a robot's state, such as its configuration or velocity over ground, from noisy, biased and intermittent sensor readings, and planning and control assume that state is known. Probabilistic estimators report a belief, so each estimate carries its uncertainty^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005.

Photo: ArnoldReinhold / Wikimedia Commons. Licence: CC BY-SA 4.0.

The Bayes filter

A belief $bel(x_t)$ is a probability distribution over the state, kept current by two alternating steps. The predict step pushes the belief through the process model, given the control $u_t$^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005:

The update step folds in the measurement $z_t$ through the sensor model $p(z_t \mid x_t)$ and sharpens the belief where the measurement points^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005:

with normalizer $\eta$. This recursion is the Bayes filter, which Thrun, Burgard and Fox call the basis of virtually every technique in their book, and each tractable filter adds assumptions or approximations that make the integral computable^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005. The bootstrap filter of Gordon, Salmond and Smith drops the linearity and Gaussian-noise assumptions and applies to any state transition or measurement model^{Source 13}13 Gordon et al. 1993, Novel approach to nonlinear/non-Gaussian Bayesian state estimation. N. J. Gordon, D. J. Salmond, A. F. M. Smith, IEE Proceedings F (Radar and Signal Processing), 1993. Drive the robot in the figure below: its cloud of doubt grows as it moves and shrinks when it spots a landmark.

Does a robot know where it is?

It drives by counting wheel turns. Each turn adds a little error, so its guess slips further off and its cloud of doubt grows.

BackNext

A robot never knows exactly where it is: moving makes it less sure and each landmark it sees makes it surer.

How this was made

The steps in words

1. It drives by counting wheel turns. Each turn adds a little error, so its guess slips further off and its cloud of doubt grows.
2. Now its camera spots a landmark: a sign it already knows, at a known place on its map.
3. A poorer camera shrinks the cloud less. The robot blends its own guess with the landmark's, trusting whichever has less doubt.
4. This blending is a Kalman filter. Spacecraft, drones and many robots use one to keep track of where they are.

The Kalman filter

The 1960 Kalman filterKalman filterThe Kalman filter is the recursive state estimator for linear systems with Gaussian noise. It predicts the estimate and covariance through the motion model, then fuses each measurement with a gain weighing model against sensor uncertainty. Kalman published it in 1960; it is minimum-variance in the linear-Gaussian case. The extended Kalman filter linearizes nonlinear models about the current estimate. reduces the recursion to matrix algebra by assuming linear process and measurement models and Gaussian noise^{Source 1}1 Kalman 1960, A New Approach to Linear Filtering and Prediction Problems. R. E. Kalman, J. Basic Engineering, 1960:

with $w_t \sim \mathcal{N}(0, Q_t)$ and $v_t \sim \mathcal{N}(0, R_t)$; Probabilistic Robotics swaps the two symbols. A Gaussian belief then stays Gaussian, so the recursion tracks only a mean and covariance. Predict^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005:

Update, through the Kalman gain $K_t$^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005:

The gain is prior uncertainty over prior-plus-sensor uncertainty: a filter that trusts its model gets a small gain and mostly ignores the sensor, and one that trusts its sensor gets a gain near one. Under linear-Gaussian assumptions the recursion is the minimum-variance estimator, and Kalman's projection-based derivation makes it the best linear estimator with white noise even without Gaussian noise^{Source 1}1 Kalman 1960, A New Approach to Linear Filtering and Prediction Problems. R. E. Kalman, J. Basic Engineering, 1960.

McGee and Schmidt's 1985 NASA history credits Schmidt's Ames group with adapting the filter to nonlinear circumlunar navigation in the early 1960s. By early 1961 simulations supported the approach, though operation on available flight computers was unverified. The report separates this work from Battin's Apollo studies at MIT and Potter's square-root implementation used in Apollo. Primary Apollo navigation used ground radar, with an onboard backup^{Source 2}2 McGee et al. 1985, Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry. Leonard A. McGee, Stanley F. Schmidt, NASA TM-86847, 1985.

Photo: NASA / Wikimedia Commons. Licence: Public domain.

Kalman filter, a whole run

The filter keeps tracking, but grows less sure without readings

Play the run

Through noisy and missing readings the filter stays close to the object, and its band shows when a robot should trust it less.

How this was made

A seeded toy simulation (seed 1): an object drifts along a line, and a position sensor reads it with random noise and misses about one step in five. The clip shows 240 steps of that run, steps 300 to 540, with time running left to right and position up and down.

Each step has two halves, which Kalman filters call predict and update: predicting where the object moved widens the band, and a reading, when one arrives, narrows it.

The band spans two standard deviations either side of the filter's estimate, about 95% of its belief. The filter's noise settings match the simulation: process noise 0.20, reading noise 1.00.

The note and its arrow mark steps 436 to 439, the longest run of missing readings in the simulation: four in a row, with that stretch of the band tinted in the note's colour. By the fourth missing step the band is three times as wide as at the last reading before the gap, and it narrows when the next reading arrives.

The clip is rendered offline from the same filter code as the scene above. It is silent; its captions and the transcript carry the words.

Illustrative: a simulated run, not measured on hardware.

Illustrative cinematic clip, 16 seconds: one Kalman filter over a whole run of 240 steps, with time running left to right and position up and down. Labels name each mark. An object moves back and forth; a dashed line marks its true position. A noisy sensor reports the object's position as scattered dots, and about one time in five it reports nothing. The filter's estimate, a solid line, stays close to the true position, and a shaded band around it shows how unsure the filter is: a wider band means less sure. A note reads: no readings, the filter grows less sure. Its arrow points at the band's widest stretch, tinted in the note's colour, where four readings in a row are missing and the band grows to three times its width; the next reading narrows it again. In filter terms, each predict step widens the band and each update with a reading narrows it. Simulated run, not measured on hardware.

The extended Kalman filter

Real robots are nonlinear: joint angles enter through sines and cosines, so the models become $x_t = g(x_{t-1}, u_t) + w_t$ and $z_t = h(x_t) + v_t$. The extended Kalman filter keeps the Gaussian belief, linearizes both models about the current estimate with their JacobiansJacobianThe Jacobian is the derivative of the forward-kinematics map: a matrix with one column per joint, relating joint velocities to end-effector velocity at the current configuration. It can lose rank at singularities, where some task-space direction needs unbounded joint speeds. Its transpose maps an end-effector wrench to the balancing joint torques, so it serves both velocity-level control and statics. $G_t = \partial g / \partial x$ and $H_t = \partial h / \partial x$, and propagates the covariance through the tangent system^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005:

The mean moves through the true maps $g$ and $h$; only the covariance takes the linearized route^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005. The Ames group moved from linearizing about a nominal reference trajectory to relinearizing about the current estimated state, which they hoped would bring "substantial advantages". An accidentally off-nominal simulation converged after initial overshoots, and the report names this modification the extended Kalman filter. It also discusses instability from round-off, inadequate statistical models and nonlinearities^{Source 2}2 McGee et al. 1985, Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry. Leonard A. McGee, Stanley F. Schmidt, NASA TM-86847, 1985.

Linearization holds near the estimate, which is itself uncertain, so where the nonlinearity is strong over the region the covariance covers, $\Sigma$ stops describing the true error, and the filter grows overconfident and can diverge^{Source 2}2 McGee et al. 1985, Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry. Leonard A. McGee, Stanley F. Schmidt, NASA TM-86847, 1985 ^{Source 6}6 Dellaert et al. 2006, Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Frank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006. Julier and Uhlmann summarize 35 years of experience: the EKF is difficult to implement and tune, and reliable only for systems almost linear on the time scale of the updates^{Source 14}14 Julier et al. 2004, Unscented Filtering and Nonlinear Estimation. S. J. Julier, J. K. Uhlmann, Proceedings of the IEEE, 2004. Li and Mourikis proved that standard EKF visual-inertial odometry is inconsistent because of how its Jacobians are computed, so it underestimates its uncertainty^{Source 15}15 Li et al. 2013, High-precision, consistent EKF-based visual-inertial odometry. Mingyang Li, Anastasios I. Mourikis, Int. J. Robotics Research, 2013.

The unscented Kalman filter of Julier and Uhlmann keeps the Gaussian and pushes deterministically chosen sample points through the true nonlinearity^{Source 4}4 Julier et al. 1997, New Extension of the Kalman Filter to Nonlinear Systems. Simon J. Julier, Jeffrey K. Uhlmann, Proc. SPIE 3068, 1997. Particle filters drop the Gaussian assumption and carry the belief as a set of particles^{Source 3}3 Thrun et al. 2005, Probabilistic Robotics. Sebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005.

Invariant filters on Lie groups remove the dependence on the estimate: in the contact-aided invariant EKF, neither the linearized error dynamics nor the linearized observation model depends on the current state estimate^{Source 16}16 Hartley et al. 2019, Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation. Ross Hartley, Maani Ghaffari, Ryan M. Eustice et al., arXiv preprint (Int. J. Robotics Research 2020), 2019.

Factor graphs: keep the whole trajectory

Rauch, Tung and Striebel related filtered and smoothed estimates in 1965 and showed numerically that smoothing reduces errors^{Source 18}18 Rauch et al. 1965, Maximum likelihood estimates of linear dynamic systems. H. E. Rauch, F. Tung, C. T. Striebel, AIAA Journal, 1965. In EKF-based SLAM, marginalizing old poses commits their linearized information to a Gaussian approximation that cannot be relinearized later. Full smoothing keeps the trajectory and can relinearize, though nonlinear optimization is not guaranteed to reach the global minimum^{Source 6}6 Dellaert et al. 2006, Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Frank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006 ^{Source 9}9 Cadena et al. 2016, Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception Age. Cesar Cadena, Luca Carlone, Henry Carrillo et al., IEEE Transactions on Robotics, 2016. The posterior over the trajectory $X = \{x_0, \dots, x_T\}$ is a product of factors, one per measurement or motion constraint^{Source 6}6 Dellaert et al. 2006, Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Frank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006:

Drawn as a bipartite graph of variable and factor nodes, this is a factor graphfactor graphA factor graph is a bipartite graph of a probability distribution's factorization: variable nodes hold unknown states, factor nodes the measurements and motion constraints linking them. Kschischang, Frey and Loeliger unified inference on these graphs under the sum-product algorithm in 2001. Over a robot trajectory, it makes smoothing and SLAM one sparse least-squares problem, as in Square Root SAM and its incremental successors.. Kschischang, Frey, and Loeliger's 2001 paper unified inference on such graphs under the sum-product algorithm^{Source 5}5 Kschischang et al. 2001, Factor Graphs and the Sum-Product Algorithm. Frank R. Kschischang, Brendan J. Frey, Hans-Andrea Loeliger, IEEE Trans. Information Theory, 2001. With Gaussian factors, the most probable trajectory solves one sparse nonlinear least-squares problem^{Source 6}6 Dellaert et al. 2006, Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Frank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006:

Dellaert and Kaess's 2006 Square Root SAM jointly estimates the SLAMSLAMSLAM (simultaneous localization and mapping) is estimating a robot's state while building a model of its environment. In Square Root SAM's landmark-based formulation, the unknowns include the robot trajectory and landmark map. Given known data associations, Gaussian process and measurement models, a uniform landmark prior and a fixed initial reference frame, joint MAP estimation becomes nonlinear least squares. Cadena et al. call MAP estimation, often expressed with factor graphs, a standard SLAM formulation, while noting high-performing EKF-based systems. trajectory and landmark map, assuming known data associations, Gaussian models, a uniform landmark prior and a fixed initial frame. It solves successive linearized systems, by QR on the measurement Jacobian or Cholesky on the information matrix, and variable ordering sets fill-in and cost^{Source 6}6 Dellaert et al. 2006, Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Frank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006. The iSAM method updates only the part of the factorization a new measurement touches^{Source 7}7 Kaess et al. 2008, iSAM: Incremental Smoothing and Mapping. Michael Kaess, Ananth Ranganathan, Frank Dellaert, IEEE Trans. Robotics, 2008.

iSAM2 adds a Bayes tree with incremental reordering and relinearization: an update re-eliminates the affected cliques and their ancestors, then reattaches unaffected subtrees. Thresholds trade accuracy for computation, and a large loop closure can cost as much as a batch solve^{Source 8}8 Kaess et al. 2012, iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree. Michael Kaess, Hordur Johannsson, Richard Roberts et al., Int. J. Robotics Research, 2012.

GTSAM is a BSD-licensed C++ library that implements sensor fusion for robotics and computer vision using factor graphs^{Source 11}11 Dellaert et al. 2026, GTSAM: Georgia Tech Smoothing and Mapping. Frank Dellaert, GTSAM Contributors, 2026. Version 4.3 moved to C++17 and added experimental CUDA nonlinear optimization and several legged-state-estimation implementations^{Source 17}17 Dellaert et al. 2026, GTSAM 4.3.0. Frank Dellaert, GTSAM Contributors, borglab/gtsam GitHub release, 2026-09-18. Building on Lupton and Sukkarieh's preintegration, Forster and colleagues' on-manifold version collapses anywhere from a few to hundreds of IMU readings between two keyframes into one factor^{Source 10}10 Forster et al. 2017, On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. Christian Forster, Luca Carlone, Frank Dellaert et al., IEEE Trans. Robotics, 2017.

Cadena and colleagues describe a classical SLAM period from 1986 to 2004, with EKF, Rao-Blackwellised particle-filter and maximum-likelihood approaches, and an algorithmic-analysis period from 2004 to 2015^{Source 9}9 Cadena et al. 2016, Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception Age. Cesar Cadena, Luca Carlone, Henry Carrillo et al., IEEE Transactions on Robotics, 2016. In Rao-Blackwellized grid mapping, each particle carries its own map^{Source 19}19 Grisetti et al. 2007, Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters. Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard, IEEE Trans. Robotics, 2007.

The survey treats MAP estimation, often over factor graphs, as the de facto standard but notes EKF systems with state-of-the-art performance. The gap shrinks when EKF linearization is accurate, sliding windows are used and inconsistency is addressed, and graphs still face unbounded growth and information loss under sparsification^{Source 9}9 Cadena et al. 2016, Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception Age. Cesar Cadena, Luca Carlone, Henry Carrillo et al., IEEE Transactions on Robotics, 2016. The MSCKF sliding-window filter keeps 3D feature positions out of the state and is optimal up to linearization errors^{Source 20}20 Mourikis et al. 2007, A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. Anastasios I. Mourikis, Stergios I. Roumeliotis, ICRA 2007, while OKVIS argues that nonlinear optimization offers superior accuracy and stays tractable through sparsity^{Source 21}21 Leutenegger et al. 2014, Keyframe-based visual–inertial odometry using nonlinear optimization. Stefan Leutenegger, Simon Lynen, Michael Bosse et al., Int. J. Robotics Research, 2014. Scene representation and mapping covers what a robot keeps from the estimate.

Where this meets the learned stack

Learned components mostly enter classical estimators as measurements with uncertainty. TLIO regresses 3D displacement and its uncertainty from IMU data and fuses both into a stochastic cloning EKF^{Source 22}22 Liu et al. 2020, TLIO: Tight Learned Inertial Odometry. Wenxin Liu, David Caruso, Eddy Ilg et al., IEEE Robotics and Automation Letters, 2020, and MAC-VO learns a metrics-aware keypoint covariance and uses it to weight pose graph optimization^{Source 23}23 Qiu et al. 2024, MAC-VO: Metrics-aware Covariance for Learning-based Stereo Visual Odometry. Yuheng Qiu, Yutian Chen, Zihao Zhang et al., arXiv preprint (ICRA 2025), 2024.

A September 2026 paper from Linus Kramer, William Talbot, Olga Vysotska and Marco Hutter is an example. For robots on construction sites, they replace the observation model of a Monte Carlo localization particle filter with a network that matches LiDAR scans against the building mesh, trained only on scans simulated inside that mesh. On real-world datasets it outperformed diffusion-based and ScanContext++ baselines at 18 ms per call^{Source 12}12 Kramer et al. 2026, Transformer-based Monte Carlo Localization in Construction Meshes. Linus Kramer, William Talbot, Olga Vysotska et al., 2026.

GAIT estimates legged-robot state without an explicit contact estimator and outperforms contact-aided model-based methods on a Unitree Go1^{Source 24}24 Seo et al. 2026, GAIT: Legged Robot Proprioceptive State Estimation with Attention over Inertial-Leg Tokens. Young-Rang Seo, Hajun Kim, Sangmin Kim et al., arXiv preprint, 2026.

Yuheng Qiu, Pieter Abbeel, Guanya Shi and colleagues at Amazon FAR, Carnegie Mellon and three other universities (October 2026) train inertial odometry from scratch without velocity or position labels: a teacher integrates the IMU over each short window, and a pose graph over many windows refines the targets^{Source 32}32 Qiu et al. 2026, Self-Supervised Inertial Odometry. Yuheng Qiu, Yutian Chen, Carlton Cort et al., 2026. On six datasets from pedestrians, drones, quadrupeds and a humanoid, average velocity error came within 0.07 m/s of a supervised network of the same architecture, though its trajectory error on the EuRoC drone flights was 10.23 m against the supervised network's 3.72 m^{Source 32}32 Qiu et al. 2026, Self-Supervised Inertial Odometry. Yuheng Qiu, Yutian Chen, Carlton Cort et al., 2026. The teacher used a reference attitude to remove gravity, and the IMU bias must be calibrated^{Source 32}32 Qiu et al. 2026, Self-Supervised Inertial Odometry. Yuheng Qiu, Yutian Chen, Carlton Cort et al., 2026.

Jiaxi Jiang, Marco Hutter, Christian Holz and colleagues at ETH Zurich (October 2026) estimate a humanoid's joint angles and world-space motion from ordinary video, training the pose decoder only on 128,000 synthetic images of sixteen humanoids^{Source 33}33 Jiang et al. 2026, Vision as Proprioception: Estimating Humanoid Robot Pose from Video. Jiaxi Jiang, Matthias Heyrman, René Zurbrügg et al., 2026. With no real pose labels, joint-angle error on three real humanoids was 18 to 32% below the best baseline on each^{Source 33}33 Jiang et al. 2026, Vision as Proprioception: Estimating Humanoid Robot Pose from Video. Jiaxi Jiang, Matthias Heyrman, René Zurbrügg et al., 2026.

Latent-dynamics world models act as learned belief states. PlaNet's latent dynamics model has deterministic and stochastic transition components trained with variational inference^{Source 25}25 Hafner et al. 2018, Learning Latent Dynamics for Planning from Pixels. Danijar Hafner, Timothy Lillicrap, Ian Fischer et al., 2018. DreamerV3 learns a model of the environment and improves its behaviour by imagining future scenarios^{Source 26}26 Hafner et al. 2025, Mastering diverse control tasks through world models. Danijar Hafner, Jurgis Pasukonis, Jimmy Ba et al., Nature 640, 647-653, 2025.

Timing is part of the state

Fusing visual and inertial measurements requires each measurement's precise sampling time^{Source 27}27 Li et al. 2014, Online temporal calibration for camera–IMU systems: Theory and algorithms. Mingyang Li, Anastasios I. Mourikis, Int. J. Robotics Research, 2014, and most consumer cameras use rolling shutters that capture each pixel row at a slightly different time^{Source 28}28 Li et al. 2013, Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera. Mingyang Li, Byung Hyung Kim, Anastasios I. Mourikis, ICRA 2013. The camera-IMU offset can be a state variable, locally identifiable except in a few degenerate motions^{Source 27}27 Li et al. 2014, Online temporal calibration for camera–IMU systems: Theory and algorithms. Mingyang Li, Anastasios I. Mourikis, Int. J. Robotics Research, 2014; OpenVINS calibrates it in its sliding-window filter^{Source 29}29 Geneva et al. 2020, OpenVINS: A Research Platform for Visual-Inertial Estimation. Patrick Geneva, Kevin Eckenhoff, Woosik Lee et al., ICRA 2020, and Kalibr estimates time offsets to a fraction of the smallest measurement period^{Source 30}30 Furgale et al. 2013, Unified temporal and spatial calibration for multi-sensor systems. Paul Furgale, Joern Rehder, Roland Siegwart, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

Persistent directional residuals in per-sensor innovations point to calibration or timing bias instead of random noise. GrandTour, which its authors call the largest open-access legged-robotics dataset, supplies RTK-GNSS and total-station ground truth^{Source 31}31 Tuna et al. 2026, GrandTour: A Legged Robotics Dataset in the Wild for Multi-Modal Perception and State Estimation. Turcan Tuna, Jonas Frey, Frank Fu et al., arXiv preprint, 2026. The calibration guide covers temporal residuals, and the ROS 2 guide covers timestamped interfaces.