State Estimation

State estimation is fundamentally a Bayesian inference problem. It treats the system's true state as a random variable with an associated probability distribution. The goal is to compute the posterior distribution—the probability of the state given all past measurements. This framework naturally handles sensor noise and system uncertainty. Key algorithms like the Kalman filter and particle filter are specific implementations of this probabilistic recursion.

All recursive state estimators operate on a two-step cycle:

• Prediction (Time Update): Uses a process model (or motion model) to propagate the state estimate forward in time based on known control inputs or dynamics. This step increases uncertainty.
• Update (Measurement Update): Corrects the predicted state by incorporating a new sensor observation via a measurement model. This step, which applies Bayes' rule, reduces uncertainty. This cycle runs continuously, fusing dynamics and sensor data to maintain a live estimate.

The accuracy of estimation hinges on the fidelity of two mathematical models:

• Process Model: Describes how the state evolves: x_k = f(x_{k-1}, u_k, w_k). It encodes system dynamics and process noise w_k.
• Measurement Model: Describes how sensors observe the state: z_k = h(x_k, v_k). It relates the true state to the sensor reading, including measurement noise v_k. For linear models with Gaussian noise, the Kalman filter is optimal. Extended Kalman Filters (EKF) and Unscented Kalman Filters (UKF) linearize these models for nonlinear systems.

A state estimate is meaningless without a measure of its certainty. Estimators explicitly track uncertainty, most commonly using a covariance matrix P. This matrix quantifies:

• Variance (diagonal elements): Uncertainty in each state variable (e.g., position error).
• Covariance (off-diagonal elements): Correlation between state variables (e.g., between x-position and x-velocity). The Kalman filter's equations optimally propagate and update this covariance. In particle filters, uncertainty is represented by the spread of the particle cloud.

Observability determines whether a system's internal state can be uniquely determined from its external outputs (measurements). An unobservable state, like a robot's global yaw using only an IMU, cannot be estimated and will drift unbounded. Drift is the accumulation of small errors over time, inherent in dead reckoning. State estimation combats drift by fusing absolute measurements (e.g., GPS, loop closures) that make the system observable. The lack of observability is a primary cause of filter divergence.

Before a sensor measurement can be used for an update, the estimator must solve the data association problem: determining which part of the internal state or map the measurement corresponds to (e.g., which landmark is this camera feature?). Incorrect association corrupts the estimate. Robust estimators employ techniques like:

• Outlier Rejection: Using Mahalanobis distance tests to discard statistically inconsistent measurements.
• RANSAC: Robustly fitting models in the presence of outliers. This ensures the system is resilient to sensor failures and perceptual aliasing.

Self-driving cars rely on tightly-coupled sensor fusion to maintain centimeter-level accuracy of their position, velocity, and orientation (pose) in real-time. This is achieved by fusing:

• GNSS (Global Navigation Satellite System) for absolute global positioning.
• Inertial Measurement Units (IMUs) for high-frequency acceleration and rotation data, critical when GPS signals are lost.
• LiDAR and camera-based Visual Odometry for relative motion and landmark matching. Algorithms like the Error State Kalman Filter (ESKF) are commonly used to manage the nonlinear dynamics and correct for sensor drift. The system must also perform outlier rejection to ignore erroneous LiDAR returns from rain or incorrect camera data associations.

Quadcopters and UAVs use state estimation for basic flight stabilization and autonomous navigation. A Micro Air Vehicle (MAV) typically employs a Visual-Inertial Odometry (VIO) system, combining a downward-facing camera and an IMU.

• The IMU provides high-rate angular velocity and linear acceleration for attitude estimation and control.
• The camera tracks visual features on the ground to estimate horizontal velocity and correct for the IMU's integration drift. This loosely-coupled or tightly-coupled fusion allows drones to hover precisely, follow trajectories, and execute maneuvers without external motion capture systems. Particle filters may be used in GPS-denied environments for robust pose estimation.

In industrial robotics and precise manipulation, state estimation extends beyond pose to infer forces and torques that cannot be directly measured. Series Elastic Actuators or robots without dedicated force-torque sensors use model-based observers.

• The process model incorporates the robot's dynamic equations of motion.
• The measurement model uses motor current and joint encoder positions. By comparing the predicted motion (from the dynamic model and commanded currents) with the actual measured motion, the algorithm (e.g., a Disturbance Observer) can estimate the external forces and torques applied to the end-effector. This enables compliant control, assembly tasks, and safe human-robot collaboration.

Spacecraft must know their orientation (attitude) with extreme precision to point antennas, solar panels, and scientific instruments. They use a suite of sensors and sophisticated filters:

• Star Trackers provide high-accuracy, low-noise attitude measurements by imaging star fields.
• Sun Sensors and Magnetometers offer coarse but reliable attitude references.
• Gyroscopes deliver high-frequency angular rate data but suffer from bias drift. An Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF) fuses these measurements, using the gyros for propagation and the other sensors for updates. The filter must also estimate and correct for time-varying gyro biases, a critical form of online sensor calibration.

State estimation enables inertial-based motion tracking for sports science, healthcare, and virtual reality. Systems using Inertial Measurement Units (IMUs) alone must solve the drift-free orientation problem.

• A Complementary Filter or Kalman filter fuses accelerometer (for gravity direction, stable when not moving) and gyroscope (for high-frequency rotation) data to estimate 3D orientation.
• Magnetometer data can be fused to provide a global heading reference, though it is susceptible to magnetic disturbances. For full-body tracking, the orientation estimates from multiple IMUs (on limbs) are combined with a biomechanical skeleton model using kinematic constraints to estimate joint angles and 3D pose, eliminating the need for optical cameras.

Autonomous Underwater Vehicles (AUVs) operate in a GPS-denied and often feature-poor environment, making state estimation exceptionally challenging. They rely on a fusion of:

• Doppler Velocity Log (DVL) for measuring velocity relative to the seafloor.
• IMU for attitude and dead reckoning.
• Pressure Sensor for depth estimation.
• Acoustic Long Baseline (LBL) or Ultra-Short Baseline (USBL) systems for intermittent absolute positioning updates, akin to underwater GPS. The core estimator, often a particle filter to handle non-Gaussian noise from acoustic multipath, must bridge long periods between absolute fixes. Simultaneous Localization and Mapping (SLAM) using sonar data is also used to build maps and correct drift.

The Kalman filter is the foundational optimal recursive algorithm for linear state estimation. It operates in a two-step predict-update cycle:

• Predict: Projects the current state and its uncertainty forward using a process model.
• Update: Corrects the prediction with a new sensor measurement using a measurement model, weighted by the respective uncertainties (covariances). It is mathematically optimal for linear systems with Gaussian noise, making it the cornerstone for GPS, inertial navigation, and countless control systems.

A particle filter is a sequential Monte Carlo method for nonlinear, non-Gaussian state estimation problems where the Kalman filter assumptions break down. It represents the state's probability distribution using a set of random samples called particles.

• Each particle is a hypothesis of the true state, with an associated weight.
• The algorithm propagates particles through the motion model, weights them according to the sensor measurement likelihood, and resamples to focus computational resources on the most probable states. This makes it highly effective for problems like robot localization in ambiguous environments.

A factor graph is a bipartite graphical model that represents the factorization of a complex probability distribution. In state estimation, it provides a unifying framework for structuring Maximum A Posteriori (MAP) estimation problems.

• Variable Nodes: Represent unknown states (e.g., robot poses, landmark positions).
• Factor Nodes: Represent constraints connecting variables, derived from process models, sensor measurements, or prior information. Algorithms perform inference on this graph to find the most likely configuration of all states. It is the backbone of modern SLAM and bundle adjustment systems.

Visual-Inertial Odometry (VIO) is a specific, high-performance state estimation technique that fuses a camera and an Inertial Measurement Unit (IMU). It provides robust, high-frequency 6-degree-of-freedom pose tracking.

• The camera provides rich but sometimes unreliable geometric constraints.
• The IMU provides high-frequency acceleration and angular velocity, excellent for motion prediction but prone to drift. Through tightly-coupled fusion, VIO uses the IMU to predict motion between camera frames and uses visual features to correct IMU drift, enabling operation in dynamic or visually degraded environments.

Observability is a fundamental system property that determines whether the internal state of a dynamic system can be uniquely inferred from its external outputs (measurements) over time.

• An observable state can be estimated.
• An unobservable state cannot be distinguished from other states, leading to estimation drift or failure. In sensor fusion, certain motions or sensor configurations can make critical states (e.g., a robot's global yaw when using only an IMU and monocular camera) unobservable. Analysis of observability is crucial for designing robust estimation systems.

The covariance matrix is the mathematical object that quantifies estimation uncertainty and correlations. For a state vector, it is a square, symmetric matrix where:

• Diagonal elements represent the variance (uncertainty) of each state variable.
• Off-diagonal elements represent the covariance (correlation) between different state variables. In a Kalman filter, the covariance matrix is propagated alongside the state mean. It dictates the Kalman gain, determining how much to trust a new measurement versus the prior prediction. A growing covariance indicates increasing uncertainty, often due to a lack of informative measurements.