State Estimation

A compact, robotics-friendly C++ project that implements:

• Bayes filter recursion (conceptual baseline)
• Kalman filter (KF)
• Extended Kalman filter (EKF)
• Unscented Kalman filter (UKF)
• Particle filter (PF)
• Generalized Gaussian filter (GGF) implemented as a Gauss–Hermite quadrature Gaussian filter (a.k.a. Gauss–Hermite Kalman filter)

The code is intentionally minimal, but designed so you can drop in your own process/measurement models (e.g., IMU preintegration, leg odometry, SE(3) pose, etc.).

Build

cmake -S . -B build -DSTATE_ESTIMATION_BUILD_EXAMPLES=ON -DSTATE_ESTIMATION_BUILD_TESTS=ON
cmake --build build -j
ctest --test-dir build

Run examples:

./build/example_linear_cv
./build/example_pendulum

Architecture

• models/ProcessModel.hpp: define x_{k+1} = f(x_k, u_k, dt) + w, with Q
• models/MeasurementModel.hpp: define z_k = h(x_k) + v, with R
• filters/*: KF, EKF, UKF, PF, and GGF
• transforms/*: moment transforms used by Gaussian filters (e.g., UT, Gauss–Hermite)

Gaussian filters as a family

EKF, UKF, and Gauss–Hermite GGF are all instances of Gaussian filtering: represent belief as (mean,cov) and approximate the necessary expectations to propagate through nonlinear f and h.

This repo exposes that idea via GaussianFilter + MomentTransform:

• UnscentedTransform → UKF
• GaussHermiteTransform → GGF

Notes for robotics extensions

1. Manifold states (SO(3), SE(3)): replace Vector with a state container supporting boxplus/boxminus and use an error-state formulation.
2. Square-root filters: swap covariance update for numerically robust square-root forms.
3. High-dimensional GGF: Gauss–Hermite grows as order^n; for large n, prefer UKF or sparse quadrature.