Sigma-point state estimation in Rust, with the variants that make a filter a fair baseline: residual gating, adaptive process noise, fading memory, consider covariance, bias-state augmentation and a multiple-model bank — plus the diagnostics that say whether the covariance it reports means anything.

[dependencies]
nonlinear-filters = "0.1"

What is here


`Ukf`	unscented Kalman filter, const-generic over the state dimension
`ScaledUnscented`	the scaled unscented transform, behind a `SigmaPoints` trait
`ResidualGate`	chi-squared test on the normalized innovation squared
`CovarianceMatching`, `InnovationNoise`, `NisInflation`	three adaptive process-noise rules
`Ukf::with_fading_memory`	exponential forgetting factor
`ConsiderUkf`	Schmidt-Kalman consider covariance for unestimated parameters
`bias::BiasAugmentedProcess`, `bias::BiasAugmentedMeasurement`	wrappers that add measurement bias states
`Imm`	interacting multiple model filter over a bank of process models
`diagnostics`	NIS, NEES, chi-squared bounds, ensemble coverage
`detection`	threshold-plus-persistence detectors and empirical threshold calibration
`chi2`	the chi-squared distribution function and quantile
`sim`	a reproducible generator and a truth-and-measurement simulator

The state and measurement dimensions are const generics, so every matrix in a filter step is stack-allocated and no step allocates once the buffers are warm. The measurement dimension belongs to the update rather than to the filter, so one filter can be fed range at one step and angles at the next.

Using it

Define how the state moves and what is measured, then run the filter.

use nalgebra::{Matrix1, Matrix2, Vector1, Vector2};
use nonlinear_filters::prelude::*;

// A one-dimensional constant-velocity target, driven by acceleration noise.
struct ConstantVelocity {
    intensity: f64,
}

impl ProcessModel<2> for ConstantVelocity {
    fn propagate(&self, _time: f64, step: f64, state: &Vector2<f64>) -> Vector2<f64> {
        Vector2::new(state[0] + step * state[1], state[1])
    }

    fn noise(&self, _time: f64, step: f64, _state: &Vector2<f64>) -> Matrix2<f64> {
        let q = self.intensity;
        Matrix2::new(
            q * step.powi(3) / 3.0, q * step.powi(2) / 2.0,
            q * step.powi(2) / 2.0, q * step,
        )
    }
}

// A sensor that reports position, badly.
struct Position {
    deviation: f64,
}

impl MeasurementModel<2, 1> for Position {
    fn predict(&self, _time: f64, state: &Vector2<f64>) -> Vector1<f64> {
        Vector1::new(state[0])
    }

    fn noise(&self, _time: f64) -> Matrix1<f64> {
        Matrix1::new(self.deviation * self.deviation)
    }
}

let process = ConstantVelocity { intensity: 0.2 };
let sensor = Position { deviation: 1.0 };

// A gated filter with adaptive process noise, started from a loose prior.
let prior = Gaussian::from_deviations(Vector2::new(0.0, 0.0), Vector2::new(10.0, 5.0));
// The window and the relaxation are both tuning parameters; `inflating` picks a
// default relaxation that is a starting point, not an answer.
let mut filter = Ukf::new(prior)
    .with_gate(ResidualGate::new(0.99)?)
    .with_adaptation(CovarianceMatching::inflating(20)?.with_relaxation(0.1)?);

for step in 0..100 {
    let time = step as f64 * 0.5;
    filter.predict(&process, time, 0.5)?;

    let measurement = Vector1::new(0.37 * (time + 0.5));
    let report = filter.update(&sensor, time + 0.5, &measurement)?;

    if !report.accepted {
        // The gate threw it out. `report.nis` says by how much.
    }
}

let estimate = filter.estimate;
# assert!(estimate.covariance[(0, 0)] > 0.0);
# Ok::<(), nonlinear_filters::FilterError>(())

Examples

cargo run --example simple        # the smallest thing that is still a real filter
cargo run --example equipped      # gate, adaptive noise and forgetting, against a plain filter
cargo run --example diagnostics   # consistency over an ensemble, then detector calibration

diagnostics is the one to read if the next two sections do not land: it prints the numbers alongside what to conclude from them.

Checking that the covariance means something

A filter reports an uncertainty, and that is a claim about its own errors. Two statistics test it: NIS, which needs only the filter's output and so works on live data, and NEES, which needs the truth and so works in simulation. Either one running above its expectation says the filter is overconfident.

A single sample says almost nothing — the 95% interval for one degree of freedom spans [0.001, 5.02] — so the test belongs over an ensemble.

use nonlinear_filters::diagnostics::{self, ChiSquaredBounds};

# let nees_samples: Vec<f64> = (0..5000).map(|i| 2.0 + 0.4 * ((i % 7) as f64 - 3.0)).collect();
// Averaging 5000 NEES values from a two-state filter: the average should fall
// inside bounds that are much tighter than a single sample's.
let test = diagnostics::ensemble_test(nees_samples.iter().copied(), 2, 0.99)?;
if !test.passes() {
    // `test.mean` against `test.bounds`, and the direction says which way.
}

