State Estimation

The Kalman Filter is the foundational recursive algorithm for optimal linear state estimation. It operates in a two-step predict-update cycle, assuming Gaussian noise and linear dynamics. It is computationally efficient and provides an optimal estimate in the mean-squared error sense for linear systems.

• Core Mechanism: Maintains a Gaussian belief state represented by a mean vector (the state estimate) and a covariance matrix (the uncertainty).
• Prediction Step: Projects the current state and its uncertainty forward in time using a linear process model.
• Update Step: Fuses a new sensor measurement by computing the Kalman Gain, which optimally weights the prediction against the new observation.
• Primary Use: Ubiquitous in guidance, navigation, and control systems (e.g., GPS receivers, aircraft autopilots) for tracking position, velocity, and orientation.

The Extended Kalman Filter is the workhorse algorithm for state estimation in nonlinear systems. It extends the classic KF by linearizing the nonlinear process and observation models around the current state estimate using a first-order Taylor series expansion.

• Core Mechanism: Locally approximates nonlinear functions as linear, allowing the standard Kalman update equations to be applied.
• Jacobian Matrices: The linearization requires calculating the Jacobian (matrix of first-order partial derivatives) of the process and measurement models.
• Limitations: Performance degrades with high nonlinearity or poor initial estimates, as the linear approximation can become invalid. It also incurs the computational cost of calculating Jacobians at each step.
• Primary Use: Standard for robot localization, visual-inertial odometry, and any fusion problem with mildly nonlinear dynamics or sensor models.

The Unscented Kalman Filter is a derivative-free alternative to the EKF for nonlinear estimation. Instead of linearizing, it uses a deterministic sampling technique called the unscented transform to propagate the state's probability distribution through the nonlinear functions.

• Core Mechanism: Selects a minimal set of sigma points around the mean. These points are propagated through the true nonlinear models, and a new Gaussian is reconstructed from the transformed points.
• Advantages over EKF: Often provides more accurate estimates for highly nonlinear systems, as it captures the posterior mean and covariance to the 3rd order (Taylor series) for any nonlinearity. It avoids the need to derive and compute Jacobian matrices.
• Computational Cost: Similar to the EKF, but trades Jacobian calculation for multiple function evaluations.
• Primary Use: Preferred over the EKF in applications with strong nonlinearities, such as attitude estimation for spacecraft or aircraft.

The Particle Filter is a sequential Monte Carlo method designed for state estimation in systems with arbitrary nonlinearities and non-Gaussian noise distributions. It represents the posterior probability distribution using a set of random samples called particles.

• Core Mechanism: Maintains a cloud of particles, each a hypothesis of the true state with an associated weight. The algorithm iteratively predicts, updates (re-weights based on sensor likelihood), and resamples the particle set.
• Strengths: Highly flexible; can model multi-modal distributions (e.g., tracking an object that may be in one of several rooms). No assumptions of linearity or Gaussian noise are required.
• Challenges: Computational cost scales with the number of particles. Can suffer from particle degeneracy, where most particles have negligible weight. Requires careful design of the proposal distribution.
• Primary Use: Robot localization in non-Gaussian environments (Monte Carlo Localization), visual object tracking, and financial time-series analysis.

Bayesian Filtering is the unifying probabilistic framework that underpins all state estimation algorithms. It formulates estimation as the recursive computation of the posterior probability distribution of the state given all past measurements.

• Core Principle: Applies Bayes' theorem sequentially over time. The goal is to compute p(state_t | measurements_1:t).
• Two Recursive Steps:
  • Prediction (Chapman-Kolmogorov): p(state_t | measurements_1:t-1) = ∫ p(state_t | state_t-1) * p(state_t-1 | measurements_1:t-1) d state_t-1
  • Update (Bayesian): p(state_t | measurements_1:t) ∝ p(measurement_t | state_t) * p(state_t | measurements_1:t-1)
• Algorithms as Implementations: The Kalman filter (and its variants) provides an analytic solution when models are linear/Gaussian. The Particle Filter provides a sampling-based numerical approximation for the general case.
• Primary Use: The theoretical foundation for designing and understanding all probabilistic state estimators.

