Kalman Filter Family — EKF, UKF & Particle Filter — Robotics Reference

Scope. The probability-theoretic derivations of the Bayes filter, the conjugate Gaussian update, the Riccati form of the steady-state Kalman gain, and the stochastic analysis of particle-filter weight degeneracy live in [[Engineering/state-space-methods]] §§9–12 and in the canonical Thrun/Burgard/Fox and Barfoot textbooks cited in §10. This note is the robotics-applied counterpart: which filter to choose for which problem, the $Q$ and $R$ numbers that survive real hardware, how to time-align multi-rate sensors, and the named systems (PX4 ekf2, Mars Perseverance, nav2_amcl) that show what production fusion stacks actually look like.

1. At a glance

Bayesian state estimation is the recursive probabilistic inference of robot state $x_t$ from noisy, partial, asynchronous measurements $z_{1:t}$. It is the layer between raw sensors ([[Robotics/sensors-pose-motion]], [[Robotics/sensors-perception]]) and every downstream consumer — feedback controllers ([[Robotics/state-space-lqr]], [[Robotics/pid-control]], [[Robotics/impedance-control]]), motion planners, behaviour layers, safety supervisors. Without it, a robot has measurements but no state.

Five forms dominate robotics:

• Kalman filter (KF, Kalman 1960) — linear-Gaussian. Exact, closed-form, the basis of everything that follows. Rare in pure form because robots are almost never linear.
• Extended Kalman filter (EKF) — local first-order linearisation of nonlinear motion/measurement models around the current mean. Workhorse of mobile robotics, AHRS, GNSS-INS fusion. Implemented in PX4 ekf2, ROS 2 robot_localization, ArduPilot AP_NavEKF3, Mars MER/MSL/Mars 2020 rover localisation.
• Unscented Kalman filter (UKF, Julier-Uhlmann 1997) — deterministic sigma-point sampling, captures mean and covariance through nonlinearity exactly to second order without computing Jacobians. Preferred when nonlinearities are strong but the posterior is still unimodal (high-rate attitude, agile flight).
• Particle filter (PF, Gordon-Salmond-Smith 1993; Dellaert-Fox-Burgard-Thrun 1999 for robotics MCL) — fully nonparametric. Samples track an arbitrary posterior, including multimodal “kidnapped robot” or “which room am I in?” hypotheses that no Gaussian filter can represent. Used in nav2_amcl for indoor localisation against a known map.
• Iterated EKF / factor-graph smoother (iSAM2, Kaess-Johannsson-Roberts-Ila-Leonard-Dellaert 2012; GTSAM) — re-linearises during the update step, or maintains the full joint over a sliding window of poses. The boundary between “filter” and “SLAM back-end”. The right answer when you want offline-quality estimates online and can afford the compute.

These five are the foundation for sensor fusion, SLAM ([[Robotics/slam]]), multi-target tracking, fault detection (consistency of innovations), and visual-inertial odometry. Every modern autonomous vehicle, every commercial drone, every advanced mobile robot has a member of this family running at 100–1000 Hz at its core.

First ask before applying: Is the posterior likely to be unimodal Gaussian? If yes (servoing, attitude tracking near a known initial condition, GNSS-aided INS in open sky) → start with EKF, escalate to UKF or iterated EKF if you see linearisation-error symptoms. Is it multimodal, or might it be after a sensor outage? (Indoor wheelchair, AGV that just teleported when someone moved it, room-scale AR tracker that lost line-of-sight) → particle filter, or a Gaussian-mixture filter as the middle ground. Do I have a fixed window of measurements I can afford to re-linearise over? → factor-graph smoother.

2. First principles

The Bayes filter recursion

The probabilistic state estimation problem: given a sequence of controls $u_{1:t}$ and measurements $z_{1:t}$, infer the posterior over state $x_t$:

$\mathrm{bel}(x_t)=p(x_t\mid z_{1:t},u_{1:t})$

The Bayes filter is the two-step recursion:

Predict (chain rule + Markov assumption on $x_t$ given $x_{t-1},u_t$):

$\overline{\mathrm{bel}}(x_t)=\int p(x_t\mid x_{t-1},u_t)\mathrm{bel}(x_{t-1})d{x}_{t-1}$

Update (Bayes’ rule + conditional independence of $z_t$ given $x_t$):

$\mathrm{bel}(x_t)=\eta p(z_t\mid x_t)\overline{\mathrm{bel}}(x_t)$

with normaliser $\eta =1/\int p(z_t\mid x_t)\overline{\mathrm{bel}}(x_t)d{x}_t$. Every filter in this note is a specific parametric representation of $\mathrm{bel}(x_t)$ that makes the two integrals tractable.

The Kalman filter — linear-Gaussian closed form

For the linear time-varying model

$x_{k+1}=A_k{x}_k+B_k{u}_k+w_k,w_k\sim \mathcal{N}(0,Q_k)$ $z_k=C_k{x}_k+v_k,v_k\sim \mathcal{N}(0,R_k)$

with $w_k,v_k$ independent and zero-mean, the posterior $\mathrm{bel}(x_k)$ remains Gaussian forever. Parameterise it by mean ${\hat{x}}_k$ and covariance $P_k$. The two-step recursion becomes:

Predict.

${\hat{x}}_{k|k-1}=A_{k-1}{\hat{x}}_{k-1|k-1}+B_{k-1}{u}_{k-1}$ $P_{k|k-1}=A_{k-1}{P}_{k-1|k-1}{A}_{k-1}^{\top }+Q_{k-1}$

Update. Innovation $y_k=z_k-C_k{\hat{x}}_{k|k-1}$, innovation covariance $S_k=C_k{P}_{k|k-1}{C}_k^{\top }+R_k$, Kalman gain

$K_k=P_{k|k-1}{C}_k^{\top }{S}_k^{-1}$

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{y}_k$

$P_{k|k}=(I-K_k{C}_k)P_{k|k-1}$

This is Kalman 1960’s contribution: the optimal MMSE estimator under the linear-Gaussian assumptions, computed recursively with $O(n^3)$ work per step. The dual to LQR is exact — see [[Robotics/state-space-lqr]] §2.

Extended Kalman filter — linearise around the mean

For the nonlinear model $x_{k+1}=f(x_k,u_k)+w_k$, $z_k=h(x_k)+v_k$, replace $A$ and $C$ in the Kalman equations by Jacobians evaluated at the current mean estimate:

$F_k={\frac{\partial f}{\partial x}|}_{{\hat{x}}_{k|k},u_k},H_k={\frac{\partial h}{\partial x}|}_{{\hat{x}}_{k|k-1}}$

Mean propagates through the nonlinear $f$ and $h$ themselves; covariance propagates through the Jacobians. This is an approximation — second-order Taylor terms in the actual posterior are dropped. The approximation is excellent when the function is nearly linear over the $1\sigma$ width of the prior and poor when the function curves on that scale (e.g., bearing-only tracking, large attitude rates, or any system traversing a singularity).

Unscented Kalman filter — sigma-point propagation

Rather than linearising, Julier-Uhlmann 1997 propagates a deterministic set of $2n+1$ sigma points through the nonlinear $f$ and $h$, then reconstructs the mean and covariance from the transformed points by weighted averaging:

${\mathcal{X}}^{(0)}=\hat{x},{\mathcal{X}}^{(i)}=\hat{x}\pm {\left(\sqrt{(n+\lambda )P}\right)}_i,i=1,\dots ,n$

with $\lambda ={\alpha }^2(n+\kappa )-n$, conventionally $\alpha =10^{-3}$, $\kappa =0$, $\beta =2$. Propagated points ${\mathcal{Y}}^{(i)}=f({\mathcal{X}}^{(i)},u)$ are averaged with weights $W_m^{(i)}$ for the mean and $W_c^{(i)}$ for the covariance (Wan-van der Merwe 2001). The result is exact to second-order Taylor for any nonlinearity, third order for symmetric noise, with no Jacobian computation. Cost is roughly $3\times$ EKF; pays off when Jacobians are awkward to derive (quaternion-vector compositions, projective camera models, SO(3) on the manifold).

Particle filter — Monte Carlo on the Bayes filter

Represent $\mathrm{bel}(x_t)$ by a set of $N$ weighted samples $\{(x_t^{(i)},w_t^{(i)}){\}}_{i=1}^N$. The recursion becomes:

1. Sample motion model. $x_t^{(i)}\sim p(x_t\mid x_{t-1}^{(i)},u_t)$ — push each particle through a stochastic forward model that includes process noise.
2. Re-weight by measurement likelihood. $w_t^{(i)}\propto w_{t-1}^{(i)}\cdot p(z_t\mid x_t^{(i)})$.
3. Resample with replacement proportional to weight, every step or every few steps (resampling itself injects sample-variance noise; resample only when the effective sample size $N_{\text{eff}}=1/\sum (w^{(i)}{)}^2$ drops below $N/2$, Arulampalam-Maskell-Gordon-Clapp 2002).

The PF has no parametric assumption on the posterior; it represents multimodal, skewed, or hard-edged densities (e.g., “I am somewhere in this rectangular hallway with uniform probability”) that no Gaussian filter can approximate. The cost is $N\cdot$ (likelihood evaluation), typically with $N\in [200,10000]$ for robotic localisation; the likelihood for laser-scan-against-occupancy-grid evaluation dominates total compute.

Information / inverse filter

Parameterise by the information vector $\xi =P^{-1}\hat{x}$ and information matrix $\Lambda =P^{-1}$ instead of $\hat{x},P$. The update step then becomes additive: ${\Lambda }^{+}={\Lambda }^{-}+H^{\top }{R}^{-1}H$ and ${\xi }^{+}={\xi }^{-}+H^{\top }{R}^{-1}z$, while the predict step becomes the costly one. This form is preferred when many independent low-rank measurements arrive per step (range-only beacons, visual feature observations), and is the natural representation inside factor-graph smoothers like GTSAM/iSAM2.

The sparse information matrix is the key to scaling SLAM to thousands of landmarks: the off-diagonals of $\Lambda$ are non-zero only between states with a direct measurement coupling them, while in $P$ they fill in densely. iSAM2 exploits this sparsity via the Bayes tree (Kaess-Johannsson-Roberts-Ila-Leonard-Dellaert 2012) for incremental updates in $O(\log N)$ per loop closure.

Sigma-point weights for the UKF — implementation detail

For state dimension $n$, the conventional Julier-Uhlmann scaling with parameters $\alpha =10^{-3}$, $\kappa =0$, $\beta =2$ (Gaussian-optimal):

$\lambda ={\alpha }^2(n+\kappa )-n$

with weights $W_m^{(0)}=\lambda /(n+\lambda )$, $W_c^{(0)}=W_m^{(0)}+1-{\alpha }^2+\beta$, and $W_m^{(i)}=W_c^{(i)}=1/(2(n+\lambda ))$ for $i\ne 0$. For $n=6$ at $\alpha =10^{-3}$, $\lambda \approx -6$, $W_m^{(0)}\approx -10^6$ — the central sigma point gets a negative weight in the covariance reconstruction. This is correct (it’s how the higher-order terms cancel) but a numerical implementation can produce a non-positive-definite covariance if the propagated sigma points are too tightly clustered. Fix: use the scaled unscented transform (Julier 2002) with $\alpha \in [0.1,1]$ for better numerical behaviour at the cost of slightly worse high-order error capture; or use the cubature Kalman filter (Arasaratnam-Haykin 2009), which fixes the weights to $1/(2n)$ for all $2n$ sigma points and skips the central point entirely — cleaner numerics, third-order exact for symmetric Gaussian.

Iterated EKF and factor graphs

The standard EKF linearises $h$ around the predicted mean ${\hat{x}}_{k|k-1}$. If the measurement is informative enough to move the estimate significantly, that linearisation point is wrong. The iterated EKF (Bell-Cathey 1993) re-linearises around the current updated estimate and repeats the update step until convergence, recovering most of the gap to the maximum-a-posteriori solution. A factor-graph smoother does this over a sliding window of poses and landmarks — see Dellaert-Kaess 2017 and [[Robotics/slam]].

Quaternion state on a manifold — the multiplicative EKF (MEKF)