let single = ChiSquaredBounds::single(2, 0.95);
let averaged = ChiSquaredBounds::ensemble(2, 5000, 0.95);
assert!(averaged.upper - averaged.lower < (single.upper - single.lower) / 50.0);
# Ok::<(), nonlinear_filters::FilterError>(())

Comparing two detectors fairly

Time-to-detect is trivially won by lowering a threshold, so comparing two detectors means first matching their false-alarm rates. detection calibrates a threshold from nominal data, which works for any scalar statistic — including one with no known nominal distribution.

use nonlinear_filters::detection::{calibrate_per_run, Detector};

# let nominal: Vec<Vec<f64>> = (0..200).map(|r| (0..100).map(|k| 0.5 + ((r * 31 + k * 17) % 23) as f64 * 0.12).collect()).collect();
# let faulted: Vec<f64> = (0..100).map(|k| if k > 50 { 40.0 } else { 1.0 }).collect();
// One per cent of nominal runs may fire. A firing is three consecutive
// exceedances, so the persistence is part of what is being calibrated.
let calibration = calibrate_per_run(&nominal, 0.01, 3)?;

// Now a delay measured against that threshold means something.
let onset = 50;
let fired = Detector::first_firing(calibration.threshold, 3, faulted.iter().copied())?;
let delay = fired.map(|index| index.saturating_sub(onset));
# assert!(delay.is_some());
# Ok::<(), nonlinear_filters::FilterError>(())

What has been checked

Against the closed form. On a linear-Gaussian system the unscented transform is exact, so the filter has to reproduce the analytic Kalman filter. Over forty steps of a four-state system with correlated noise the largest disagreement in any covariance entry is 2.7e-15, against entries of order 9 — about twelve units in the last place. It holds for any alpha, beta and kappa.
Consistency over an ensemble. 200 runs of 60 steps: mean NEES 4.0177 against [3.9338, 4.0668] at 99% for four degrees of freedom, mean NIS 1.9675 against [1.9533, 2.0473], and a 95% coverage rate of 0.9536.
Each variant against its fault. Gating against injected 30-sigma outliers: position RMS 4.035 to 1.602. Adaptive noise against an unmodeled 0.3 m/s^2: mean NEES 30.7 to 4.2. Consider covariance against an unestimated scale factor: mean NEES 650 to 1.67, with the accuracy unchanged, which is the point. Bias augmentation against a 5-unit sensor offset: RMS 2.567 to 0.503 and the offset recovered as 4.985. An IMM across a mode switch: whole-arc RMS 0.770 against 4.060 for the quiet mode alone and 0.875 for the noisy one.
Nonlinear benchmarks. The univariate nonstationary growth model, where the measurement is blind to the sign of the state: RMSE 7.69 with the sign wrong on 36% of steps, which is the regime the literature reports for a sigma-point filter on that problem. Range-and-bearing tracking: mean NEES 3.861 against an expectation of 4.
The calibrator. On chi-squared samples with a known 99% point of 9.2103 it returns 9.1058, a 1.1% error against the 4.5% sampling error of that quantile at this sample size.

The numbers above are what the test suite measured, not estimates. cargo test re-runs all of it.

What it costs

A whole predict-and-update step, measured with criterion on one machine, for a constant-velocity state of the given width against a single range measurement:

state	plain	with a gate	with the covariance match
2	93 ns	94 ns	128 ns
6	298 ns	294 ns	415 ns
14	2.55 µs	2.65 µs	3.03 µs

Whether that is good needs something to compare against. On a four-state, two-measurement problem the closed-form Kalman filter — the floor, available only because that problem is linear — takes 50 ns against the filter's 178 ns. More useful: give the process model an RK4 propagation and compare against the cost of the propagations alone.

model	whole filter step	the 9 propagations alone	filter overhead
RK4, 1 substep	259 ns	92 ns	167 ns
RK4, 4 substeps	650 ns	481 ns	169 ns
RK4, 16 substeps	2.276 µs	2.086 µs	190 ns

The overhead is roughly constant, so a model that does any real work dominates the step. cargo bench --bench overhead re-runs that.

Re-running the identical binary moved individual figures by up to 5.8%, so that is the floor for reading anything into these:

A gate is free. Every plain-against-gated difference above is inside the noise. The gate caches its threshold per measurement dimension, which it has to: a chi-squared quantile is 1.63 µs, five times the whole six-state step, so computing one per update would dominate the filter.
The covariance match costs 35-40% at the two smaller sizes and 19% at the largest. That is the inversion of the state covariance needed for the filter's implied measurement sensitivity, and it is the only mechanism here that needs one. The other two adaptive rules work from scalars and a state-sized vector.
Sigma-point generation is 75 ns at six states, and a step generates two sets, so half the plain step is Cholesky factorization and point arithmetic.

cargo bench re-runs all of it.

License

MIT.