Graph Optimization is a batch estimation technique, contrasting with the recursive filters. It formulates the state estimation problem—particularly Simultaneous Localization and Mapping (SLAM)—as a graph to be solved via nonlinear least squares.

• Core Mechanism: Constructs a graph where nodes represent system states (e.g., robot poses at different times) and landmark positions. Edges represent constraints between nodes derived from sensor measurements (odometry, lidar scans, loop closures).
• Solving: The graph is solved by finding the configuration of node values that minimizes the error across all constraints, typically using algorithms like Gauss-Newton or Levenberg-Marquardt. This is often more accurate than filtering as it can re-optimize past states.
• Factor Graphs: A related representation that explicitly factors the joint probability distribution, making the optimization structure clear.
• Primary Use: The standard backend for modern, high-accuracy SLAM systems (e.g., Google Cartographer, ORB-SLAM) and offline sensor network calibration.

A Kalman filter is a recursive algorithm that uses a series of noisy measurements observed over time to produce optimal estimates of unknown variables, assuming that errors are Gaussian-distributed and the underlying system is linear. It operates in a two-step predict-update cycle:

• Prediction: Projects the current state and its uncertainty forward using a process model.
• Update: Corrects the prediction with a new sensor measurement, weighting the correction by the relative certainty of the prediction versus the measurement (the Kalman gain). It is foundational for real-time tracking in GPS, inertial navigation, and control systems.

A particle filter is a sequential Monte Carlo method used for state estimation in nonlinear and non-Gaussian systems. It represents the posterior probability distribution with a set of random samples, or particles, and their associated weights.

• Each particle is a hypothesis about the system's state.
• On each new measurement, particles are re-weighted based on their likelihood.
• A resampling step discards low-weight particles and duplicates high-weight ones to focus computation on probable states. This approach is highly flexible and is used for robot localization, visual tracking, and financial modeling where linear/Gaussian assumptions fail.

Simultaneous Localization and Mapping is the computational problem of constructing or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it. It is a quintessential state estimation challenge where the state includes both the robot's pose and the positions of landmarks.

• Front-end: Handles sensor data association (e.g., matching lidar points to map features).
• Back-end: Performs graph optimization to minimize the error across all pose and landmark constraints. SLAM is critical for autonomous vehicles, drones, and augmented reality.

Bayesian filtering is the general probabilistic framework for recursively estimating the state of a dynamic system from noisy sensor data. It provides the mathematical foundation for all modern state estimators.

• It maintains a belief state, which is a probability distribution over all possible states.
• The belief is updated via Bayes' rule, combining a prior belief (the prediction from a process model) with the likelihood of new sensor data (from a sensor model). The Kalman filter, particle filter, and hidden Markov model are all specific implementations of this Bayesian recursion.

A factor graph is a bipartite graph used to represent the factorization of a complex probability distribution. In state estimation and SLAM, it provides a powerful framework for structuring and solving large-scale optimization problems.

• Variable Nodes: Represent unknown states (e.g., robot poses, landmark positions).
• Factor Nodes: Represent constraints or measurements that connect variables (e.g., odometry between poses, a lidar observation of a landmark). Algorithms like Gaussian Belief Propagation or nonlinear least squares (e.g., Levenberg-Marquardt) are used to find the variable values that maximize the product of all factors, yielding the optimal state estimate.

Robust estimation refers to statistical methods designed to be insensitive to outliers and model violations, which are critical in real-world sensor fusion where measurements can be erroneous.

• RANSAC (Random Sample Consensus): Iteratively fits a model to random subsets of data to find the set with the most inliers, discarding outliers.
• M-estimators: Replace the squared-error loss (used in least squares) with a less aggressive function (e.g., Huber loss) that reduces the influence of large residuals.
• Mahalanobis Distance Gating: Uses the covariance matrix of an expected measurement to define a probabilistic gate; measurements falling outside this gate are rejected as potential outliers. These techniques ensure state estimators maintain accuracy despite sensor noise, multipath errors, or temporary occlusions.