Quaternions live on the unit 3-sphere $S^3$, not in ${\mathbb{R}}^4$. Adding a Kalman correction $\delta q$ directly to $q$ ($q^{+}=q+Ky$) is wrong on two counts: it breaks the unit-norm constraint, and it represents the error as a 4-vector when the manifold is 3-dimensional. Lefferts-Markley-Shuster 1982 introduced the multiplicative form: keep the state $q$ on the manifold, parameterise the error as a 3-vector $\delta \theta$ in the tangent space (“small rotation about $q$”), run the Kalman update on $\delta \theta$, then multiplicatively apply the correction $q^{+}=q\otimes \delta q(\delta \theta )$ and reset $\delta \theta \to 0$. This is the standard form in every modern AHRS (PX4 ekf2, ROS 2 imu_filter_madgwick+EKF, VectorNav firmware) and the foundation of Sola 2017’s “error-state Kalman filter” tutorial.

The error-state recursion for IMU-driven attitude:

$\delta {\theta }_{k+1}=\mathrm{Exp}(-\omega \Delta t)\cdot \delta {\theta }_k+n_g\Delta t$

with $n_g$ the gyro white noise. The covariance prediction in the tangent space is the standard $FP{F}^{\top }+Q$ with $F=\mathrm{Exp}(-\omega \Delta t)$ replacing the identity. After the update, reset the error to zero and absorb it multiplicatively into the nominal state — this is what distinguishes the error-state form from a regular EKF on a 3-vector representation of rotation.

Complementary filter — the linear-Gaussian limit at the boundary

Before the full EKF machinery, robotics still uses the complementary filter (Higgins 1975; Mahony-Hamel-Pflimlin 2008 for SO(3)) for AHRS in tight-budget hardware: combine a high-pass-filtered gyro (good short-term, drifts long-term) with a low-pass-filtered accelerometer + magnetometer (noisy short-term, drift-free long-term):

${\hat{q}}^{+}=(1-\alpha )\cdot {\hat{q}}_{\text{gyro-int}}+\alpha \cdot {\hat{q}}_{\text{from-accel-mag}}$

with $\alpha \in [0.001,0.05]$ chosen as the gyro-trust time constant. ROS 2’s imu_filter_madgwick and imu_complementary_filter implement variants of this; they are the linear-Gaussian limit of a 2-channel Kalman filter with fixed $Q/R$ ratio — equivalent in steady state, simpler in code, no online tuning. When you don’t need bias estimation and the loop runs at IMU rate, the complementary filter is often sufficient.

3. Practical math & worked examples

Example A — 2D mobile robot EKF with wheel odometry + GPS

State $x=[x,y,\theta ,v{]}^{\top }$ (position in metres, heading in radians, body speed in m/s). Control input $u=[a,\omega {]}^{\top }$ (longitudinal acceleration and yaw rate, measured by IMU at 100 Hz). External aiding: GPS at 1 Hz reporting $z=[x_{\text{GPS}},y_{\text{GPS}}{]}^{\top }$.

Motion model (constant-turn-rate, constant-acceleration, $\Delta t=0.01$ s):

$f(x,u)=\left[\begin{array}{c}x+v\cos \theta \cdot \Delta t\\ y+v\sin \theta \cdot \Delta t\\ \theta +\omega \cdot \Delta t\\ v+a\cdot \Delta t\end{array}\right]$

Motion Jacobian $F_k=\partial f/\partial x$:

$F_k=\left[\begin{array}{cccc}1& 0& -v\sin \theta \cdot \Delta t& \cos \theta \cdot \Delta t\\ 0& 1& v\cos \theta \cdot \Delta t& \sin \theta \cdot \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

Process noise (per $\Delta t=0.01$ s step), derived from the IMU noise specs of a Bosch BMI088 ([[Robotics/sensors-pose-motion]] §6):

$Q=\mathrm{diag}\left(10^{-6},10^{-6},10^{-5},10^{-3}\right){\text{(m}}^2{\text{, m}}^2{\text{, rad}}^2{\text{, m}}^2/{\text{s}}^2\text{)}$

GPS measurement model $h(x)=[x,y{]}^{\top }$, so $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$. Consumer GPS measurement covariance: $R=\mathrm{diag}(2.5,2.5)$ m² (≈ 1.6 m CEP, typical of $u$-blox NEO-M9N in clear sky).

Update on a GPS fix. Innovation covariance $S=HP{H}^{\top }+R$, gain $K=P{H}^{\top }{S}^{-1}$. Notice that the GPS update directly reduces variance in $x,y$ but only indirectly improves $\theta$ and $v$ — by the cross-covariance $P_{x\theta }$ that has grown during the prediction phase. This is the entire reason GPS-INS fusion works: dead-reckoning correlates heading and speed errors with position error, and a position fix observes the correlated states for free.

After 10 s with the robot moving at 1.5 m/s in a straight line: starting from a fresh-fix prior $P_0=\mathrm{diag}(2.5,2.5,0.05,0.01)$, position variance at 11th fix has been driven back below $P_0$, heading variance has decreased from 0.05 rad² to ≈ 0.008 rad² thanks to the path observability. Stop the robot and the heading observability collapses — a stationary robot’s heading is unobservable from GPS alone. ([[Robotics/sensors-pose-motion]] §3 example B has the IMU-only side of this story.)

Reference implementation (filterpy-style, ~30 lines):

import numpy as np
from scipy.linalg import block_diag
 
dt = 0.01
def f(x, u):
    p_x, p_y, th, v = x; a, w = u
    return np.array([p_x + v*np.cos(th)*dt,
                     p_y + v*np.sin(th)*dt,
                     th + w*dt,
                     v + a*dt])
def F(x, u):
    p_x, p_y, th, v = x
    return np.array([[1, 0, -v*np.sin(th)*dt, np.cos(th)*dt],
                     [0, 1,  v*np.cos(th)*dt, np.sin(th)*dt],
                     [0, 0, 1, 0],
                     [0, 0, 0, 1]])
H_gps = np.array([[1,0,0,0],[0,1,0,0]])
Q = np.diag([1e-6, 1e-6, 1e-5, 1e-3])
R_gps = np.diag([2.5, 2.5])
 
x_hat = np.zeros(4); P = np.diag([100,100,1,4])
# predict
x_hat = f(x_hat, u_k);  Fk = F(x_hat, u_k)
P = Fk @ P @ Fk.T + Q
# update on GPS
y = z_gps - H_gps @ x_hat
S = H_gps @ P @ H_gps.T + R_gps
K = P @ H_gps.T @ np.linalg.inv(S)
x_hat = x_hat + K @ y
P = (np.eye(4) - K @ H_gps) @ P @ (np.eye(4) - K @ H_gps).T + K @ R_gps @ K.T  # Joseph

The Joseph form on the last line is essential — the textbook $(I-KH)P$ update silently loses positive-definiteness within minutes of float32 operation, then the next Cholesky factorisation in $S^{-1}$ blows up. This is the single most common failure mode in production EKF code.

Example B — Drone attitude UKF with MEKF state

Quadrotor attitude estimation from a 9-DOF IMU. State

$x=[q_w,q_x,q_y,q_z,b_{g,x},b_{g,y},b_{g,z}{]}^{\top }\in {\mathbb{R}}^7$

with the body-to-nav quaternion $q$ and gyro biases $b_g$. The error state is $\delta x=[\delta {\theta }_x,\delta {\theta }_y,\delta {\theta }_z,\delta b_{g,x},\delta b_{g,y},\delta b_{g,z}{]}^{\top }\in {\mathbb{R}}^6$ — the error covariance $P\in {\mathbb{R}}^{6\times 6}$ acts on this 6-vector tangent space, not on the 7-vector state directly.

Gyro at 200 Hz drives the prediction:

$q^{+}=q\otimes \mathrm{Exp}(\tilde{\omega }\cdot \Delta t),\tilde{\omega }={\omega }_{\text{meas}}-b_g$

with the quaternion exponential $\mathrm{Exp}(\varphi )=[\cos (|\varphi |/2),(\varphi /|\varphi |)\sin (|\varphi |/2)]$.

The accelerometer at 100 Hz provides the gravity-direction measurement: $a_{\text{meas}}=R(q{)}^{\top }\cdot [0,0,-g{]}^{\top }+v_a$ (when the drone is not accelerating). The magnetometer measures the local Earth field projected into the body frame.

Sigma points ($n=6$, $2n+1=13$ points) are constructed in the error space:

$\delta {\mathcal{X}}^{(0)}=0,\delta {\mathcal{X}}^{(i)}=\pm {\left(\sqrt{(n+\lambda )P}\right)}_i$

then “exp-mapped” onto the manifold by composing with the current $\hat{q}$. Propagating each through the nonlinear gyro integration over $\Delta t$ and re-fitting a mean and covariance on $S^3$ uses the quaternion mean of Markley 2007 (eigenvector of the weighted outer-product matrix). The result: the UKF tracks attitude through rate transients of 500°/s without the linearisation error that drags an EKF off the manifold and forces a renormalise-and-pretend recovery.

In typical numbers (PX4 ekf2 vs Adriaens-De Schutter 2020 UKF comparison on a Bebop 2 dataset): EKF attitude error 0.6° (1σ) at hover, 4.2° during a 360°/s yaw rotation; UKF 0.5° hover, 0.9° during the rotation.

Accelerometer update under the no-acceleration assumption:

$z_{\text{acc}}=R(q{)}^{\top }\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]+v_a,v_a\sim \mathcal{N}(0,R_a)$

The “no acceleration” gate uses $||a_{\text{meas}}|-g|<0.5{\text{m/s}}^2$ — if the drone is accelerating, the accelerometer is no longer a clean gravity measurement and the update is skipped (not just down-weighted). PX4 ekf2 uses exactly this gate, with a hysteresis to avoid chattering. UKF handles this gracefully — sigma points span the actual rotation; the predicted measurement is the weighted mean of $R(q^{(i)}{)}^{\top }g$ across the 13 propagated points, not the linearised value at the mean.

Magnetometer update. Earth field $m_{\text{nav}}=[m_N,0,m_D]$ in NED (NED-Z is down, magnetic-inclination angle determines the ratio). The measurement is $z_m=R(q{)}^{\top }{m}_{\text{nav}}+b_m+v_m$ with body-mounted bias $b_m$ in the state. The hard-iron bias must be in the state (otherwise the filter chases the rotor-magnet field as if it were heading change).

Example C — Particle filter for indoor localisation (MCL)

nav2_amcl and its amcl predecessor implement Adaptive Monte Carlo Localisation (Fox 2003) for differential-drive and omnidirectional bases against a known 2D occupancy grid map.

Setup. $N=5000$ particles, each $x^{(i)}=[x_i,y_i,{\theta }_i]$ in the map frame. Initial distribution: either uniform across the free space (global localisation) or Gaussian around a user-specified pose (pose tracking).

Motion update. Each particle is advanced by the odometry increment $\Delta x_{\text{odom}}$ from the wheel encoders, with added Gaussian noise per the four-parameter motion model of Thrun-Burgard-Fox ch. 5 (forward noise as fraction of forward translation, rotation-from-translation, translation-from-rotation, rotation-from-rotation). Typical values: ${\alpha }_1=0.05$ rad/m, ${\alpha }_2=0.05$ rad/rad, etc., tuned per platform.

Measurement update — likelihood field. For each LiDAR beam endpoint at $(z_x,z_y)$ in the particle’s hypothetical frame, look up the distance to nearest obstacle $d$ in the precomputed likelihood field (a Gaussian-blurred occupancy grid). The beam likelihood is

$p(z_k\mid x^{(i)},m)=z_{\text{hit}}\cdot \mathcal{N}(d;0,{\sigma }_{\text{hit}}^2)+z_{\text{rand}}/z_{\text{max}}$

with the second term capturing unmodelled obstacles (people, dynamic clutter). Multiply across beams (assume independence — wrong but cheap) and that becomes the particle’s weight contribution. With a 360-beam LiDAR at 10 Hz, that’s $5000\times 360=1.8\times 10^6$ field lookups per scan, finished in ≈ 30 ms on a Cortex-A72.

Resampling. Use low-variance sampling (Thrun-Burgard-Fox ch. 4): draw a single uniform offset and step through the cumulative weight array with stride $1/N$. This guarantees that a particle with weight $\ge 1/N$ is sampled at least once, minimising weight-variance noise.

KLD-sampling adaptation. Adjust $N$ at each step so that the posterior bound on KL divergence from the true distribution stays below $ϵ$. Global localisation can start with $N=100000$; once the variance collapses around a single mode, drop to $N=200$ to save compute (Fox 2003).

