State Estimation

The Kalman Filter is an optimal recursive algorithm for estimating the state of a linear dynamic system perturbed by Gaussian noise. It operates in a two-step predict-update cycle:

• Predict: Projects the current state and uncertainty forward using the system's motion model.
• Update: Corrects the prediction using a new sensor measurement, weighting the prediction and measurement by their respective uncertainties (covariances).

Its optimality and efficiency make it foundational for systems with well-modeled linear dynamics, such as basic tracking and navigation.

The Extended Kalman Filter is the de facto standard for state estimation in nonlinear systems. It linearizes the nonlinear motion and observation models around the current state estimate at each timestep, then applies the standard Kalman Filter equations to the linearized approximations.

Key Applications:

• Robot localization (e.g., fusing wheel odometry with landmark observations).
• Visual-inertial odometry (VIO).
• GPS/INS sensor fusion.

Its main limitation is the introduction of linearization errors, which can cause divergence if the system is highly nonlinear or the initial estimate is poor.

The Unscented Kalman Filter addresses the linearization errors of the EKF by using a deterministic sampling technique called the Unscented Transform. Instead of linearizing, it selects a minimal set of sample points (sigma points) that capture the mean and covariance of the state distribution. These points are propagated through the true nonlinear functions, and a new Gaussian state estimate is reconstructed from the transformed points.

Advantages over EKF:

• More accurate for strongly nonlinear systems.
• Avoids the need to compute Jacobian matrices analytically.
• Often more robust to poor initial estimates.

The Particle Filter is a sequential Monte Carlo method designed for highly nonlinear and non-Gaussian estimation problems. It represents the state's probability distribution using a set of random samples (particles), each with an associated weight.

Algorithm Steps:

1. Prediction: Each particle is propagated through the motion model, adding noise.
2. Update: Particle weights are updated based on the likelihood of the new sensor measurement.
3. Resampling: Particles with low weights are discarded, and particles with high weights are duplicated to focus computation on likely states.

It is computationally expensive but powerful for problems like global robot localization (Monte Carlo Localization) and tracking in cluttered environments.

Moving Horizon Estimation formulates state estimation as an online optimization problem. Instead of only using the current measurement, it considers a sliding window of the most recent measurements and control inputs. It solves for the sequence of states within that window that best fits the data, subject to the system's dynamics model and constraints.

Core Characteristics:

• Explicit Constraint Handling: Can directly incorporate physical bounds on state variables (e.g., joint limits).
• Accuracy: Often more accurate than recursive filters for nonlinear systems, as it reconsiders past data.
• Computational Cost: Higher per-timestep cost than filters, as it requires solving an optimization problem, limiting its use to slower systems or as an offline refinement tool.

The Complementary Filter is a simple, lightweight frequency-domain sensor fusion technique. It combines two sensors measuring the same quantity but with complementary noise characteristics: one accurate at low frequencies (e.g., accelerometer for attitude) and one accurate at high frequencies (e.g., gyroscope for angular rate).

How it works: A high-pass filter is applied to the high-frequency sensor data, and a low-pass filter is applied to the low-frequency sensor data. The results are summed to produce a full-bandwidth estimate.

Use Case: It is extremely common in lightweight attitude and heading reference systems (AHRS) for drones and mobile robots due to its low computational overhead and effectiveness in fusing IMU data.

The Kalman Filter is an optimal recursive algorithm for estimating the state of a linear dynamic system from a series of noisy measurements. It operates in a two-step predict-update cycle:

• Predict: Projects the current state estimate forward in time using the system's dynamic model.
• Update: Corrects the prediction with a new measurement, weighting the correction based on the estimated uncertainty (covariance) of the prediction versus the measurement. It is the workhorse for sensor fusion, combining data from sources like IMUs and GPS for tasks like robot localization.

The Extended Kalman Filter (EKF) is the non-linear version of the Kalman Filter. It linearizes the system's non-linear dynamics and measurement models around the current state estimate at each time step using a first-order Taylor expansion (the Jacobian).

• Core Use: Essential for robotics where motion and sensor models are inherently non-linear (e.g., wheeled robot kinematics, camera projection models).
• Limitation: The linearization can introduce significant error if the system is highly non-linear or the estimate is poor, potentially leading to filter divergence.

A Particle Filter is a sequential Monte Carlo method used for state estimation in non-linear, non-Gaussian systems. It represents the state's probability distribution using a set of random samples called particles.

• Process: Each particle is a hypothesis of the true state. They are propagated through the system model, then weighted according to how well they explain the latest sensor observation. Low-weight particles are resampled.
• Key Application: Monte Carlo Localization (MCL) for robots in environments with ambiguous sensor data, where the belief is multi-modal (e.g., the robot could be in one of several identical corridors).

Sensor Fusion is the process of combining sensory data from disparate sources to produce information that is more accurate, complete, and reliable than that provided by any individual sensor. It is the primary engineering goal of state estimation.

• Types: Includes complementary fusion (e.g., fusing high-frequency, drift-prone IMU data with low-frequency, absolute GPS data) and competitive fusion (e.g., fusing data from multiple cameras).
• Algorithms: Implemented using filters (Kalman, Particle) or optimization-based methods. It directly addresses observation space mismatch by providing a unified, robust state estimate from heterogeneous real-world sensors.

SLAM 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 joint state estimation problem where the state includes both the robot's pose and the positions of landmarks in the map.

• Front-end: Handles sensor data association (e.g., feature matching between lidar scans).
• Back-end: Performs state estimation via non-linear optimization (e.g., Graph SLAM) or filtering to find the most likely map and robot trajectory given all observations.

Uncertainty Quantification is the rigorous assessment of the confidence or error bounds associated with a model's predictions or a system's state estimate. In state estimation, it is represented by a covariance matrix (in Kalman filters) or a particle cloud distribution (in particle filters).

• Critical for Sim-to-Real: Provides a measure of when the system is operating outside its trained domain or when sensor data is unreliable.
• Enables Safety: High uncertainty estimates can trigger fallback behaviors or request human intervention, forming a core part of safety constraints in autonomous systems.