State Estimation

Control closes the loop under one quiet assumption: that someone knows the state. The pole's angle, the arm's configuration, the quadruped's velocity over ground. Nobody does. What the robot actually has is a stream of sensor readings, each one noisy, some biased, a few missing outright. State estimation is the discipline that turns that stream into a usable estimate, with an honest number attached describing how wrong the estimate might be.

The honest number is what separates the field from curve fitting. A controller that knows the estimate's uncertainty can act cautiously when the number is large and aggressively when it is small; a controller handed a bare guess cannot. Every estimator in this module is built to carry both.

The lab below is that idea running. A target wanders in one dimension, a position sensor drops about one reading in five, and the blue band is the filter's own claim about how wrong it might be. Move the measurement-noise slider up and the estimate smooths and lags while the band widens; move the process-noise slider up instead and the estimate hugs every reading. Nothing about the world changed either time. Only what the filter believes about it.

The Bayes filter

The full probabilistic answer starts with a belief: not a guess at the state, but a probability distribution over it, $bel(x_t)$. Two alternating steps keep the belief current Thrun 2005↓Probabilistic RoboticsSebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005.

The predict step pushes the belief through the process model. Given the control $u_t$, the distribution over the new state convolves yesterday's belief with the motion's uncertainty:

Prediction only ever smears the belief outward, because motion adds uncertainty. The update step then folds in the measurement $z_t$ through the sensor model $p(z_t \mid x_t)$, sharpening the belief where the measurement says the state is likely:

with $\eta$ the normalizer. Predict, update, repeat: that alternation is the Bayes filter, and every tractable estimator in robotics is this recursion under extra assumptions that make the integral computable Thrun 2005↓Probabilistic RoboticsSebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005.

The Kalman filter

Kalman's 1960 result is the case where the recursion collapses into matrix algebra Kalman 1960↓A New Approach to Linear Filtering and Prediction ProblemsR. E. Kalman, J. Basic Engineering, 1960. The Kalman filterKalman filterThe recursive state estimator for linear systems with Gaussian noise: a predict step propagates the state estimate and its covariance through the motion model, and an update step fuses each new measurement with a gain that weighs the model's uncertainty against the sensor's. Kalman published the recursion in 1960, and it is the minimum-variance estimator for the linear-Gaussian case. When the dynamics or the measurement model is nonlinear, the extended Kalman filter linearizes both about the current estimate instead. assumes the process and measurement models are linear and every noise term is Gaussian:

with $w_t \sim \mathcal{N}(0, Q_t)$ and $v_t \sim \mathcal{N}(0, R_t)$. Under those assumptions a Gaussian belief stays Gaussian forever, and the whole recursion reduces to tracking its mean and covariance. Predict:

Update, through the Kalman gain $K_t$:

The gain is the whole idea in one ratio: prior uncertainty over prior-plus-sensor uncertainty. A filter that trusts its model gets a small gain and mostly ignores the sensor; a filter that trusts its sensor gets a gain near one and jumps to the reading. Under the linear-Gaussian assumptions this recursion is the minimum-variance estimator outright, and Kalman's projection-based derivation still makes it the best linear estimator when the noise is merely white rather than Gaussian Kalman 1960↓A New Approach to Linear Filtering and Prediction ProblemsR. E. Kalman, J. Basic Engineering, 1960.

McGee and Schmidt's November 1985 NASA history credits Schmidt's Ames group with adapting Kalman's linear filter to nonlinear circumlunar navigation in the early 1960s. By early 1961, their simulations supported the approach, but operation on available flight computers had not yet been verified. The report distinguishes this work from Battin's Apollo navigation studies at MIT and Potter's square-root implementation used in Apollo. It also records the decision to use ground radar for primary Apollo navigation, with an onboard backup. This is the participants' retrospective account, not a claim that Ames alone engineered the flight system
McGee 1985↓Discovery of the Kalman Filter as a Practical Tool for Aerospace and IndustryLeonard A. McGee, Stanley F. Schmidt, NASA TM-86847, 1985.

That is what the lab at the top of this module is showing. The near-black trace is the truth, the gray dots are the readings, the blue trace is the estimate, and the band is the filter's own $\pm 2\sigma$ position uncertainty. The world's noise is fixed by the seed in the readout; the sliders set what the filter believes about the noise, and the defaults are matched to the truth. Raising $\sigma_r$ drops the gain, and raising $\sigma_q$ climbs it: the band then swells across each measurement gap, the predict-update sawtooth made visible.

One more thing to notice: the band is a claim the filter makes about itself. Set the sliders too low for the world it actually lives in and the estimate draws a narrow band that the truth keeps escaping. An overconfident filter is worse than no filter, because nothing downstream knows to distrust it.

The extended Kalman filter