Convergence. Global indoor localisation in a typical office (50 × 30 m floor plan) converges to a single mode in 8–15 s of driving with 0.3 m/s forward velocity, 0.5 rad/s rotation — driven by the information content of corners and doorways, which are the features that disambiguate the global pose.

Beam vs likelihood-field model. The original Thrun beam model evaluates each ray by ray-casting through the occupancy grid to the expected return distance, mixing four components: Gaussian-hit at correct range, exponential-short for unexpected obstacles, uniform-random for sensor failures, and a delta-mass at $z_{\text{max}}$ for missing returns. The likelihood-field model precomputes a 2D Gaussian-blur of the obstacle map and looks up each beam endpoint directly — cheaper, smoother (no discrete jumps when beams cross occupancy cells), and the default in nav2_amcl. The trade-off: likelihood-field cannot distinguish “obstacle behind expected obstacle” from “obstacle at expected range”, but this only matters for transparent / mirror surfaces where neither model copes anyway.

Example D — Tightly-coupled GNSS-INS for an autonomous vehicle

State $x\in {\mathbb{R}}^{16}$: NED position (3), NED velocity (3), error-quaternion (3 in tangent space), gyro bias (3), accel bias (3), receiver clock bias and drift (1+1). At each GNSS epoch (5 Hz), the filter receives a set of $N_{\text{sat}}$ pseudoranges ${\rho }_i=\Vert p_{\text{rx}}-p_{\text{sat}}^{(i)}\Vert +c\cdot \delta t_{\text{rx}}+n_i$ and Doppler observations ${\dot{\rho }}_i$. Each pseudorange is a measurement update of position + clock bias; each Doppler is an update of velocity + clock drift. Even with only three satellites visible (insufficient for a stand-alone GNSS fix), the filter still converges because the inertial mechanisation provides the missing dimensions.

This is the principle behind NovAtel SPAN, Septentrio AsteRx-i, and most automotive ADAS GNSS-INS units: in an urban canyon where 3–4 of the constellation’s visible satellites are blocked, a loosely-coupled receiver outputs “no fix”; the tightly-coupled EKF outputs a slightly larger covariance and keeps going.

Example E — Range-only beacon localisation with iterated EKF

Three UWB anchors at known positions $b_i$. The robot measures range ${\rho }_i=\Vert p_{\text{robot}}-b_i\Vert +v_i$ with $v_i\sim \mathcal{N}(0,0.01^2)$ m² (10 cm 1σ, typical of a Decawave DW1000 / Qorvo DWM3000 chip in line-of-sight). State $x=[p_x,p_y,p_z]$, three measurements per epoch.

The measurement Jacobian per anchor is $H_i=(p-b_i{)}^{\top }/\Vert p-b_i\Vert$ — a unit-vector pointing from anchor to robot. The EKF linearises this at the predicted position; if the prediction is 1 m off and the anchor is 3 m away, the unit-vector is wrong by ~20°, and the resulting position update lands at the wrong place.

Iterated EKF: after the first update, re-evaluate $H$ at the updated position, recompute the gain, and update again. Typically 2–4 iterations converge to within 1e-6 m of the MAP estimate; the un-iterated EKF in the same setup carries a 5–15 cm steady-state bias on aggressive starting offsets. This is exactly the scenario Bell-Cathey 1993 analysed: highly nonlinear $h$ with informative measurements is where iteration pays off.

Example F — Steady-state Kalman gain for a 1D constant-velocity tracker

For the simplest non-trivial Kalman filter — a 1D constant-velocity model with position measurement only — the steady-state $P_{\infty }$ and gain $K_{\infty }$ have a closed-form. Let $x=[p,v{]}^{\top }$, $F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$, $H=[1,0]$, $Q=\mathrm{diag}(0,q\Delta t)$ ($q$ = velocity-noise power spectral density), $R=r$.

Define the dimensionless manoeuvre index $\lambda =q\Delta t^4/r$. Then (Bar-Shalom-Li-Kirubarajan eq. 6.5.1-7):

$K_{\infty }=\left[\begin{array}{c}\alpha \\ \beta /\Delta t\end{array}\right],\alpha =1-{\sigma }^2,\beta =(1-\sigma {)}^2$

where $\sigma$ is the smaller root of a quartic in $\sigma$ parameterised by $\lambda$. For $\lambda =1$ (well-tuned tracker), $\alpha \approx 0.65$, $\beta \approx 0.21$ — exactly the $\alpha$-$\beta$ filter the radar community has used since the 1950s. The Kalman filter is the optimal $\alpha$-$\beta$ filter for known $\lambda$.

This is also the limit your “production EKF for 1D position tracking” converges to after the transient. If you find yourself building a constant-velocity tracker, you can hard-code the gain from $\lambda$ and skip the Riccati recursion entirely — useful on Cortex-M0 where every divide costs.

4. Design heuristics

Five-question screen before committing to a filter

1. Is the state observable from the available sensors? Compute the observability matrix $\mathcal{O}=[H,HF,H{F}^2,\dots ,H{F}^{n-1}{]}^{\top }$ at a representative operating point; rank should equal $n$. If not, either add a sensor or remove unobservable states from the model. A 2D mobile robot with only GPS has heading unobservable when stationary — accept this and never let the filter assert tight $P$ on $\theta$ at rest.
2. What’s the loop rate of the consumer? A 1 kHz control loop needs filtered state at 1 kHz; a 5 Hz path planner doesn’t. Don’t over-engineer for a consumer that doesn’t care.
3. What’s the worst-case innovation magnitude? If 99 % of innovations are < 1σ and the worst is 10σ (outlier), gating + robust kernels are essential. If you have no outliers (lab conditions), skip them and save the compute.
4. How quickly must I recover from a kidnap or sensor failure? Local-pose-tracking filters fail catastrophically if the user picks up and moves the robot. PFs recover in 5–10 s; EKFs need explicit reset logic.
5. What’s the offline compute budget for tuning? Some filters (PF with thousands of particles, iEKF with many iterations) take significant tuning effort to make work; if you have to ship in a week, an EKF with conservative $Q$ is the safer choice.

Choosing the filter by problem

Problem class	Recommended filter	Why
Truly linear, Gaussian noise (rare)	KF	Exact, fastest.
Mild nonlinearity, unimodal, fast loop (≥ 100 Hz)	EKF	Industry default.
Strong nonlinearity, unimodal, awkward Jacobians	UKF	Sigma-point exactness without symbolic differentiation.
Highly informative update vs prior (vision, range)	Iterated EKF	Relinearise to recover MAP.
Multimodal posterior (kidnapped robot, global localisation)	PF	Only nonparametric option that scales to real-time.
Slow process, many measurements, offline-quality online	Factor-graph smoother (iSAM2)	Re-linearise across a window; output equals batch MAP.
Tracking with data association	JPDAF, MHT, GM-PHD ([[Robotics/sensors-perception]] §7)	Multiple-target machinery on top of any of the above.

Bayesian-filter design checklist (one-pager)

Before writing any filter code:

1. State vector $x$ — minimal but observable; manifold-aware for orientation states.
2. Motion model $f(x,u)$ — physical derivation, not curve-fit; biases as random walks.
3. Process noise $Q$ — from un-modelled accelerations / rate-random-walk specs.
4. Measurement model $h(x)$ for each sensor — proper coordinate frame, sign convention, latency.
5. Measurement noise $R$ — from datasheet, validated against Allan variance for IMU.
6. Initialisation ${\hat{x}}_0,P_0$ — large $P_0$ on unknowns, conservative on knowns.
7. Gating — Mahalanobis with 99 % ${\chi }^2$ threshold by default.
8. Numerical form — Joseph update minimum; UD or square-root for embedded float32.
9. Diagnostics — log NIS, innovation, gain norm, $P$ eigenvalues for every step.
10. Failover — what to do when the filter is rejected by its own consistency tests.

Skipping any one of these is the proximate cause of about half the production EKF bugs filed against PX4, ArduPilot, and robot_localization over the past decade.

Tuning Q and R

Both noise covariances must be set to numbers that match physical reality. The two failure modes are symmetric:

• $R$ too small (over-trusting sensor) → filter chases measurement noise; estimate is jittery, innovation is whitened but spectrum-flat.
• $R$ too large → filter ignores sensor; estimate lags reality.
• $Q$ too small → filter over-trusts model; covariance shrinks too fast; updates have no leverage; eventually diverges as un-modelled dynamics accumulate (“filter divergence by over-confidence”).
• $Q$ too large → filter never tightens; estimate is noisy in steady state.

Procedure.

1. Start with the sensor datasheet. Set $R$ entries to the published standard deviation squared. IMU specs ([[Robotics/sensors-pose-motion]] §2): for an ADIS16505, ARW = 0.15°/√hr translates to a continuous-time gyro noise PSD of $(0.15\cdot \pi /180/60{)}^2=6.9\times 10^{-9}$ rad²/s, discretised as $R=\text{PSD}/\Delta t$ for the $\Delta t$ of your filter.
2. Set $Q$ entries from physical un-modelled accelerations. A wheeled robot whose dynamic model assumes constant velocity should have $Q$ in the velocity entry equal to $({\sigma }_a\cdot \Delta t{)}^2$, where ${\sigma }_a$ is the largest “unexpected” acceleration the robot can experience.
3. Run NIS / innovation tests. The Normalised Innovation Squared ${\nu }_k=y_k^{\top }{S}_k^{-1}{y}_k$ should be chi-squared distributed with $\dim z$ degrees of freedom. The time-averaged NIS should sit inside $[{\chi }_{0.025}^2,{\chi }_{0.975}^2]$ for the chosen confidence; persistent values above the band mean $Q$ is too small or $R$ is too small; persistent values below mean too large. See Bar-Shalom/Li/Kirubarajan ch. 5.
4. Innovation autocorrelation. If the innovation sequence is correlated (autocorrelation at lag 1 significantly nonzero), the process noise model is wrong — usually $Q$ is missing a state (e.g., un-modelled bias).
5. Iterate. Reasonable filters need one or two passes through real recorded data with these diagnostics on.

State and observation choice

• Minimal observable state. Do not add states the filter cannot observe (e.g., GPS-only heading on a stationary platform). The Kalman filter’s covariance will not collapse on unobservable directions — it’ll grow until the linearisation around the wrong mean ruins everything.
• Avoid redundant states. Two states that always sum to a third make $P$ singular and the Riccati equation fails; check observability of the augmented system.
• Quaternions on the manifold. Use the MEKF / error-state Kalman filter (Lefferts-Markley-Shuster 1982, Sola 2017) — never a 4-component quaternion as the Kalman state.
• Biases as random walks. Model IMU biases as $\dot{b}=w_b$ with $Q_b$ chosen from the Allan-variance rate-random-walk slope ([[Robotics/sensors-pose-motion]] §2 Allan curves). Don’t model them as constants or you will diverge.
• Augment state with delayed measurements. When a measurement has known latency (e.g., a 50 ms vision pipeline), augment the state with the pose 50 ms ago and update that with the vision measurement — Mourikis-Roumeliotis 2007 (MSCKF).
• Body frame vs nav frame. Choose the frame that minimises the Jacobian — typically the body frame for IMU-driven filters and the nav frame for position-tracking filters. Mixing frames inside the state vector is legal but leads to messy Jacobians; if you must, factor the state into block-diagonal sub-states each in its own frame.
• Scale ambiguity in monocular VIO. A monocular camera + IMU is technically observable (Kelly-Sukhatme 2011, Martinelli 2014) but the scale is weakly observable, requiring sufficient excitation (acceleration, not constant velocity) to resolve. A stationary monocular VIO sensor reports a degenerate solution; the EKF’s $P$ on scale grows; the moment the camera moves with nonzero linear acceleration, scale snaps to truth.

Initialisation

