Bayesian Filtering

The Kalman Filter is the optimal recursive estimator for linear dynamic systems with Gaussian noise. It operates in two steps: a prediction step that projects the state forward using a linear motion model, and an update step that corrects the prediction with a new measurement.

• Assumptions: Requires a known linear process model and measurement model.
• Optimality: Provides the minimum mean-square error estimate for linear Gaussian systems.
• Core Output: A Gaussian posterior distribution, fully characterized by a mean (state estimate) and a covariance matrix (uncertainty).
• Primary Use: Foundational algorithm for tracking and navigation where linearity is a valid approximation.

The Extended Kalman Filter is the de facto standard for nonlinear state estimation. It linearizes the nonlinear process and measurement models around the current state estimate using a first-order Taylor expansion (the Jacobian). It then applies the standard Kalman filter equations to this local linear approximation.

• Core Mechanism: Employs Jacobian matrices for linearization at each timestep.
• Limitation: Linearization errors can cause filter divergence if the system is highly nonlinear or the initial estimate is poor.
• Ubiquity: Historically the most widely used algorithm for robotic state estimation (e.g., Visual-Inertial Odometry).

The Unscented Kalman Filter addresses the linearization errors of the EKF by using a deterministic sampling approach called the unscented transform. It selects a minimal set of sigma points that capture the mean and covariance of the state distribution, propagates these points through the true nonlinear functions, and then recomputes the mean and covariance of the transformed points.

• Advantage over EKF: Provides more accurate estimation of the posterior mean and covariance for strong nonlinearities, without requiring Jacobian calculations.
• Computational Cost: Similar to the EKF, but often more robust.
• Typical Application: Attitude estimation, GPS-INS integration, and other systems with significant nonlinearities.

The Particle Filter is a sequential Monte Carlo method that represents the posterior probability distribution using a set of random samples called particles. Each particle is a hypothesis of the system's state with an associated weight. The algorithm involves prediction, weight update based on sensor likelihood, and resampling to avoid degeneracy.

• Key Strength: Makes no assumptions of linearity or Gaussian noise; can model arbitrary distributions.
• Computational Demand: Accuracy scales with the number of particles, making it more computationally intensive than Kalman-family filters.
• Classic Use Case: Monte Carlo Localization for robots, tracking in cluttered environments, and any problem with multi-modal distributions (e.g., global localization).

The Information Filter is the algebraic dual of the Kalman filter. Instead of representing the state estimate with a mean and covariance (the moment form), it uses an information vector and an information matrix (the inverse of the covariance matrix).

• Key Advantage: The update step is computationally simpler, as measurements are additive in the information form. This makes it efficient for sensor networks and decentralized fusion.
• Disadvantage: The prediction step becomes more complex.
• Primary Role: Foundational for multi-robot systems and distributed state estimation architectures.

The Error State Kalman Filter is a specialized variant designed for robust navigation, particularly attitude estimation. It maintains a nominal state (propagated with kinematics) and a separate, small error state (estimated by the filter). The error state is always kept close to zero, where linearization is valid.

• Core Benefit: Excellent numerical stability for orientation estimation using quaternions or rotation matrices, avoiding singularities and normalization issues.
• Common Framework: The standard implementation for modern Visual-Inertial Odometry (VIO) and LiDAR-Inertial Odometry (LIO) systems like FAST-LIO.
• Process: The filter estimates the error; this error is then used to correct the nominal state, which is then reset.

The Kalman filter is the optimal recursive solution to the Bayesian filtering problem for linear dynamic systems with Gaussian noise. It operates in a predict-update cycle:

• Predict: Projects the current state and its uncertainty forward using the process model.
• Update: Corrects the prediction with a new sensor measurement, weighting the prediction and observation based on their respective covariances. It is the workhorse algorithm for countless applications, from GPS navigation to financial forecasting, due to its computational efficiency and optimality under its assumptions.

A particle filter is a sequential Monte Carlo method that approximates the posterior state distribution using a set of random samples called particles. It is designed for systems that are highly nonlinear and/or have non-Gaussian noise.

• Each particle represents a hypothesis of the system's true state.
• Particles are propagated through the motion model and then weighted according to how well they explain the latest sensor measurement.
• A resampling step discards low-weight particles and duplicates high-weight ones. This approach is powerful but computationally intensive, making it suitable for problems like robotic localization in complex environments.

A factor graph is a bipartite graphical model used to represent the factorization of a complex probability distribution. It is the underlying data structure for modern graph-based optimization techniques in state estimation and SLAM.

• Variable Nodes: Represent unknown states to be estimated (e.g., robot poses, landmark positions).
• Factor Nodes: Represent probabilistic constraints between variables, derived from process models, sensor measurements, or prior knowledge. Algorithms like Gauss-Newton or Levenberg-Marquardt optimize over this graph to find the Maximum A Posteriori (MAP) estimate. Frameworks like g2o and GTSAM use this paradigm.

Maximum A Posteriori (MAP) estimation is the process of finding the state that maximizes the posterior probability P(state | measurements). It is the core objective of batch optimization techniques in robotics.

• It directly incorporates prior knowledge (the 'prior' in Bayesian terms).
• For Gaussian distributions, the MAP estimate often reduces to solving a non-linear least squares problem.
• Unlike recursive filters (e.g., Kalman), MAP estimation considers all measurements simultaneously, making it ideal for smoothing problems like full-trajectory estimation and bundle adjustment in computer vision.

The covariance matrix is a square, symmetric matrix that quantifies the uncertainty and correlations between all elements of an estimated state vector. It is central to probabilistic estimation.

• The diagonal elements represent the variance (uncertainty squared) of each state variable.
• The off-diagonal elements represent the covariance between two variables, indicating how they change together. In the Kalman filter, the covariance matrix is propagated through the predict step (uncertainty grows) and reduced in the update step (uncertainty shrinks). It is used to compute the Mahalanobis distance for outlier rejection.

These are the two fundamental mathematical models in any Bayesian filtering framework:

• Process Model: Also called the motion model or state transition model. It describes how the system state evolves over time: x_k = f(x_{k-1}, u_k, w_k). It incorporates control inputs u_k and process noise w_k.
• Measurement Model: Describes how sensor observations are generated from the true state: z_k = h(x_k, v_k). It maps the state to the expected sensor reading, including sensor noise v_k. The accuracy and differentiability (for EKF) of these models are the primary determinants of filter performance. They encode the physics and geometry of the system.