Real robots are not linear. Joint angles enter through sines and cosines, range sensors see a nonlinear slice of the world, and the process model becomes $x_t = g(x_{t-1}, u_t) + w_t$ with measurements $z_t = h(x_t) + v_t$. The extended Kalman filter keeps the Gaussian belief and makes the recursion local: linearize both models about the current estimate using their JacobiansJacobianThe derivative of the forward-kinematics map: the matrix relating joint velocities to end-effector velocity at the current configuration, with one column per joint. It changes with configuration and can lose rank at singularities, where covering some task-space direction would demand unbounded joint speeds. The same matrix maps a wrench at the end effector back to the joint torques that balance it through its transpose, which makes it the working object of both velocity-level control and statics. $G_t = \partial g / \partial x$ and $H_t = \partial h / \partial x$, then propagate the covariance through the tangent system Thrun 2005↓Probabilistic RoboticsSebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005:

The mean moves through the true nonlinear maps $g$ and $h$; only the covariance takes the linearized route. In McGee and Schmidt's history, the Ames group moved from linearization about a nominal reference trajectory to relinearization about the current estimated state. They thought this "might offer substantial advantages". An accidentally off-nominal simulation converged after initial overshoots; the report calls this modification the extended Kalman filter. That example was not a general convergence guarantee: the report also discusses instability from round-off, inadequate statistical models and nonlinearities
McGee 1985↓Discovery of the Kalman Filter as a Practical Tool for Aerospace and IndustryLeonard A. McGee, Stanley F. Schmidt, NASA TM-86847, 1985.

The failure mode is baked into that construction. Linearization is valid near the estimate, and the estimate is exactly what you are unsure about. When the nonlinearity is strong over the region the covariance covers, the propagated $\Sigma$ stops describing the true error: the filter grows overconfident, the covariance and the actual error part ways, and the estimate can diverge. Reasoning about the covariance structure of spatial estimates is where the robotics community learned these lessons, in the work of Smith, Self, and Cheeseman on uncertain spatial relationships Smith 1990↓Estimating Uncertain Spatial Relationships in RoboticsRandall C. Smith, Matthew Self, Peter Cheeseman, Autonomous Robot Vehicles, 1990. The standard escapes are to keep the Gaussian but drop the analytic linearization, pushing a few deterministically chosen sample points through the true nonlinearity instead, which is the unscented Kalman filter of Julier and Uhlmann Julier 1997↓New Extension of the Kalman Filter to Nonlinear SystemsSimon J. Julier, Jeffrey K. Uhlmann, Proc. SPIE 3068, 1997, or to drop the Gaussian assumption entirely and carry the belief as a set of particles Thrun 2005↓Probabilistic RoboticsSebastian Thrun, Wolfram Burgard, Dieter Fox, MIT Press, 2005.

Factor graphs: keep the whole trajectory

In EKF-based SLAM, marginalizing old poses commits their linearized information to a Gaussian approximation; the filter cannot later relinearize those discarded pose variables. Full smoothing retains the trajectory and can revisit its linearization, but this does not guarantee that nonlinear optimization reaches the global minimum Dellaert 2006↓Square Root SAM: Simultaneous Localization and Mapping via Square Root Information SmoothingFrank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006Cadena 2016↓Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception AgeCesar Cadena, Luca Carlone, Henry Carrillo et al., IEEE Transactions on Robotics, 2016. Keep the entire trajectory $X = \{x_0, \dots, x_T\}$ as the thing being estimated, and write the posterior over it as a product of factors, one per measurement and one per motion constraint:

That factorization, drawn as a bipartite graph of variable nodes and factor nodes, is a factor graphfactor graphA bipartite graph that displays the factorization of a probability distribution: variable nodes hold the unknown states, and factor nodes hold the measurements and motion constraints that tie groups of variables together. Kschischang, Frey, and Loeliger unified the inference algorithms on these graphs under the sum-product algorithm in 2001. In robotics, a factor graph over the whole trajectory turns smoothing and SLAM into one sparse least-squares problem, the formulation behind Square Root SAM and its incremental successors., and Kschischang, Frey, and Loeliger's 2001 paper is the reference that unified the inference algorithms on such graphs under the sum-product algorithm Kschischang 2001↓Factor Graphs and the Sum-Product AlgorithmFrank R. Kschischang, Brendan J. Frey, Hans-Andrea Loeliger, IEEE Trans. Information Theory, 2001. With Gaussian factors, maximizing the posterior is one sparse nonlinear least-squares problem over the whole trajectory at once:

Dellaert and Kaess's 2006 Square Root SAM formulation jointly estimates the SLAMSLAMSimultaneous localization and mapping: estimating a robot's state while building a model of its environment. In the landmark-based formulation studied by Square Root SAM, the unknowns include the robot trajectory and landmark map. With known data associations, Gaussian process and measurement models, a uniform landmark prior, and the initial reference frame fixed, joint MAP estimation becomes nonlinear least squares. Cadena and colleagues describe MAP estimation, often expressed with factor graphs, as a standard SLAM formulation while also noting high-performing EKF-based systems. trajectory and landmark map. With known data associations, Gaussian process and measurement models, a uniform landmark prior, and the initial reference frame fixed, this gives a nonlinear least-squares problem. Nonlinear optimization solves successive linearized systems: for a full-rank system, QR acts on the measurement Jacobian, while Cholesky acts on the information matrix. Variable ordering controls fill-in and therefore the cost Dellaert 2006↓Square Root SAM: Simultaneous Localization and Mapping via Square Root Information SmoothingFrank Dellaert, Michael Kaess, Int. J. Robotics Research, 2006. iSAM made the approach incremental, updating only the part of the factorization a new measurement actually touches Kaess 2008↓iSAM: Incremental Smoothing and MappingMichael Kaess, Ananth Ranganathan, Frank Dellaert, IEEE Trans. Robotics, 2008. With Gaussian noise models, iSAM2 uses a Bayes tree for incremental nonlinear least-squares estimation, with incremental variable reordering and relinearization. An update re-eliminates the affected cliques and their ancestors, then reattaches unaffected subtrees. Changes in estimates can still propagate down the tree; relinearization and partial-update thresholds trade accuracy for computation. The update cost is not constant: large loop closures can be as expensive as a batch solution Kaess 2012↓iSAM2: Incremental Smoothing and Mapping Using the Bayes TreeMichael Kaess, Hordur Johannsson, Richard Roberts et al., Int. J. Robotics Research, 2012. This family is what modern visual-inertial odometry and offline mapping run on: GTSAM, a BSD-licensed C++ library that implements smoothing and mapping using factor graphs and Bayes networks, powers many such systems in academia and industry Dellaert 2026↓GTSAM: Georgia Tech Smoothing and MappingFrank Dellaert, GTSAM Contributors, 2026; Forster and colleagues' on-manifold preintegration is what made high-rate inertial data fit the framework, collapsing anywhere from a small number to hundreds of IMU readings between two keyframes into a single preintegrated factor Forster 2017↓On-Manifold Preintegration for Real-Time Visual-Inertial OdometryChristian Forster, Luca Carlone, Frank Dellaert et al., IEEE Trans. Robotics, 2017.

Cadena and colleagues describe a classical SLAM period from 1986 to 2004 that included EKF, Rao-Blackwellised particle-filter and maximum-likelihood approaches, followed by an algorithmic-analysis period from 2004 to 2015. Their survey presents MAP estimation, often expressed using factor graphs, as the de-facto standard formulation, but also highlights EKF-based systems with state-of-the-art performance. It reports a smaller filtering-versus-MAP performance gap when EKF linearization is accurate, sliding-window filters are used, and inconsistency is addressed. Graphs do not remove resource limits: the survey discusses unbounded graph growth and information-loss tradeoffs in sparsification, not a universal rule that smoothing wins Cadena 2016↓Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception AgeCesar Cadena, Luca Carlone, Henry Carrillo et al., IEEE Transactions on Robotics, 2016. What a robot then keeps from that estimate, a point cloud, a signed-distance field, a mesh, and which of those a planner can actually consume, has its own treatment in scene representation and mapping.

Where this meets the learned stack

State estimation is the layer of the classical stack that learning has penetrated least, and the reason is the honest number: a covariance you can trust is worth more than a point estimate that is usually right. Learned components mostly enter as better sensors. A learned depth model or a learned visual-odometry frontend feeds the same filter or factor graph, which keeps the uncertainty bookkeeping classical even when the measurement model is a network.

One connection is deeper than the rest. The latent-dynamics world models in the world-models module are learned state estimators in disguise: a recurrent latent state that predicts the next observation is a belief state by another name, trained end to end instead of derived. What learning changes is where the models come from; the predict-update structure underneath has survived every regime change since 1960.

With estimation in place, the classical stack closes: kinematics describes the body, planning picks the motion, control executes it, and estimation tells all three what actually happened.

Timing is part of the state

Many apparent estimator failures begin before the filter. A camera exposure, encoder reading and inertial sample that carry the same receipt time may describe different physical instants. Feeding them into a precise model creates a confident estimate of a state that never existed. Clock offset, transport delay and rolling-shutter timing belong in the measurement contract beside frame and covariance.

Test the pipeline in that order: verify clock domains and timestamp monotonicity, measure sensor latency, validate transforms, then tune process and measurement noise. Replay a recorded motion and inspect innovations by sensor; persistent directional residuals suggest calibration or timing bias rather than random noise. The calibration guide covers spatial and temporal residuals, while the ROS 2 guide covers timestamped message and transform interfaces. A larger covariance can express uncertainty. It cannot repair misassociated time.