• Don’t start with $P_0=0$ unless you genuinely know the state. Set $P_0$ large in unknown directions (10× the steady-state value), modest in observable directions.
• Bootstrap from one measurement. For an AHRS, the first accelerometer reading at rest sets initial pitch and roll directly; the magnetometer sets yaw; biases start at zero with large $P_b$.
• Outlier-reject during warmup. During the first second, use a looser gate threshold; biases haven’t converged and innovations can be large for legitimate reasons.
• Stillness detection for IMU bias estimation. Hold the robot still for 5–30 s on first power-up; collect gyro samples; the mean is the gyro bias initialisation. Detect “still” by accelerometer variance < $(0.05{\text{m/s}}^2{)}^2$ over a 1-s window plus gyro variance < $(0.005\text{rad/s}{)}^2$. Industrial-grade IMUs need < 5 s; consumer MEMS need ≥ 30 s to reach a stable thermal point.
• Cold-start vs warm-start. A cold-started filter sets all biases to zero, $P_0$ large; a warm-started filter loads the last-saved $(\hat{x},P)$ from NVM. PX4 saves to NV at the end of each flight; ArduPilot saves periodically during flight; ROS 2 robot_localization does not save by default — you have to wire it up.

Outlier rejection (gating)

The innovation $y_k$ at a normally-operating filter is $\mathcal{N}(0,S_k)$, so the Mahalanobis-squared $\nu =y^{\top }{S}^{-1}y$ is ${\chi }_{\dim z}^2$. Reject the measurement if $\nu >{\chi }_{\dim z,0.99}^2$ — that’s the 99 % gate, a common default. For a 2-D GPS measurement, ${\chi }_{2,0.99}^2=9.21$; for a 3-D accelerometer, $11.34$.

For robust kernels (still update, but with reduced weight on outliers): Huber loss adjusts $R$ effectively to $R_{\text{eff}}=R\cdot \nu /k$ for $\nu >k$, giving graceful degradation. Cauchy or DCS (Dynamic Covariance Scaling, Agarwal 2013) work in factor-graph back-ends.

Gating with under-confident innovation covariance. A common failure mode: your filter’s $S=HP{H}^{\top }+R$ is large (filter is under-confident), so $\nu$ is artificially low, and no measurements are gated. The whole gating mechanism becomes a no-op and bad measurements corrupt the estimate. Fix: validate gate behaviour during commissioning by injecting synthetic 5σ outliers; they must be rejected. If they’re not, $P$ is too inflated.

Sequential vs simultaneous update. With $m$ independent measurement components arriving at the same timestamp, you can update them one at a time ($m$ separate scalar updates) or all at once. Mathematically identical for linear-Gaussian; sequential is preferred in embedded code because (a) the inverse is a scalar division instead of an $m\times m$ matrix inversion, (b) you can interleave each scalar update with a Mahalanobis gate, accepting some components and rejecting others.

Discrete-time integration order

The continuous-time prediction integral $\int p(x_t\mid x_{t-1},u_t)\mathrm{bel}(x_{t-1})d{x}_{t-1}$ must be approximated. Three common choices:

• Forward Euler. $x_{k+1}=x_k+f(x_k,u_k)\Delta t$. Simplest, $O(\Delta t)$ accuracy, dominant in ROS 2 robot_localization and toy code. Acceptable when $\Delta t<$ 1/(20 × bandwidth of the fastest mode).
• RK4. Four function evaluations per step, $O(\Delta t^4)$ accuracy. Used by PX4 ekf2, ArduPilot, most aerospace INS. The right default once you can afford it.
• Closed-form quaternion exponential. For attitude, $q^{+}=q\otimes \mathrm{Exp}(\omega \Delta t)$ is exact for a constant rate over $\Delta t$ — no numerical approximation needed.
• Mid-point rule. Evaluate $f$ at $(x_k+x_{k+1})/2$; useful when control acts on velocity and position symmetrically. Two evaluations per step, $O(\Delta t^2)$.

The covariance prediction has its own integration: the continuous-time Lyapunov equation $\dot{P}=FP+P{F}^{\top }+Q$ becomes $P^{+}=\Phi P{\Phi }^{\top }+Q_d$ in discrete form. For small $\Delta t$, $\Phi \approx I+F\Delta t$ and $Q_d\approx Q\Delta t$; for larger $\Delta t$, use van Loan’s matrix-exponential trick (van Loan 1978) for exact discretisation.

Computational budget

Filter	State size $n$	Cost per step	Realisable rate (Cortex-A53, single core)
Linear KF, $n=10$	10	$\sim 10$ µs	100 kHz
EKF, $n=15$, IMU 6-DOF + bias + position + velocity	15	$\sim 30$ µs	30 kHz
EKF, $n=24$ (PX4 ekf2 full state)	24	$\sim 200$ µs	5 kHz (runs at 250 Hz with budget to spare)
UKF, $n=15$	15	$\sim 120$ µs	8 kHz
Iterated EKF, $n=30$, 5 iterations	30	$\sim 2$ ms	500 Hz
Particle filter, $N=500$, 60-beam scan	per-particle	$\sim 4$ ms	250 Hz
Particle filter, $N=5000$, 360-beam scan	per-particle	$\sim 30$ ms	30 Hz
Factor-graph smoother (iSAM2, 20-pose window)	growing	5–30 ms	30–200 Hz

Cost scales as $O(n^3)$ for EKF/UKF and $O(N\cdot k)$ for PF (k = likelihood cost per particle).

Numerical stability

• Joseph form of the covariance update: $P_{k|k}=(I-KH)P_{k|k-1}(I-KH{)}^{\top }+KR{K}^{\top }$. Symmetric and positive-semi-definite by construction; the simpler form $(I-KH)P$ can lose symmetry under floating-point. Always use Joseph when $R$ is small or the gain is high.
• UD decomposition (Bierman 1977). Factor $P=UD{U}^{\top }$ with $U$ unit-upper-triangular and $D$ diagonal; propagate $U$ and $D$ directly. Cuts the dynamic range of stored numbers in half, lets you run an EKF in float32 on Cortex-M without divergence.
• Square-root filtering (Potter, then Carlson, then van Loan). Store and propagate $S=\sqrt{P}$ (Cholesky factor) directly. Modern Eigen + LDLT or the Kalman classes in filterpy / Stone Soup support this directly.
• Symmetrise $P$ every step: $P\leftarrow (P+P^{\top })/2$ to suppress drift due to non-commuting operations.
• Clip negative eigenvalues of $P$. After update, eigen-decompose $P$, clip eigenvalues below $ϵ=10^{-12}$ up to $ϵ$, recompose. Expensive ($O(n^3)$) but a single guard against unphysical $P$ after numerical disaster. Run every $N$ steps in non-realtime sanity loops, never every step.
• Avoid matrix inversion in the innovation step. $S^{-1}$ is the slowest part of an EKF; use the Cholesky factorisation of $S$ + back-substitution for $K=P{H}^{\top }{S}^{-1}$. Eigen::LLT::solve(...) is the idiom.

Adaptive Kalman filters

When sensor noise or process noise vary slowly with operating conditions (vehicle speed, lighting, GPS sky view), a fixed $Q$/$R$ filter is sub-optimal. Three adaptation strategies:

• Innovation-based (Mehra 1970, Maybeck 1979 vol 2 ch. 5). Estimate $R$ online from the empirical innovation covariance: $\hat{R}=\mathrm{cov}(y_k)-HP{H}^{\top }$. Window the estimate over 50–500 steps. Simple, widely used in GNSS-INS when GNSS quality drops in canyons.
• Multiple-model adaptive estimation (MMAE). Run a bank of filters with different $(Q,R)$ assumptions in parallel; weight their outputs by the likelihood of each model given the innovation history. Used in IMM (Interacting Multiple Model) trackers for manoeuvring targets (Blom-Bar-Shalom 1988).
• Variational Bayes filtering (Särkkä-Nummenmaa 2009). Model $R$ as inverse-Wishart-distributed; recursively update its parameters alongside the state. Heavier compute; superior for slowly-varying noise statistics; appearing in newer SLAM stacks.

Adaptive filters are the right answer when fixed tuning manifestly fails across the operating envelope. They are not a replacement for understanding the noise sources; over-eager $Q$ adaptation can mask un-modelled biases that should have been added to the state instead.

Sensor fusion architecture

Five common topologies (the first three are organisational; the last two cut across them):

• Centralised — every sensor feeds raw measurements into one big filter. Optimal estimator under linear-Gaussian assumptions; what PX4 ekf2 does for the IMU + GPS + mag + baro + airspeed stack. Brittle: a single sensor bug (e.g., GPS reporting NaN) can corrupt the whole state.
• Federated — each sensor has its own local filter; a master filter fuses the local estimates. Carlson 1990. Modular, fault-tolerant; theoretically conservative (federation factor reduces effective measurement information) unless carefully tuned.
• Hierarchical (cascaded) — fast inner filter (attitude) running at IMU rate; slower outer filter (position) running at GPS rate, consuming the inner filter’s output as a measurement. ROS 2 robot_localization supports this via two stacked EKF nodes.
• Loosely coupled — sensor produces a high-level estimate (e.g., GPS reports lat/lon/alt with covariance) → main EKF treats it as a measurement of the position state. Easy to integrate, but loses information.
• Tightly coupled — raw observables (pseudoranges, Doppler, image keypoints) feed the EKF directly; the filter knows the sensor model. More robust under partial outage (one satellite is better than zero, and a loose-coupled GPS will refuse to fix with three). Standard in modern automotive GNSS-INS (NovAtel SPAN, Septentrio AsteRx-i, Trimble BD992-INS, VectorNav VN-300).
• Ultra-tightly coupled (deep integration) — the inertial state feeds the GNSS receiver’s tracking loops directly, recovering acquisition under high dynamics and jamming. Honeywell HG1700 + Rockwell GPS, military / GNSS-denied platforms only.

Manifold-aware filters in modern practice

Naïve filters treat $SE(3)$ or $SO(3)$ states as ${\mathbb{R}}^n$, glue normalisation onto the end, and accept the bias. Modern filters keep the state on the manifold and represent the error in the tangent space:

• MEKF — $SO(3)$ via quaternion + 3-vector error (Lefferts-Markley-Shuster 1982). The default for AHRS.
• Right-invariant EKF (Bonnabel 2007; Barrau-Bonnabel 2017) — $SE(3)$ or $S{E}_2(3)$ (extended pose with velocity), with the error defined as a Lie-group element. Provides autonomous error dynamics (the error equation is independent of the actual trajectory), which removes the linearisation-error pathology and gives true global convergence guarantees. Adopted in Hartley-Ghaffari-Eustice-Grizzle 2020 for legged-robot state estimation; ROS 2 kalani implements it.
• EquiF (equivariant filter, Mahony-van Goor-Hamel 2023) — generalisation of invariant EKF to any system with a symmetry group. Gives Kalman-like complexity with optimisation-like accuracy.

The practical effect: on a 6-DOF tracking task, an iEKF converges in ~5 iterations; an invariant EKF converges in 1 iteration to a near-MAP estimate; the linearisation error simply doesn’t depend on the trajectory.

• Ultra-tightly coupled — GNSS tracking loops driven by the INS itself. Mil-grade only.

Time-alignment and out-of-sequence measurements

A 100 Hz IMU, a 25 Hz LiDAR, and a 5 Hz GNSS sharing a single EKF cannot be aligned by host-side timestamps alone — Linux user-space scheduling can stretch a timestamp by milliseconds, more than enough to corrupt a 3-m/s vehicle’s state by tens of centimetres per cycle. Three patterns work:

1. Hardware sync pulse. A single PPS signal (1 Hz from a GPS module) triggers a counter on every sensor MCU. Every sample is tagged with the counter value, then translated to a common time base in the filter. This is the PX4 / ArduPilot / Septentrio approach.
2. PTP / IEEE 1588. A grandmaster clock on the network distributes a synchronised time to every sensor’s host CPU; Ethernet hardware timestamping pins the precision to sub-microsecond. Required for any LiDAR-camera-IMU automotive stack (see [[Languages/Tier3/ros2-robotics-config]] for ROS 2 DDS timestamps).
3. Per-sample buffer + replay. Each sample is tagged with sensor-side timestamp; the filter maintains a buffer of past states; when an out-of-sequence sample arrives, rewind to its timestamp and replay forward. Used in PX4 ekf2 for vision and GPS, both of which can lag the IMU by 50–200 ms.

5. Components & sourcing — software libraries

General-purpose filter libraries

