State Estimation

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.

• Prediction: Projects the current state estimate forward in time using the system's linear model.
• Update: Corrects the prediction with a new measurement, weighting the correction based on the estimated uncertainty (covariance).

It is optimal for linear Gaussian systems, minimizing the mean squared error of the estimates. It's a cornerstone for inertial navigation systems and basic sensor fusion.

The Extended Kalman Filter is the de facto standard for state estimation in nonlinear systems. It linearizes the system's nonlinear dynamics and measurement models around the current state estimate at each timestep.

• Local Linearization: Uses the Jacobian matrix (first-order partial derivatives) of the nonlinear functions.
• Suboptimal but Effective: While not optimal like the standard KF, it provides a practical and computationally tractable solution for many robotics problems, such as visual-inertial odometry and GPS/IMU fusion for vehicle localization.

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.

• Sigma Points: A small set of carefully chosen sample points (sigma points) are propagated through the true nonlinear functions.
• Captures Higher-Order Moments: This approach often provides more accurate estimates of the mean and covariance than the EKF's first-order approximation, especially for highly nonlinear systems. It is widely used in attitude estimation and target tracking.

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

• Bayesian Filtering: Each particle has a state hypothesis and an associated weight. The algorithm propagates, weights, and resamples these particles based on incoming measurements.
• Handles Multi-Modality: Unlike Kalman-based filters, it can represent complex, multi-modal distributions, making it ideal for global localization (the 'kidnapped robot' problem) and simultaneous localization and mapping (SLAM) in ambiguous environments.

Moving Horizon Estimation formulates state estimation as a constrained optimization problem solved over a sliding window of the most recent measurements. It explicitly handles state and measurement constraints.

• Finite Horizon Optimization: At each timestep, it solves for the sequence of states over a fixed past window that best fits the measurements, subject to the system model and known bounds (e.g., joint limits).
• Handles Constraints & Nonlinearities: By directly using the nonlinear model, MHE can be more accurate than recursive filters for complex systems and is robust to outlier measurements. It is computationally intensive but used in high-performance process control and advanced robotics.

The Complementary Filter is a simple, frequency-domain sensor fusion technique commonly used for attitude and heading estimation. It combines high-frequency data from one sensor (e.g., gyroscope) with low-frequency data from another (e.g., accelerometer, magnetometer).

• High-Pass & Low-Pass: The gyro's integrated angle (which drifts) is high-pass filtered, while the angle derived from gravity/magnetic field (which is noisy but absolute) is low-pass filtered. The outputs are summed.
• Computationally Efficient: It requires minimal computation compared to Kalman filters, making it ideal for resource-constrained embedded systems like drones and wearable devices. It's a foundational concept for sensor fusion.

System identification is the process of constructing mathematical models of a dynamic system from measured input-output data. It focuses on learning the structure and parameters of the system's governing equations.

• Contrast with State Estimation: While state estimation infers the current internal variables (state) of a known model, system identification learns the model itself from data.
• Sequential Relationship: Accurate system identification often provides the dynamic model required for high-performance state estimation (e.g., in a Kalman filter).
• Example: Using torque commands and resulting joint angle measurements from a robot arm to estimate its mass, inertia, and friction coefficients.

Observability is a fundamental system property that determines whether the internal state of a dynamic system can be uniquely reconstructed from a finite sequence of its external outputs (sensor measurements).

• Mathematical Condition: A system is observable if the observability matrix, constructed from its system matrices, has full column rank.
• Critical for Estimation: If a system is unobservable, no state estimator can reliably infer the full state. Engineers must redesign the sensor suite or excitation trajectory.
• Example: Estimating a car's position and velocity is observable with a GPS. Estimating both position and a constant sensor bias may require additional maneuvering to become observable.

The Kalman filter is an optimal recursive algorithm for state estimation in linear dynamic systems with Gaussian noise. It produces estimates that minimize the mean squared error.

• Two-Step Process: Prediction (propagates state using the model) and Update (corrects state using new sensor data).
• Widely Extended: Basis for the Extended Kalman Filter (EKF) for nonlinear systems and the Unscented Kalman Filter (UKF).
• Core Output: Provides not just a state estimate, but also a covariance matrix representing the uncertainty of that estimate.

Sensor fusion is the process of combining sensory data from disparate sources to produce estimates that are more accurate, complete, and reliable than those provided by any individual sensor.

• State Estimation as the Engine: Fusion is often implemented using a state estimator (like a Kalman filter) as the mathematical framework for combining data.
• Complementary Sensors: Fuses high-frequency, noisy sensors (e.g., IMUs) with low-frequency, accurate sensors (e.g., GPS) to achieve robust perception.
• Example: An autonomous vehicle fusing LiDAR, camera, radar, and GPS to estimate its precise pose and map its surroundings.

Model uncertainty quantifies the lack of perfect knowledge about a system's true dynamics, arising from simplifications, unmodeled effects, or inaccurate parameters in the state estimator's internal model.

• Primary Challenge: The discrepancy between the estimator's model and reality is a major source of estimation error and divergence.
• Mitigation Strategies: Use adaptive estimation, increase process noise covariance (Q matrix) in Kalman filters, or employ robust estimators like the H∞ filter.
• Direct Link: In sim-to-real, simulation bias and unmodeled dynamics are key contributors to model uncertainty during deployment.

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 canonical application of state estimation.

• Joint Estimation Problem: The state vector includes both the robot's pose (localization) and the positions of landmarks in the environment (mapping).
• Dependency: Accurate localization requires a good map, and accurate mapping requires good localization—solved via state estimation techniques.
• Algorithms: Modern solutions often use factor graphs and nonlinear optimization (Graph-based SLAM) or particle filters (FastSLAM).