Introduction

This library implements the Square-Root Unscented Kalman Filter (SR-UKF), a numerically robust algorithm for state estimation in nonlinear dynamic systems. It's designed both for practical use and as a learning resource.

Why Kalman Filtering?

Imagine you're tracking a robot's position. You have:

A motion model predicting where the robot should be
Sensor readings telling you where sensors think it is

Neither source is perfect. The motion model accumulates drift; sensors are noisy. The Kalman filter optimally fuses these imperfect sources, weighting each by its uncertainty. The result: an estimate better than either source alone.

The Unscented Transform: Intuition

The classic Kalman filter only works for linear systems. For nonlinear systems, we need tricks. The Extended Kalman Filter (EKF) linearizes via Jacobians, but this can be inaccurate and requires you to derive analytical derivatives.

The Unscented Kalman Filter (UKF) takes a different approach:

‍"It's easier to approximate a probability distribution than to approximate an arbitrary nonlinear function." — Julier & Uhlmann

Instead of linearizing the function, the UKF:

Picks a small set of carefully chosen sigma points that capture the mean and covariance of the current state estimate
Propagates each sigma point through the (possibly nonlinear) function
Reconstructs the mean and covariance from the transformed points

This captures nonlinear effects up to 2nd order (vs 1st for EKF) without needing any Jacobians.

Sigma Points: The Core Idea

For an N-dimensional state, we generate \(2N+1\) sigma points:

\[ \begin{aligned} \chi_0 &= \bar{x} & \text{(the mean)} \\ \chi_i &= \bar{x} + \gamma \cdot S_{:,i} & i = 1, \ldots, N \\ \chi_{i+N} &= \bar{x} - \gamma \cdot S_{:,i} & i = 1, \ldots, N \end{aligned} \]

Where:

\(\bar{x}\) is the current state estimate
\(S\) is the square-root of the covariance (so \(P = SS^T\))
\(\gamma = \sqrt{N + \lambda}\) is a scaling factor
\(\lambda = \alpha^2(N + \kappa) - N\) controls the spread

These points form a "cloud" around the mean that captures the uncertainty ellipse. When propagated through a nonlinear function, the cloud deforms, and we can recover the new mean and covariance from the deformed cloud.

Why Square-Root?

The standard UKF works with the full covariance matrix \(P\), but this has problems:

Numerical instability: Small numerical errors can make \(P\) non-positive-definite, causing the filter to fail
Efficiency: We often need \(\sqrt{P}\) anyway (for sigma points)

The Square-Root UKF works directly with \(S\) where \(P = SS^T\):

\(S\) is always well-defined (no need to ensure positive-definiteness)
Numerical precision is effectively doubled (errors in \(S\) become squared errors in \(P\))
The Cholesky decomposition is computed only once at initialization

The key operations become:

QR decomposition to compute square-root of sums of outer products
Cholesky rank-1 updates/downdates to efficiently modify \(S\)

Algorithm Overview

Predict Step

Given state \(\hat{x}\) with sqrt-covariance \(S\):

Generate sigma points from \((\hat{x}, S)\)
Propagate each through the process model: \(\mathcal{Y}_i = f(\chi_i)\)
Compute predicted mean: \(\bar{x}^- = \sum w^m_i \mathcal{Y}_i\)
Compute deviations: \(\mathcal{D}_i = \sqrt{|w^c_i|}(\mathcal{Y}_i - \bar{x}^-)\)
Update sqrt-covariance via QR: \(S^- = \text{qr}\left([\mathcal{D}_{1:2N}^T \;|\; Q_{sqrt}^T]\right)^T\)
If \(w^c_0 < 0\), apply Cholesky downdate with \(\mathcal{D}_0\)

Correct Step

Given predicted state \((\bar{x}^-, S^-)\) and measurement \(z\):

Generate sigma points from \((\bar{x}^-, S^-)\)
Propagate through measurement model: \(\mathcal{Z}_i = h(\chi_i)\)
Compute predicted measurement: \(\bar{z} = \sum w^m_i \mathcal{Z}_i\)
Compute measurement sqrt-covariance \(S_{yy}\) via QR (like step 4-6 above)
Compute cross-covariance: \(P_{xz} = \sum w^c_i (\chi_i - \bar{x}^-)(\mathcal{Z}_i - \bar{z})^T\)
Compute Kalman gain: \(K = P_{xz} S_{yy}^{-T} S_{yy}^{-1}\)
Update state: \(\hat{x} = \bar{x}^- + K(z - \bar{z})\)
Update sqrt-covariance via M Cholesky downdates: \((S^-)^2 \leftarrow (S^-)^2 - (KS_{yy})(KS_{yy})^T\)

Tuning Parameters

The UKF has three tuning parameters ( \(\alpha, \beta, \kappa\)):

Parameter	Typical Value	Effect
\(\alpha\)	0.001 - 1	Controls sigma point spread. Smaller = tighter

The derived parameter \(\lambda = \alpha^2(N + \kappa) - N\) determines the actual spread. For small \(\alpha\), \(\lambda \approx -N\), which can make \(w^c_0\) negative. This library handles negative \(w^c_0\) correctly via Cholesky downdates.

Basic Usage

// 1. Create filter (N=3 states, M=2 measurements)
srukf *ukf = srukf_create(3, 2);
 
// 2. Set noise covariances (square-roots)
srukf_mat *Qsqrt = SRUKF_MAT_ALLOC(3, 3);  // process noise
srukf_mat *Rsqrt = SRUKF_MAT_ALLOC(2, 2);  // measurement noise
// ... fill in Qsqrt, Rsqrt as lower-triangular Cholesky factors
srukf_set_noise(ukf, Qsqrt, Rsqrt);
 
// 3. Initialize state
srukf_reset(ukf, 1.0);  // zero state, identity covariance scaled by 1.0
 
// 4. Run filter loop
while (have_data) {
    srukf_predict(ukf, process_model, NULL);
    srukf_correct(ukf, measurement, measurement_model, NULL);
 
    // Read out current estimate
    srukf_get_state(ukf, state_out);
}
 
// 5. Cleanup
srukf_free(ukf);

Thread Safety

This library is NOT thread-safe:

The diagnostic callback (srukf_set_diag_callback()) is global
Each srukf instance should only be accessed from one thread

Dependencies

Requires CBLAS and LAPACKE (e.g., OpenBLAS).

Attribution

Matrix utilities derived from LAH (Linear Algebra Helpers) by maj0e, MIT License. See https://github.com/maj0e/linear-algebra-helpers

References

Julier, S.J. & Uhlmann, J.K. (1997). "A New Extension of the Kalman Filter to Nonlinear Systems"
Van der Merwe, R. & Wan, E.A. (2001). "The Square-Root Unscented Kalman Filter for State and Parameter-Estimation"
Wan, E.A. & Van der Merwe, R. (2000). "The Unscented Kalman Filter for Nonlinear Estimation"