Library	Language	Coverage	Notes
filterpy (Roger Labbe 2015–)	Python	KF, EKF, UKF, PF, $H_{\infty }$	Pure NumPy, pedagogical; pairs with Labbe’s free book “Kalman and Bayesian Filters in Python”.
stone-soup (UK DSTL 2017–)	Python	Tracking filters incl. PHD, JPDA	Heavyweight; multi-target tracking community.
Eigen + custom	C++	Anything	Standard for embedded; ~200-line EKF/UKF templates abound.
kalman (Markus Bader)	C++ header-only	EKF, UKF, square-root	Eigen-based; embedded-friendly.
GTSAM (Dellaert 2010–)	C++, Python bindings	Factor graphs, iSAM2, GPS+IMU, VIO front-end	The reference for graph-based smoothing.
Ceres Solver (Google 2010–)	C++	Nonlinear least-squares; batch smoothing	Used as the back-end for VINS-Fusion, BA, MSCKF variants.
g²o (Kümmerle 2011–)	C++	Pose-graph and BA optimisation	Older than GTSAM/Ceres but still active in SLAM.
Sophus (Strasdat)	C++ header-only	SO(3)/SE(3)/Sim(3) Lie-group ops	The standard manifold library; underpins MEKF / VIO code.
MATLAB Sensor Fusion Toolbox	MATLAB	EKF, UKF, GSF, PF, IMM	Industrial / aerospace prototyping standard.

Robot-specific filter stacks

Project	Filter	Application
PX4 ekf2 (PX4 Autopilot, 2017–)	24-state quaternion EKF	Drone / VTOL / fixed-wing attitude+position+wind. Runs at 250 Hz on STM32F7/H7.
ArduPilot AP_NavEKF3	24-state EKF, two parallel instances	Backup to PX4-style design; uses lane-switching for robustness.
ROS 2 robot_localization (Tom Moore, OpenRobotics 2013–)	EKF and UKF flavours	Generic mobile-robot pose fusion (odom + IMU + GPS + visual). Workhorse since ROS 1.
ROS 2 nav2_amcl	Adaptive MCL particle filter	2D occupancy-grid localisation.
Cartographer (Google 2016)	UKF for local pose + pose-graph optimisation for global	2D and 3D LiDAR SLAM.
imu_filter_madgwick / imu_complementary_filter	Complementary filter (not Kalman)	Lightweight AHRS for ROS; covers AHRS without state-aug.
VINS-Fusion (HKUST Aerial Robotics 2018)	Sliding-window factor-graph smoother	Visual-inertial odometry.
OpenVINS (RPNG, U. Delaware 2019)	MSCKF + sliding-window SLAM	Tightly-coupled VIO; reference research code.
Basalt (TUM 2019)	VIO + nonlinear factor-graph back-end	Stereo VIO; competitive on EuRoC benchmark.
rtabmap_ros	EKF + graph SLAM	RGB-D SLAM with loop closure.
Autoware ndt_localizer + EKF	EKF over NDT pose + IMU + odom	Autonomous-vehicle localisation in HD maps.

Hardware AHRS / VIO appliances

These ship the filter inside the box; you receive fused state via SPI/UART/CAN/Ethernet.

Vendor	Product	Filter inside	Output
VectorNav	VN-100 / VN-200 / VN-300	EKF	Attitude / position / heading
Xsens	MTi-30 / MTi-680G	EKF	AHRS / INS
Microstrain	3DM-CV7-INS	EKF	INS
Honeywell	HG4930 + HG1700	Internal EKF, optional	Tactical INS
Septentrio	AsteRx-i	Tightly-coupled GNSS-INS	RTK pose
NovAtel	SPAN family	Tightly-coupled GNSS-INS	RTK pose
Realsense	T265 (EoL 2022)	VIO inside Movidius VPU	6-DOF pose at 200 Hz

6. Reference data

Symbol cheat-sheet

Symbol	Meaning
$x_k$	state vector at step $k$
${\hat{x}}_{k\mid k-1}$, ${\hat{x}}_{k\mid k}$	predicted and updated mean
$P_{k\mid k-1}$, $P_{k\mid k}$	predicted and updated covariance
$u_k$	control input
$z_k$	measurement vector
$w_k$, $v_k$	process and measurement noise
$Q_k$, $R_k$	process and measurement noise covariances
$F_k$, $H_k$	motion and measurement Jacobians (EKF)
$K_k$	Kalman gain
$y_k$	innovation $z_k-h({\hat{x}}_{k\mid k-1})$
$S_k$	innovation covariance $HP{H}^{\top }+R$
${\nu }_k$	normalised innovation squared $y^{\top }{S}^{-1}y$
$\Lambda$, $\xi$	information matrix and information vector
$q$	quaternion (body-to-nav unless specified)
$\delta \theta$	rotation error in tangent space of $SO(3)$
$b_g$, $b_a$	gyroscope and accelerometer biases
$\mathrm{Exp}(\varphi )$	quaternion exponential of rotation vector $\varphi$

Chi-squared gate thresholds

For a measurement of dimension $m$, the $(1-\alpha )$ confidence gate rejects innovations with $\nu =y^{\top }{S}^{-1}y>{\chi }_{m,1-\alpha }^2$. Common values:

$m$	${\chi }_{m,0.95}^2$	${\chi }_{m,0.99}^2$	${\chi }_{m,0.999}^2$	Sensor
1	3.84	6.63	10.83	Range, altimeter, single-axis gyro
2	5.99	9.21	13.82	2D GPS, image keypoint reprojection
3	7.81	11.34	16.27	3D GPS, 3-axis accelerometer, magnetometer
4	9.49	13.28	18.47	Quaternion measurement (rare)
6	12.59	16.81	22.46	Full pose measurement
12	21.03	26.22	32.91	Full IMU snapshot

Filter algorithm comparison

Filter	Posterior form	Per-step cost (state $n$, obs $m$)	Linearisation strategy	Multimodal?	Real-world fit
KF	Gaussian	$O(n^3)$	None (linear)	No	Theoretical / very-rare.
EKF	Gaussian	$O(n^3)$	First-order Taylor at mean	No	Most production robotics.
iEKF	Gaussian	$O(n^3\cdot k)$	First-order Taylor, iterated	No	Range / bearing-only / tightly-coupled GNSS.
UKF	Gaussian	$O(n^3)$, $\sim 3\times$ EKF	Sigma points, second-order exact	No	High-rate attitude; awkward Jacobians.
Square-root EKF	Gaussian	$O(n^3)$	First-order, Cholesky form	No	Long missions, numerical stability.
Information filter	Inverse Gaussian	$O(n^3)$ predict, $O(n^{2}m)$ update	First-order	No	Sparse high-dim systems (SLAM).
Particle filter	Sample set	$O(N\cdot k)$, $k$ = likelihood cost	None (Monte Carlo)	Yes	Global localisation, kidnapped robot.
Rao-Blackwellised PF	Mixed	$O(N\cdot k)$ + per-particle KF	Partial linear-Gaussian sub-state	Yes	FastSLAM-class problems.
Gaussian-mixture filter	Sum of Gaussians	$O(M\cdot n^3)$	First-order per component	Limited	Tracking with ambiguous data association.
Factor-graph smoother	Joint Gaussian or sample	$O(\text{window})$	First-order, periodic relin	Limited	iSAM2, GTSAM; near-MAP online.

Filter selection by sensor stack

Sensor stack	Recommended filter	Notes
IMU only (gyro + accel)	Complementary or MEKF	AHRS — orientation only, no position.
IMU + GPS	EKF (loosely coupled)	Classic GNSS-INS.
IMU + GPS + magnetometer	EKF, MEKF state	Drone autopilot baseline (PX4 ekf2 minus wind/terrain).
IMU + multi-constellation GNSS pseudoranges	Tightly-coupled EKF	Septentrio AsteRx-i class.
IMU + wheel odometry	EKF	Indoor AMR; needs slip detection.
IMU + wheel odom + 2D LiDAR + map	EKF + PF	EKF for short-term, MCL for global localisation.
IMU + stereo cameras	MSCKF or VINS-Fusion	Tightly-coupled VIO.
IMU + mono camera	MSCKF with scale-aware update	Scale weakly observable; needs excitation.
IMU + lidar + camera + GNSS	Factor-graph back-end + EKF front-end	Autonomous-vehicle / LIO-SAM class.
IMU + UWB beacons	iEKF	Linearisation-error-sensitive; iteration helps.
Encoders only on arm joint	KF (per joint)	Plant is approximately linear; Kalman is exact.
Force-torque + arm encoders	EKF	Compliance / contact estimation; see [[Robotics/impedance-control]].

Common robot state vectors

Platform	Typical state $x$	Size	Filter
2D mobile robot, no slip	$[x,y,\theta ]$	3	EKF or PF
2D mobile robot, with velocity	$[x,y,\theta ,v,\omega ]$	5	EKF
AHRS (drone attitude)	$[q_{wxyz},b_g]$	7 (6 error)	MEKF or UKF
Quadrotor with INS, no GNSS	$[p,v,q,b_g,b_a]$	16 (15 error)	EKF
Quadrotor with GNSS + baro (PX4 ekf2)	$[p,v,q,b_g,b_a,b_{\text{mag}},{\text{wind}}_{xy},\text{terrain}]$	24	EKF
6-DOF arm joint state	$[q_{1:6},{\dot{q}}_{1:6}]$	12	EKF per joint or block-diag
Surgical robot (RCM-constrained)	$[q_{1:n},{\dot{q}}_{1:n},p_{\text{tool}}]$	14–20	EKF
Differential-drive AGV (robot_localization)	$[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},\dot{\varphi },\dot{\theta },\dot{\psi },\ddot{x},\ddot{y},\ddot{z}]$	15	EKF or UKF
Visual-inertial odometry (MSCKF)	$[p,v,q,b_g,b_a]$ + sliding-window of past poses	15 + $6N_{\text{wnd}}$	EKF
Multi-target tracker, $K$ targets	per-target $[p,v]$ stack	$4K$ to $6K$	JPDA / GM-PHD

Numerical stability options

Option	Symmetry-preserving	Positive-definite-preserving	Cost	When to use
Naive $(I-KH)P$	No	No	Cheapest	Toy code only.
Joseph form	Yes	Yes	$+2\times$	Default in any production EKF.
UD decomposition (Bierman 1977)	Yes	Yes by construction	$\sim$ same as Joseph	Embedded / float32.
Square-root EKF (Carlson)	Yes	Yes by construction	$\sim 2\times$	Long missions, ill-conditioned.
Information filter	Yes (on $\Lambda$)	Yes	Cheap on update	Sparse / many measurements.
Periodic re-symmetrisation $(P+P^{\top })/2$	Yes	No	Negligible	Always add it.

Manifold parameterisations for state on a Lie group

Group	Dimension (manifold / tangent)	Use	Library
$SO(2)$	1 / 1	2D heading	trivial
$SO(3)$	3 / 3	3D rotation	Sophus SO3<>, GTSAM Rot3
$SE(2)$	3 / 3	2D pose	Sophus SE2<>, GTSAM Pose2
$SE(3)$	6 / 6	3D pose	Sophus SE3<>, GTSAM Pose3
$\mathrm{Sim}(3)$	7 / 7	3D pose + scale (mono-VIO)	Sophus Sim3<>
$S{E}_2(3)$	9 / 9	Pose + velocity (invariant EKF)	own implementation

For each, the exp/log map translates between the tangent space (where the Kalman update lives) and the manifold (where the state lives). Always use the library’s exp/log rather than rolling your own — the Bortz integration formula for $SO(3)$ has subtle high-order terms that hand-coded versions commonly get wrong.

Q, R magnitude templates by sensor

(Standard deviations; square for covariance entries. All discretised to $\Delta t=0.01$ s except where stated.)

Sensor	Typical noise (1σ)	$R$ entry	Notes
Wheel encoder, 4096 cpr quadrature	velocity 0.01 m/s @ 1 m/s	$10^{-4}$ m²/s²	After M/T-method extraction.
Industrial MEMS gyro (ADIS16505)	0.15°/√hr ARW	$7\times 10^{-9}$ rad²/s	Continuous PSD; discretise.
Consumer MEMS gyro (BMI088)	0.014°/s = 4 × 10⁻⁴ rad/s	$1.6\times 10^{-7}$ rad²/s²	At 100 Hz BW.
Consumer MEMS accel (BMI088)	1.75 mg @ 100 Hz BW	$3\times 10^{-4}$ m²/s⁴	$\sigma \cdot g$ converted.
Consumer GPS (u-blox NEO-M9N)	1.6 m CEP horizontal	2.5 m²	Open-sky lat/lon.
RTK GNSS (Septentrio)	1 cm horizontal	$10^{-4}$ m²	Fixed-integer ambiguity.
Magnetometer (BMM150)	0.3 µT	$9\times 10^{-14}$ T²	Subject to hard/soft-iron error.
Barometer (BMP388)	0.5 m altitude RMS	0.25 m²	Slow drift with weather.
LiDAR 2D scan, 905 nm class	30 mm range	$9\times 10^{-4}$ m² per ray	Plus beam-model outlier term.
Visual feature reprojection	1 px	1 px²	Convert via camera intrinsics.
Wheel odometry (linear), good floor	1 % distance	linearly grows with travel	Use Q on the position, not R.
Wheel odometry (linear), slippery	5 % distance + bias	Bigger Q + outlier check	Detect slip with IMU vs encoder disagreement.

Update rates seen in production

System	Predict rate	Update rate	Output rate to consumer
PX4 ekf2 (drone)	250 Hz IMU	5 Hz GPS, 100 Hz mag, 25 Hz baro	250 Hz
ArduPilot EKF3	400 Hz IMU	5–10 Hz GPS, 100 Hz mag	400 Hz
ROS 2 robot_localization	50 Hz IMU	1–10 Hz GPS, 10 Hz odom	50 Hz
nav2_amcl	20 Hz odom	10–25 Hz LiDAR	on-demand pose query
VINS-Fusion	200 Hz IMU pre-integration	20 Hz camera keyframes	200 Hz
MSCKF (OpenVINS)	200 Hz IMU	20 Hz camera	200 Hz
Honeywell HG9900	600 Hz INS	1 Hz GNSS, 50 Hz radar alt	100 Hz
Spot body state (BD)	1 kHz	200 Hz stereo VO, 250 Hz joint enc	1 kHz
Tesla Autopilot	50 Hz vehicle stack	30 Hz vision, 10 Hz GNSS, 100 Hz CAN	50 Hz
Apple Vision Pro VIO	1 kHz IMU	60–90 Hz camera	1 kHz

7. Failure modes & debugging

• Filter divergence — covariance grows without bound; estimate runs away. Causes: process noise too small ($Q$ under-set), unobservable mode being driven, model bias not modelled as state. Fix: check observability of $(F,H)$ at the operating point; inflate $Q$; add bias state if missing.
• Filter inconsistency — average squared error in repeated runs exceeds the trace of $P$; the filter is “over-confident”. Diagnose with NIS test. Fix: increase $Q$ or $R$ until NIS sits inside its chi-squared band.
• EKF linearisation error at high rates — Jacobian computed at the current mean is wrong over the $1\sigma$ ball when the function curves on that scale. Symptoms: bias-like residual on high-rate manoeuvres; estimate “snaps” after a measurement instead of smoothly converging. Fix: switch to UKF, or use iterated EKF on the informative measurements.
• Quaternion normalisation drift — additive 4-vector updates leave $|q|\ne 1$ after every step; the matrix $R(q)$ stops being a rotation. Fix: use the MEKF / error-state form (Lefferts-Markley-Shuster 1982). If you must use the additive form, re-normalise $q\leftarrow q/|q|$ every step and tolerate the resulting bias.
• Singular or rank-deficient $P$ — redundant states, $Q=0$ on a state, accumulated round-off. Diagnose by np.linalg.cond(P) exceeding 1e10. Fix: factor out the redundancy, add small process noise on every state, use UD or square-root form.
• Outlier corrupts state — a single spurious GPS jump moves $\hat{x}$ several metres; subsequent measurements see large innovations and the filter convalences slowly. Fix: Mahalanobis gating before update (reject if $\nu >{\chi }_{m,0.99}^2$); robust kernels in the update; sensor-level voting if redundant sensors exist.
• Bias drift not modelled — IMU bias varies with temperature, supply voltage, or just walks slowly. If modelled as a constant, $\hat{b}$ never moves and the filter accumulates the drift into the orientation state. Fix: model bias as a random walk with $Q_b$ from the Allan-variance bias-instability plateau and rate-random-walk slope ([[Robotics/sensors-pose-motion]] §2).
• Particle depletion — after a few resampling steps, most particles share a small number of ancestors; effective sample size $N_{\text{eff}}$ collapses; filter can’t recover from a measurement model error. Fix: resample only when $N_{\text{eff}}<N/2$; add a small “regularisation” noise to particles after resampling; auxiliary particle filter (Pitt-Shephard 1999); increase $N$.
• Latency mismatch — vision update labelled with current time arrives describing a pose 50 ms ago. Applied naïvely, the filter back-rotates the IMU-integrated state. Fix: buffer past states/covariances and apply out-of-sequence measurements at their actual timestamp, then re-propagate (Bar-Shalom 2002 OOSM algorithms); or use the augmented-state / sliding-window trick of MSCKF (Mourikis-Roumeliotis 2007).
• Time-sync drift across sensors — IMU on STM32 clocked from one quartz, LiDAR clocked from another, GPS clocked from PPS. After hours, the timestamps drift apart by milliseconds. Fix: hardware-time-stamp every sample (PTP / IEEE 1588 over Ethernet, or a single hardware sync pulse), never rely on host system clocks ([[Languages/Tier3/ros2-robotics-config]]).
• Numerical breakdown in long missions — after hours of filter operation in float32, $P$ loses positive-definiteness, the Cholesky factorisation fails, the gain is junk. Fix: UD or square-root form; periodic eigen-decomposition + clipping of negative eigenvalues to $10^{-12}$.
• Initialisation sensitivity in MCL — global localisation with $N=500$ particles in a 100-m hallway misses the correct mode. Fix: larger initial $N$ (5000–50000); KLD-sampling to adapt $N$ down once converged; injection of small percentage of uniform-random particles each step to enable recovery from kidnapping.
• Multi-rate sensors — naïvely down-sampling the IMU to GPS rate throws away information. Fix: run prediction at the IMU rate; apply each measurement at its own rate; never aggregate measurements to the slowest one.
• Backwards-in-time correction (OOSM) — measurement timestamp older than the latest filter time. Solutions: rollback-and-replay (store last $N$ states + measurements, rewind to OOSM time, re-step forward); Bar-Shalom 2002 algorithms B1/B2 (state augmentation, avoids rewind); reject if too old.
• Quaternion sign ambiguity — $q$ and $-q$ represent the same rotation; the filter might flip sign between steps, breaking continuity in plots and consumers. Fix: enforce $q_w\ge 0$ (or whichever component is convention) after every update.
• Innovation autocorrelation — innovation is meant to be white. Significant lag-1 autocorrelation indicates a missing state. Fix: examine which sensor’s innovation is correlated; add an integrating state in the relevant channel.
• G-sensitivity of MEMS gyro under sustained acceleration — gyro reports nonzero rate when no rotation is present, simply because of linear $g$. Manifests on automotive platforms as heading bias during cornering. Fix: factor-in g-sensitivity coefficients from the IMU calibration ([[Robotics/sensors-pose-motion]] §7); industrial-grade IMUs only.
• ZUPT (zero-velocity update) missing — when the platform is provably stationary (foot in stance phase for a humanoid, vehicle at red light per CAN gear-in-park, AUV resting on sea-bed), inject a pseudo-measurement $z=0$ on velocity with very small $R$. Pulls the filter’s velocity estimate (and bias) to truth; the only way to bound IMU drift without external aiding. Used heavily in pedestrian dead-reckoning (Foxlin 2005) and dismounted-soldier INS.
• NHC (non-holonomic constraint) missing — a car-like vehicle’s lateral velocity in the body frame is essentially zero (no side-slip in normal driving). Injecting $v_y^{\text{body}}=0$ as a pseudo-measurement with small $R$ adds nearly as much information as a wheel encoder. Standard in automotive GNSS-INS.
• Wind state diverges in fixed-wing fusion — PX4’s ekf2 includes a horizontal wind state $w_{n,e}$. With no airspeed sensor it becomes weakly observable and drifts; the GPS-derived ground velocity then absorbs wind-vs-airspeed confusion as a heading bias. Fix: require an airspeed measurement (Pitot tube) before enabling wind estimation, or fix $w=0$ during ground-vehicle / multi-rotor operation.
• Magnetic-declination misset — magnetometer points to magnetic north, not true north; declination varies by location and slowly with date. A 5° declination error masquerades as constant 5° heading bias. Fix: apply WMM-2025 declination from a lookup or NOAA-API query at startup based on GPS-derived position.

Convergence diagnostics — what a healthy filter looks like

A correctly tuned EKF, observed over a few minutes of nominal operation, shows:

• Innovation mean ≈ 0 (no persistent bias). Bias > 0.5σ on any axis indicates a missing state or a frame-transform error.
• Innovation standard deviation ≈ $\sqrt{\text{diag}(S)}$ (innovation white and matches predicted covariance).
• NIS (normalised innovation squared) averaged over a 10-s window inside the chi-squared confidence band for $m$ degrees of freedom.
• Innovation autocorrelation at lag 1 < 0.1 (innovation should be near-white, like a discrete white-noise sequence).
• Trace of $P$ decreases when measurements arrive, increases between them; reaches a steady-state plateau.
• Eigenvalues of $P$ all positive (no NaNs, no infinities, no zeros below 1e-15).

Logging all five quantities at filter output is cheap (a few floats per step) and routinely catches a wide range of bugs before they corrupt the downstream stack. PX4 ekf2’s estimator_innovations and estimator_status uORB messages expose exactly this telemetry; ROS 2 robot_localization exposes innovations on a debug topic; ArduPilot logs them in the dataflash binary.

Sensor-specific failure patterns

• GPS multipath in urban canyons — pseudo-range measurements wobble by 5–50 m as reflections off glass buildings dominate the direct path. Manifests as innovation jumps that pass the 99 % gate but are still wrong. Fix: raise gate threshold to 95 % during urban driving (accept more rejection), or feed the GNSS carrier-to-noise density $C/N_0$ as a per-channel quality indicator that scales $R$.
• GPS spoofing / jamming — adversarial or unintentional RF interference; the receiver reports a confident but wrong position. Fix: cross-check against IMU-derived motion (jump detection), against magnetometer-derived heading change, and against multi-constellation diversity if available.
• Magnetometer hard-iron drift after mission — rotating servos, drone payloads, or new wiring re-magnetise the chassis. Existing calibration becomes invalid. Fix: in-flight calibration that continuously updates the hard-iron offset (PX4 mag_cal background process); periodic ground recalibration on schedule.
• LiDAR motion-distortion in moving frame — a 10 Hz spinning LiDAR captures returns over 100 ms during which the platform moves up to 10 cm at 1 m/s. Naïvely stacking the returns into a single point cloud corrupts measurements at the edges of the sweep. Fix: deskew each return using the IMU-integrated motion within the scan period; ROS 2 packages like pointcloud_motion_deskew and Velodyne’s native driver do this.
• Camera rolling shutter — pixels are exposed sequentially; a fast rotation distorts the image. The measurement model $h(\cdot )$ must include the row-dependent timing. The default global-shutter camera model produces 1–2° heading bias on aggressive manoeuvres with a 30 Hz rolling-shutter camera. Fix: explicit rolling-shutter model in $h$, or replace with a global-shutter camera.
• Wheel encoder slip on uneven terrain — wheel encoders report rotation that didn’t translate into ground motion. The IMU disagrees with the encoder-derived velocity by tens of percent. Fix: detect via IMU-vs-encoder cross-check; inflate $Q$ on position state when disagreement exceeds threshold, effectively de-weighting wheel odometry until the platform stabilises.

Common debugging recipes

Symptom	First check	Second check
Estimate drifts under no measurements	Process model unbiased? Bias state present?	$Q$ on bias non-zero?
Estimate jitters under measurements	$R$ too small	Out-of-band sensor noise (sample-rate jitter, EMI)
NIS chronically high	$R$ too small or $Q$ too small	Linearisation error → UKF
NIS chronically low	$R$ or $Q$ too large	Consistent over-conservative tuning
Covariance grows without bound	Unobservable state, or $H$ rank-deficient	Singular $S$, gain explodes
Filter “snaps” on each update	Linearisation error far from mean	Switch to iEKF or UKF
MCL fails global localisation	Too few particles	Likelihood field ${\sigma }_{\text{hit}}$ too tight
Particle filter resampling never triggers	Weights stay flat → likelihood model too loose	${\sigma }_{\text{hit}}$ too wide
Quaternion drift in attitude estimate	Additive update on $q$	Switch to MEKF

8. Case studies

IMU pre-integration for visual-inertial systems

When a slower vision update arrives every 33 ms (30 Hz camera), running a separate filter step per 1 kHz IMU sample is wasteful and stack-unfriendly. IMU pre-integration (Lupton-Sukkarieh 2012; Forster-Carlone-Dellaert-Scaramuzza 2016 RSS) compresses a chunk of $N$ IMU samples into a single relative-pose increment + covariance, expressed in a bias-corrected, body-frame-relative form that does not need to be recomputed when biases change. The savings: a visual-inertial back-end (VINS-Fusion, OpenVINS, Kimera) re-optimises the whole keyframe window every camera frame; without pre-integration this would require re-integrating thousands of IMU samples per keyframe; with pre-integration, only the delta-bias correction is reapplied per iteration, making real-time VIO at 30 Hz tractable on Cortex-A class hardware.

Common pitfall — using float32 everywhere

Embedded EKF code routinely runs float32 to save memory and exploit hardware FPU width on Cortex-M4/M7. Two areas where this bites:

1. Covariance update. $P$ can have a dynamic range spanning 6–8 orders of magnitude (position variance ~10⁻⁴ m², gyro-bias-error variance ~10⁻¹² rad²/s²). The product $HP{H}^{\top }$ inside the Kalman gain can lose 4–5 significant digits to subtraction cancellation. UD form (Bierman 1977) cuts the dynamic range in half by storing $\sqrt{P}$.
2. Time elapsed. A 32-bit float second-counter loses 1 ms resolution after 8.4 hours and 1 s resolution after a month — irrelevant for most robots but catastrophic for any mission-time-stamped log analysis. Use uint64_t microseconds for timestamps and convert to float only at the math boundary.

PX4 ekf2 uses float (32-bit) for state and covariance throughout, with UD form on the covariance and uint64_t microseconds for timestamps; ArduPilot uses float and uint64_t similarly. ROS 2 robot_localization uses double (64-bit) by default at the cost of memory and per-step compute — acceptable on Cortex-A but not on Cortex-M.

Common pitfall — using the same filter for control and estimation

A filter tuned for publication (smooth, low-noise estimates) is not the same filter tuned for control (low-latency, no peeking into the future). Pure Kalman filters do not introduce phase lag (they’re causal MMSE), but heavy outlier-rejection, fixed-window smoothing, or long latency-buffering does. For tight control loops, prefer the filter output before any smoothing; for telemetry, log the smoothed version separately.

PX4 ekf2 — 24-state quaternion EKF (drone autopilot reference)

PX4 Autopilot’s ekf2 is the de-facto open-source EKF for multicopters, fixed-wings, and VTOLs. Its state vector (Williams-Chambers PX4 EKF2 documentation, 2020):

$x=[q_{wxyz},v_{n,e,d},p_{n,e,d},b_{g,xyz},b_{a,xyz},m_{n,e,d},m_{b,xyz},w_{n,e}{]}^{\top }\in {\mathbb{R}}^{24}$

with body-to-NED quaternion, NED velocity, NED position, gyro bias, accel bias, Earth magnetic field (estimated, not fixed), magnetometer body bias, and horizontal wind. Implementation in error-state (MEKF) form, so the error covariance is ${\mathbb{R}}^{23\times 23}$.

The IMU drives prediction at 250 Hz on STM32F7 / H7; GPS, magnetometer, baro, airspeed, range finder, external vision (mavlink ODOMETRY messages), and optical-flow all feed measurement updates. The 24-state size is just within the budget for float32 EKF on the hardware — bench tests show drift over a 4-hour flight without divergence. Square-root form, periodic symmetrisation, and aggressive innovation gating are the keys.

Lane-switching robustness. PX4 v1.13+ runs multiple instances of ekf2 in parallel; a watchdog monitors NIS in each and switches the published output to the most consistent lane. Compensates for transient sensor faults (a single noisy magnetometer reading) without rejecting the sensor entirely.

ArduPilot AP_NavEKF3 — production EKF with lane switching

ArduPilot runs two parallel instances of EKF3 by default (configurable up to four), each with its own IMU and magnetometer source. Each instance computes its own state, innovation history, and “lane innovation index” — a smoothed measure of innovation-vs-covariance health. The active lane (whose output drives the autopilot) is the one with the lowest lane index over the last 1 s. On sensor-induced degradation, ArduPilot switches lanes within ~10 ms with no glitch in the published state, since both lanes were tracking in parallel. This architecture catches partial sensor failures (one IMU saturates, one magnetometer drifts) that a single-lane filter would silently absorb.

The state vector matches PX4’s 24-state design closely but with implementation differences: ArduPilot uses additive quaternion + post-update renormalisation rather than full MEKF; Cholesky-square-root form for the covariance; and per-axis innovation gates rather than full-vector Mahalanobis. The latter lets ArduPilot accept a partial-validity measurement (e.g., 2-of-3-axes valid magnetometer) where PX4 would reject the whole vector.

NASA Mars rovers — Spirit, Opportunity, Curiosity, Perseverance

Mars surface localisation has no GPS. Each rover dead-reckons by wheel odometry corrected by visual odometry (Matthies-Maimone 2007), with the Honeywell MIMU providing the inertial attitude reference. The fusion is an EKF whose state is reset against external sources (sun sensor, surface-relative VO, and once per sol the Earth-radio-tracking position fix) at intervals.

Spirit (landed 2004) and Opportunity ran visual odometry sparingly because the early-2000s RAD6000 CPU could not keep up with continuous VO; VO triggered only on high-slip terrain. Curiosity (2012) used dual stereo-camera Navcams with offline VO; Perseverance (2021) added the AutoNav stack with onboard VO running continuously on a Virtex-5QV FPGA companion to the RAD750. Tracked position uncertainty during long traverses on Perseverance: < 1 % of distance driven (vs 5–10 % for wheel odometry alone on the Curiosity dunes — Maimone-Cheng-Matthies 2007 documented exactly this failure mode that drove the AutoNav investment).

The IMU (an LN-200S-class FOG-based tactical unit; precise model is in [[Robotics/sensors-pose-motion]] §8) is the bias-stable reference against which VO and wheel-odometry residuals are computed. Without it, attitude drift would corrupt the photogrammetric VO baselines within hundreds of metres.

Honeywell HG9900 navigation INS — strapdown with ring-laser gyros

Used on the 787 Dreamliner, A350, and several military rotorcraft. Three ring-laser gyros (bias < 0.003 °/hr) and three quartz-flexure accelerometers (bias < 25 µg) at the front; a navigation-grade Kalman filter at 600 Hz computing position, velocity, attitude in WGS-84 ECEF. Aiding from dual-antenna GNSS at 1 Hz, dual barometric altimeters, and Doppler radar altimeter near the ground. The filter implements full-Earth strapdown (Coriolis + transport-rate terms, Schuler tuning, polar singularity handling), maintains ~15 minutes of full-INS performance in GNSS-denied flight, and is the reference implementation against which civilian autonomy stacks measure their drift.

nav2_amcl — Adaptive Monte Carlo Localisation in production

nav2_amcl is the standard ROS 2 indoor localisation node for differential-drive and omnidirectional mobile robots, used in tens of thousands of deployed AMRs (Locus Robotics, Fetch, MiR, Boston Dynamics Spot’s NavApp). 2D occupancy-grid map, 360° lidar (Hokuyo URG/UST, RPLIDAR, Velodyne VLP-16 reduced to a horizontal slice, Ouster OS1 likewise), differential-drive odometry.

Convergence times on the standard benchmark (Stanford’s gates_floor1 map, 50 × 30 m): 12 s mean global localisation with $N_{\text{max}}=5000$, KLD-sampling adaptive. Tracking accuracy in steady state: 5–10 cm RMS (occupancy-grid resolution at 5 cm, particle effective resolution at 1 cm after weighted averaging). Pose-recovery time after a kidnap event: 4–8 s once the user-configured recovery behaviour injects fresh uniform particles.

Tesla Autopilot inertial fusion (HW3+/HW4)

Tesla’s autonomy stack (as inferred from teardowns, FCC filings, and Tesla’s own AI Day disclosures 2021–2022) fuses automotive MEMS IMU (Bosch SMI230 class), dual-antenna GNSS for heading, four wheel-encoder Hall pickups, eight cameras, and 12 ultrasonics. The “Occupancy Network” + “HydraNets” run on the in-house FSD-chip neural fabric; their output is fused with the inertial state via a learned-residual EKF that supplies pose and velocity to the planner at 50 Hz (Karpathy AI Day 2021 talk hints). No LiDAR. Heavy reliance on vision-derived motion vectors as the primary translation observation; IMU plus wheel encoders provide the high-rate body-frame motion model.

Boston Dynamics Spot — body-state fusion across legs

Spot’s body state is fused from twelve joint encoders ([[Robotics/sensors-pose-motion]] §8), an industrial MEMS IMU mounted near the centre of mass (ADIS16470 class, gyro bias ~2 °/hr), and stereo vision from the perception heads. The whole-body controller consumes a 6-DOF body pose at 1 kHz, while the navigation stack runs an outer EKF + factor-graph back-end at 25 Hz to fuse stereo VO and IMU into a globally consistent trajectory. Foot contact, critical for legged dynamics, is inferred by an estimator that combines accelerometer impulses, joint-torque residuals ([[Robotics/impedance-control]]), and a leg-dynamics model — the IMU is what makes that fusion robust.

Skydio drones — onboard MSCKF VIO

The Skydio R1/R2/2+ consumer drones run a tightly-coupled MSCKF-class VIO (Mourikis-Roumeliotis 2007 lineage) on a Snapdragon Flight platform. State: IMU pose + velocity + biases + a sliding window of 25 past camera poses augmented into the state. Six fisheye cameras at 30 Hz each contribute keypoint observations, fused with IMU pre-integration factors. The result: < 1 % drift over 100 m of free flight without any GPS, sufficient to obstacle-avoid through forest canopies. Skydio’s published 2018 ICRA paper (Loianno-Brunner-Faessler-Scaramuzza et al. lineage) documents the design choices: sliding-window MSCKF over factor-graph back-end was chosen for power budget reasons; the latter would have given marginally better accuracy at ~3× compute.

Waymo / Cruise autonomous-vehicle stack

Waymo (5th-generation Driver, 2024) and Cruise (Origin, EoL late 2023) both run multi-layer fusion: tightly-coupled GNSS-INS at the bottom (proprietary IMUs, RTK GNSS, wheel-encoder feedback), an HD-map matching layer that fuses high-resolution LiDAR scans against a pre-built map, and a separate object-tracking stack (GM-PHD or JPDA) for surrounding agents. The state estimate at the top is published at 100 Hz to the planner with sub-decimetre uncertainty on a clear day, degrading gracefully in heavy rain (LiDAR clutter), snow (point cloud sparsity), and GPS-denied urban canyons (NDT scan-matching to map fills the gap). Both stacks rely on factor-graph smoothers (in-house, but architecturally similar to GTSAM/iSAM2) for the back-end, with sliding-window EKF for the real-time front-end.

Snap Spectacles / Meta Quest / Apple Vision Pro — VIO at AR scale

Modern AR/VR headsets run VIO on dedicated silicon (Meta XR2, Snap’s Spectacles SoC, Apple R1) at 1 kHz IMU + 60–120 Hz camera with sub-millisecond latency between motion and display update. Filter: hardware-accelerated EKF or sliding-window smoother; state similar to Skydio but with the camera-to-IMU extrinsic also estimated online. Drift requirement: < 1 cm over a 5 m room, < 1° rotation after 10 minutes. Mahalanobis gating on every visual feature update is critical — a single mismatched feature on a textureless wall would otherwise corrupt the pose for hundreds of milliseconds, manifesting as visible “jump” in the virtual content.

Roomba 980 / Roomba j7 — VSLAM + low-cost MCL

iRobot’s Roomba 980 (2015) introduced VSLAM based on a single upward-facing camera + a wheel-odometry EKF. The 2D pose-tracking filter is a simple EKF; the visual back-end maintains a sparse map of ceiling features. The j7 (2021) added bottom-facing obstacle classification, but the localisation core remains a wheel-odom + visual-feature EKF augmented periodically with map alignment. Production deployment: tens of millions of units, near-zero field tuning required — a case study in robust filter design at consumer scale.

Apollo Lunar Module — the first robotic Kalman filter

Stanley Schmidt at NASA Ames adapted Kalman’s 1960 paper to the Apollo Guidance Computer state estimator (Schmidt 1966). Running in 11 kB of read-only core memory on the 4 kHz AGC, the Schmidt-Kalman filter estimated the lunar module’s position and velocity from star-tracker fixes during translunar coast and radar-altimeter measurements during descent. To fit the budget, Schmidt introduced what’s now called the consider-state Kalman filter: states with poorly known biases (like sextant alignment) are modeled in the covariance but not actually estimated — they affect $P$ growth without consuming compute on their update. This trick remains standard in modern spacecraft INS (NG SIRU, Honeywell HG9900) where the gravity-anomaly model has 200+ states but only the first ~40 are actively estimated.

Skydio R1 lessons learned (Hertel et al. 2018 ICRA)

The Skydio R1’s tightly-coupled VIO ran into early issues with insufficient initial bias estimation: a 1° accel-bias error at takeoff produced 30 cm position error after 10 s of flight. Fix: enforce a 10 s pre-flight bias-estimation phase with the drone resting on the ground; ZUPT pseudo-measurements collapse the bias variance before flight. Subsequent products (Skydio 2, 2+) carry this initialisation discipline forward and reach < 5 cm drift over a typical 60 s flight.

9. Cross-references

• [[Robotics/sensors-pose-motion]] — IMU/encoder/resolver noise specs and Allan-variance characterisation feed directly into the $Q$, $R$ matrices of every filter here.
• [[Robotics/sensors-perception]] — LiDAR, RGB-D, ToF, and camera measurement models feed the update step; multi-target tracking machinery extends what’s here.
• [[Robotics/sensors-force-tactile]] — force/torque sensors and tactile arrays similarly require filtering and bias estimation; the same EKF templates apply.
• [[Robotics/state-space-lqr]] — LQR/Kalman duality; LQG is exactly the LQR controller closed around the Kalman estimate from this note.
• [[Robotics/pid-control]] — when fusion is overkill, a complementary filter + PID can be the right answer; see PID note for the simpler tool.
• [[Robotics/impedance-control]] — uses fused force/state estimates; foot-contact inference depends on the IMU stack discussed here.
• [[Robotics/slam]] — SLAM back-ends use factor-graph smoothers (iSAM2, GTSAM) that are the multi-pose generalisation of the iterated EKF here.
• [[Robotics/kinematics-dh]] — forward kinematics evaluates the measurement model $h(x)$ for end-effector measurements.
• [[Robotics/dynamics-rigid-body]] — supplies the motion model $f(x,u)$ that gets linearised in the EKF / propagated by sigma points in the UKF.
• [[Robotics/trajectory-generation]] — planners consume the filtered state; latency and uncertainty of the filter shape the achievable plan.
• [[Engineering/state-space-methods]] — the full mathematical theory (Riccati equation, observability Gramian, separation principle).
• [[Engineering/digital-control]] — discretisation of the continuous-time filter onto an MCU.
• [[Engineering/classical-control]] — complementary filter as the linear-Gaussian limit when one channel is high-pass and one is low-pass.
• [[Languages/Tier3/robotics-control]] — ROS 2 messages and DDS QoS choices for streaming fused state.

10. Citations

• Kalman, R. E. (1960). “A New Approach to Linear Filtering and Prediction Problems.” Journal of Basic Engineering, 82(D), 35–45. The foundational paper.
• Thrun, S., Burgard, W. & Fox, D. (2005). Probabilistic Robotics. MIT Press. Canonical robotics-estimation textbook; covers Bayes filter, KF/EKF/UKF, particle filter, MCL, EKF-SLAM, FastSLAM.
• Barfoot, T. D. (2024). State Estimation for Robotics (2nd ed.). Cambridge University Press. Free PDF at the author’s site. The modern textbook; emphasises Lie-group / manifold-aware estimation.
• Simon, D. (2006). Optimal State Estimation: Kalman, $H_{\infty }$, and Nonlinear Approaches. Wiley-Interscience. Comprehensive treatment with worked code.
• Bar-Shalom, Y., Li, X.-R. & Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. Wiley. The canonical tracking / NIS / OOSM reference.
• Maybeck, P. S. (1979). Stochastic Models, Estimation, and Control (Vols 1–3). Academic Press. Foundational engineering treatment of stochastic estimation.
• Julier, S. J. & Uhlmann, J. K. (1997). “A New Extension of the Kalman Filter to Nonlinear Systems.” Proc. AeroSense, SPIE 3068, 182–193. The UKF paper.
• Wan, E. A. & van der Merwe, R. (2001). “The Unscented Kalman Filter.” In Kalman Filtering and Neural Networks (S. Haykin, ed.). Wiley. The implementation-focused UKF reference; sigma-point scaling, $\alpha ,\beta ,\kappa$ choice.
• Arulampalam, M. S., Maskell, S., Gordon, N. & Clapp, T. (2002). “A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking.” IEEE Transactions on Signal Processing, 50(2), 174–188. The canonical PF tutorial.
• Gordon, N. J., Salmond, D. J. & Smith, A. F. M. (1993). “Novel Approach to Nonlinear/Non-Gaussian Bayesian State Estimation.” IEE Proceedings F, 140(2), 107–113. The bootstrap-particle-filter paper.
• Dellaert, F., Fox, D., Burgard, W. & Thrun, S. (1999). “Monte Carlo Localization for Mobile Robots.” Proc. ICRA, 1322–1328. The MCL paper.
• Fox, D. (2003). “Adaptive Particle Filters.” International Journal of Robotics Research, 22(12), 985–1003. KLD-sampling.
• Lefferts, E. J., Markley, F. L. & Shuster, M. D. (1982). “Kalman Filtering for Spacecraft Attitude Estimation.” Journal of Guidance, Control, and Dynamics, 5(5), 417–429. The Multiplicative EKF.
• Markley, F. L. (2007). “Averaging Quaternions.” Journal of Guidance, Control, and Dynamics, 30(4), 1193–1197. The quaternion-mean algorithm used in UKF on $S^3$.
• Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508. The modern practitioner’s tutorial.
• Mourikis, A. I. & Roumeliotis, S. I. (2007). “A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation.” Proc. ICRA, 3565–3572. The MSCKF that underpins OpenVINS / Skydio / Snap Spectacles VIO.
• Dellaert, F. & Kaess, M. (2017). “Factor Graphs for Robot Perception.” Foundations and Trends in Robotics, 6(1–2), 1–139. The modern factor-graph reference.
• Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. J. & Dellaert, F. (2012). “iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree.” IJRR, 31(2), 216–235. The iSAM2 algorithm.
• Bierman, G. J. (1977). Factorization Methods for Discrete Sequential Estimation. Academic Press. UD decomposition; the standard reference for numerically-stable embedded Kalman filters.
• Bell, B. M. & Cathey, F. W. (1993). “The Iterated Kalman Filter Update as a Gauss-Newton Method.” IEEE Transactions on Automatic Control, 38(2), 294–297.
• Pitt, M. K. & Shephard, N. (1999). “Filtering via Simulation: Auxiliary Particle Filters.” Journal of the American Statistical Association, 94(446), 590–599.
• Agarwal, P., Tipaldi, G. D., Spinello, L., Stachniss, C. & Burgard, W. (2013). “Robust Map Optimization Using Dynamic Covariance Scaling.” Proc. ICRA, 62–69.
• Carlson, N. A. (1990). “Federated Square Root Filter for Decentralized Parallel Processes.” IEEE Transactions on Aerospace and Electronic Systems, 26(3), 517–525.
• Williams, P. & Chambers, A. (2020). “PX4 EKF2 Estimator Documentation.” PX4 Autopilot user guide, https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf.html.
• Maimone, M., Cheng, Y. & Matthies, L. (2007). “Two Years of Visual Odometry on the Mars Exploration Rovers.” Journal of Field Robotics, 24(3), 169–186.
• Matthies, L. & Maimone, M. (2007). “Visual Odometry on the Mars Exploration Rovers — a Tool to Ensure Accurate Driving and Science Imaging.” IEEE Robotics & Automation Magazine, 13(2), 49–61.
• Labbe, R. R. (2018). Kalman and Bayesian Filters in Python. Free book at https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python. Pedagogical companion to filterpy.
• Dellaert, F. (2012). “Factor Graphs and GTSAM: A Hands-on Introduction.” Georgia Tech Tech Report GT-RIM-CP&R-2012-002.
• Moore, T. & Stouch, D. (2014). “A Generalized Extended Kalman Filter Implementation for the Robot Operating System.” Proc. International Conference on Intelligent Autonomous Systems. The robot_localization paper.
• ArduPilot Development Team (2024). AP_NavEKF3 source code and documentation, https://ardupilot.org/dev/docs/ekf3-affinity-and-lane-switching.html.
• Strasdat, H. (2012). “Local Accuracy and Global Consistency for Efficient Visual SLAM.” PhD thesis, Imperial College London. The Sophus C++ library origin.
• Schmidt, S. F. (1966). “Application of State-Space Methods to Navigation Problems.” Advances in Control Systems, Vol. 3, Academic Press. The original Apollo Kalman implementation paper; introduces the consider-state extension.
• Forster, C., Carlone, L., Dellaert, F. & Scaramuzza, D. (2016). “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry.” IEEE Transactions on Robotics, 33(1), 1–21. The modern IMU pre-integration reference.
• Lupton, T. & Sukkarieh, S. (2012). “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions.” IEEE Transactions on Robotics, 28(1), 61–76. The original IMU pre-integration paper.
• Mahony, R., Hamel, T. & Pflimlin, J.-M. (2008). “Nonlinear Complementary Filters on the Special Orthogonal Group.” IEEE Transactions on Automatic Control, 53(5), 1203–1218. The reference geometric complementary filter for SO(3).
• Barrau, A. & Bonnabel, S. (2017). “The Invariant Extended Kalman Filter as a Stable Observer.” IEEE Transactions on Automatic Control, 62(4), 1797–1812.
• Hartley, R., Ghaffari, M., Eustice, R. M. & Grizzle, J. W. (2020). “Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation.” International Journal of Robotics Research, 39(4), 402–430.
• Mahony, R., van Goor, P. & Hamel, T. (2023). “Observer Design for Non-Linear Systems with Equivariance.” Annual Review of Control, Robotics, and Autonomous Systems, 6, 221–252.
• Arasaratnam, I. & Haykin, S. (2009). “Cubature Kalman Filters.” IEEE Transactions on Automatic Control, 54(6), 1254–1269. A cleaner alternative to the UKF.
• Foxlin, E. (2005). “Pedestrian Tracking with Shoe-Mounted Inertial Sensors.” IEEE Computer Graphics and Applications, 25(6), 38–46. The ZUPT paper.
• Kelly, J. & Sukhatme, G. S. (2011). “Visual-Inertial Sensor Fusion: Localization, Mapping and Sensor-to-Sensor Self-Calibration.” International Journal of Robotics Research, 30(1), 56–79.
• Martinelli, A. (2014). “Closed-Form Solution of Visual-Inertial Structure from Motion.” International Journal of Computer Vision, 106(2), 138–152. Observability proofs for VIO scale recovery.
• Mehra, R. K. (1970). “On the Identification of Variances and Adaptive Kalman Filtering.” IEEE Transactions on Automatic Control, 15(2), 175–184.
• Blom, H. A. P. & Bar-Shalom, Y. (1988). “The Interacting Multiple Model Algorithm for Systems with Markovian Switching Coefficients.” IEEE Transactions on Automatic Control, 33(8), 780–783.
• Särkkä, S. & Nummenmaa, A. (2009). “Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations.” IEEE Transactions on Automatic Control, 54(3), 596–600.
• Van Loan, C. (1978). “Computing Integrals Involving the Matrix Exponential.” IEEE Transactions on Automatic Control, 23(3), 395–404. The matrix-exponential trick for exact discretisation.
• Hertel, A., Lott, A., Bay, S., et al. (2018). “Skydio R1 Visual Inertial Localization System.” Presentation at ICRA Workshop on Aerial Swarms.
• Higgins, W. T. (1975). “A Comparison of Complementary and Kalman Filtering.” IEEE Transactions on Aerospace and Electronic Systems, AES-11(3), 321–325.
• Bonnabel, S. (2007). “Left-Invariant Extended Kalman Filter and Attitude Estimation.” Proc. IEEE Conf. Decision and Control, 1027–1032.