connor/nonlinear-filters
1
0 Fork 0
Code Issues 9 Pull Requests Actions Packages Projects Releases Wiki Activity

Examples, overhead benchmark, and a covariance-matching doc fix

Browse Source
This commit is contained in:
connor committed 2026-10-03 19:33:33 -04:00
1 parent a6e4aa7e0d
commit 481497d76b
10 files changed
+1097 -10
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

Cargo.toml
+4
View File
@@ -23,3 +23,7 @@ criterion = "0.7"
[[bench]]
name = "ukf"
harness = false
[[bench]]
name = "overhead"
harness = false
benches/overhead.rs
+195
View File
@@ -0,0 +1,195 @@
//! What a filter step costs, against two reference points.
//!
//! The per-dimension table in `ukf.rs` says what a step costs. It does not say
//! whether that is good, which needs something to compare against:
//!
//! - `analytic_kf` is the closed-form Kalman filter on the same problem. That is
//! the floor — the least arithmetic that answers the same question — and it is
//! available only because the problem is linear.
//! - each `ukf_rk4xN` against its `bare_rk4xN_x9` is the filter against the cost of
//! the propagations alone. The difference is the filter's own overhead, and the
//! interesting result is that it stays flat as the model gets more expensive.
//!
//! Nine propagations because a four-state scaled unscented transform uses `2N + 1`
//! points.
use criterion::{criterion_group, criterion_main, Criterion};
use nalgebra::{SMatrix, SVector};
use nonlinear_filters::prelude::*;
use std::hint::black_box;
type M4 = SMatrix<f64, 4, 4>;
type V4 = SVector<f64, 4>;
type H24 = SMatrix<f64, 2, 4>;
type M2 = SMatrix<f64, 2, 2>;
type V2 = SVector<f64, 2>;
fn transition() -> M4 {
M4::from_row_slice(&[
0.95, 0.40, 0.00, 0.10, //
-0.10, 0.90, 0.25, 0.00, //
0.00, 0.05, 0.85, 0.30, //
0.15, 0.00, -0.20, 0.70,
])
}
fn process_noise() -> M4 {
M4::from_row_slice(&[
0.40, 0.10, 0.05, 0.00, //
0.10, 0.30, 0.00, 0.02, //
0.05, 0.00, 0.25, 0.08, //
0.00, 0.02, 0.08, 0.20,
])
}
fn observation() -> H24 {
H24::from_row_slice(&[1.0, 0.0, 0.5, 0.0, 0.0, 1.0, 0.0, -0.3])
}
fn measurement_noise() -> M2 {
M2::from_row_slice(&[1.50, 0.25, 0.25, 0.80])
}
/// The dynamics, applied as one matrix product: as cheap as a process model gets.
struct Coasting {
transition: M4,
noise: M4,
}
impl ProcessModel<4> for Coasting {
fn propagate(&self, _time: f64, _step: f64, state: &V4) -> V4 {
self.transition * state
}
fn noise(&self, _time: f64, _step: f64, _state: &V4) -> M4 {
self.noise
}
}
/// The same dynamics, integrated rather than applied, so that the propagation
/// costs something. This is what a real process model looks like.
struct Integrated {
transition: M4,
noise: M4,
substeps: usize,
}
impl Integrated {
fn step(&self, state: &V4, step: f64) -> V4 {
let mut state = *state;
let h = step / self.substeps as f64;
for _ in 0..self.substeps {
let k1 = self.transition * state;
let k2 = self.transition * (state + k1 * (h * 0.5));
let k3 = self.transition * (state + k2 * (h * 0.5));
let k4 = self.transition * (state + k3 * h);
state += (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (h / 6.0);
}
state
}
}
impl ProcessModel<4> for Integrated {
fn propagate(&self, _time: f64, step: f64, state: &V4) -> V4 {
self.step(state, step)
}
fn noise(&self, _time: f64, _step: f64, _state: &V4) -> M4 {
self.noise
}
}
/// The measurement, as its own type — see the note in `nonlinear_filters::model`
/// on why it is not the same type as the process.
struct Observed {
observation: H24,
noise: M2,
}
impl MeasurementModel<4, 2> for Observed {
fn predict(&self, _time: f64, state: &V4) -> V2 {
self.observation * state
}
fn noise(&self, _time: f64) -> M2 {
self.noise
}
}
fn overhead(c: &mut Criterion) {
let sensor = Observed {
observation: observation(),
noise: measurement_noise(),
};
let start = Gaussian::new(V4::new(1.0, -2.0, 0.5, 3.0), M4::identity() * 4.0);
let z = V2::new(0.3, -0.4);
let mut group = c.benchmark_group("overhead");
// The floor: the closed-form filter for this problem.
group.bench_function("analytic_kf", |b| {
let (f, q, h, r) = (
transition(),
process_noise(),
observation(),
measurement_noise(),
);
b.iter(|| {
let mut mean = start.mean;
let mut covariance = start.covariance;
mean = f * mean;
covariance = f * covariance * f.transpose() + q;
let innovation = z - h * mean;
let s = h * covariance * h.transpose() + r;
let gain =
covariance * h.transpose() * s.try_inverse().expect("invertible by construction");
mean += gain * innovation;
covariance -= gain * h * covariance;
black_box((mean, covariance))
})
});
// The filter on the same problem, with the cheapest possible model.
group.bench_function("ukf_matrix_model", |b| {
let model = Coasting {
transition: transition(),
noise: process_noise(),
};
let mut filter = Ukf::new(start);
b.iter(|| {
filter.estimate = start;
filter.predict(&model, 0.0, 1.0).unwrap();
black_box(filter.update(&sensor, 1.0, &z).unwrap())
})
});
// And with a model that costs something, against the propagations alone.
for substeps in [1usize, 4, 16] {
let model = Integrated {
transition: transition(),
noise: process_noise(),
substeps,
};
group.bench_function(format!("ukf_rk4x{substeps}"), |b| {
let mut filter = Ukf::new(start);
b.iter(|| {
filter.estimate = start;
filter.predict(&model, 0.0, 1.0).unwrap();
black_box(filter.update(&sensor, 1.0, &z).unwrap())
})
});
group.bench_function(format!("bare_rk4x{substeps}_x9"), |b| {
b.iter(|| {
let mut total = V4::zeros();
for _ in 0..9 {
total += model.step(&black_box(start.mean), 1.0);
}
black_box(total)
})
});
}
group.finish();
}
criterion_group!(benches, overhead);
criterion_main!(benches);
examples/diagnostics.rs
+373
View File
@@ -0,0 +1,373 @@
//! Is the filter's covariance honest, and when did the model stop describing the
//! data?
//!
//! Two questions, and the two halves of this example. Neither can be answered by
//! looking at a single run, which is most of why they need their own machinery.
//!
//! Run with `cargo run --example diagnostics`.
use nalgebra::{Matrix1, Matrix2, Vector1, Vector2};
use nonlinear_filters::detection::{calibrate_per_run, calibrate_per_step, Detector};
use nonlinear_filters::diagnostics::{self, ChiSquaredBounds};
use nonlinear_filters::prelude::*;
use nonlinear_filters::sim::{simulate, GaussianSampler, Rng};
const STEP: f64 = 0.5;
const STEPS: usize = 100;
const SEEDS: usize = 200;
/// The step at which the faulted runs start accelerating.
const ONSET: usize = 50;
/// How many consecutive exceedances count as a detection.
const PERSISTENCE: usize = 3;
struct Coasting {
intensity: f64,
}
impl ProcessModel<2> for Coasting {
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,
)
}
}
/// Coasts, then accelerates from `onset`. The filter is never told.
struct Faulted {
inner: Coasting,
onset: f64,
acceleration: f64,
}
impl ProcessModel<2> for Faulted {
fn propagate(&self, time: f64, step: f64, state: &Vector2<f64>) -> Vector2<f64> {
let a = if time >= self.onset {
self.acceleration
} else {
0.0
};
Vector2::new(
state[0] + step * state[1] + 0.5 * a * step * step,
state[1] + a * step,
)
}
fn noise(&self, time: f64, step: f64, state: &Vector2<f64>) -> Matrix2<f64> {
self.inner.noise(time, step, state)
}
}
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)
}
}
/// What one ensemble of runs produced.
struct Ensemble {
/// NEES per step, pooled over runs. Needs the truth, so simulation only.
nees: Vec<f64>,
/// NIS per step, pooled over runs. Needs only the filter's own output.
nis: Vec<f64>,
/// NIS per step, kept per run, for the detector.
streams: Vec<Vec<f64>>,
}
/// Runs an ensemble: truth from `truth_process`, filter believing
/// `filter_process`.
fn run_ensemble(
truth_process: &dyn ProcessModel<2>,
filter_process: &dyn ProcessModel<2>,
sensor: &Position,
prior: &Gaussian<2>,
seed_base: u64,
draw_from_prior: bool,
) -> Result<Ensemble, FilterError> {
let prior_sampler = GaussianSampler::new(&prior.covariance);
let mut ensemble = Ensemble {
nees: Vec::new(),
nis: Vec::new(),
streams: Vec::new(),
};
for seed in 0..SEEDS as u64 {
let mut rng = Rng::new(seed_base + seed);
// The hypothesis under test says the truth is a draw from the prior, so
// that is how the truth is generated. Starting every run from the prior's
// mean instead would make the first steps inconsistent by construction and
// the diagnostic would be measuring the setup, not the filter.
let truth = if draw_from_prior {
prior.mean + prior_sampler.sample(&mut rng)
} else {
prior.mean
};
let run = simulate(truth_process, sensor, &truth, 0.0, STEP, STEPS, &mut rng)?;
let mut filter = Ukf::new(*prior);
let mut stream = Vec::with_capacity(run.len());
for step in 1..run.len() {
filter.predict(filter_process, run.times[step - 1], STEP)?;
let report = filter.update(sensor, run.times[step], &run.measurements[step])?;
ensemble.nis.push(report.nis);
stream.push(report.nis);
let error = filter.estimate.mean - run.states[step];
ensemble
.nees
.push(diagnostics::nees(&error, &filter.estimate.covariance)?);
}
ensemble.streams.push(stream);
}
Ok(ensemble)
}
fn main() -> Result<(), FilterError> {
let sensor = Position { deviation: 1.0 };
let nominal = Coasting { intensity: 0.3 };
let prior = Gaussian::from_deviations(Vector2::new(0.0, 1.0), Vector2::new(5.0, 2.0));
// ========================================================================
println!("part 1: is the covariance honest?");
println!("=================================\n");
println!("A filter reports an estimate and an uncertainty. The uncertainty is a");
println!("claim about its own errors, and it is testable. Two statistics:\n");
println!(" NEES = e' P^-1 e needs the truth, so simulation only.");
println!(" Should average the state dimension, here 2.");
println!(" NIS = d' S^-1 d needs only the filter's output, so it works live.");
println!(" Should average the measurement dimension, here 1.\n");
// Why one sample tells you nothing.
let single = ChiSquaredBounds::single(1, 0.95);
println!(
"One NIS sample from a one-component measurement has a 95% interval of\n [{:.4}, {:.3}] around an expectation of 1. That is why these are ensemble",
single.lower, single.upper
);
println!(" statistics: a single value of 4 means nothing at all.\n");
let good = run_ensemble(&nominal, &nominal, &sensor, &prior, 0x900d_0000, true)?;
let nees_test = diagnostics::ensemble_test(good.nees.iter().copied(), 2, 0.99)?;
let nis_test = diagnostics::ensemble_test(good.nis.iter().copied(), 1, 0.99)?;
println!("a correctly specified filter, {SEEDS} runs x {STEPS} steps:\n");
println!(" mean 99% interval for the mean verdict");
for (name, test) in [("NEES", &nees_test), ("NIS", &nis_test)] {
println!(
" {:<6} {:>10.4} [{:>7.4}, {:>7.4}] {}",
name,
test.mean,
test.bounds.lower,
test.bounds.upper,
if test.passes() { "consistent" } else { "FAILS" }
);
}
println!(
"\n Averaging {} samples shrank the NIS interval from [{:.4}, {:.3}] to\n [{:.4}, {:.4}] — a factor of {:.0} — which is what makes a verdict possible.",
nis_test.samples,
single.lower,
single.upper,
nis_test.bounds.lower,
nis_test.bounds.upper,
(single.upper - single.lower) / (nis_test.bounds.upper - nis_test.bounds.lower)
);
// Coverage is the same question asked per sample rather than on the mean.
let coverage = diagnostics::coverage(good.nis.iter().copied(), &single);
println!(
"\n Per-sample coverage against the 95% interval: {:.1}% inside, {} high,\n {} low, of {}. For a consistent filter that rate is the confidence level.",
100.0 * coverage.rate().unwrap_or(0.0),
coverage.above,
coverage.below,
coverage.samples
);
// The same filter, starved of process noise.
let starved = Coasting { intensity: 0.003 };
let bad = run_ensemble(&nominal, &starved, &sensor, &prior, 0xbad0_0000, true)?;
let bad_nees = diagnostics::ensemble_test(bad.nees.iter().copied(), 2, 0.99)?;
let bad_coverage =
diagnostics::coverage(bad.nees.iter().copied(), &ChiSquaredBounds::single(2, 0.95));
println!("\nthe same filter given a hundredth of the process noise it needs:\n");
println!(
" NEES {:>10.4} [{:>7.4}, {:>7.4}] {}",
bad_nees.mean,
bad_nees.bounds.lower,
bad_nees.bounds.upper,
if bad_nees.passes() {
"consistent"
} else {
"FAILS"
}
);
println!(
"\n It fails {:.0}x above the upper bound, and the direction is the diagnosis:\n {} samples above the interval against {} below. Above means overconfident —\n the covariance is smaller than the errors justify. Below would mean merely\n conservative, which wastes information but misleads nobody.",
bad_nees.mean / bad_nees.bounds.upper,
bad_coverage.above,
bad_coverage.below
);
// ========================================================================
println!("\n\npart 2: when did the model stop describing the data?");
println!("====================================================\n");
println!("NIS answers this one too, because it needs no truth. The question is");
println!("what threshold to compare it against, and the answer cannot be chosen");
println!("freely: a lower threshold always detects sooner, so a detection delay");
println!("quoted without its false-alarm rate is not a measurement.\n");
// Calibrate on nominal runs, where every firing is by definition a false alarm.
let calibration = calibrate_per_run(&good.streams, 0.01, PERSISTENCE)?;
println!(
"calibrating on {} nominal runs, for a 1% per-run false-alarm rate\nwith a persistence of {} consecutive exceedances:\n",
good.streams.len(),
PERSISTENCE
);
println!(
" threshold {:.4}, achieving {:.2}% on the calibration set",
calibration.threshold,
100.0 * calibration.achieved
);
// Now the faulted runs.
let faulted_process = Faulted {
inner: Coasting { intensity: 0.3 },
onset: ONSET as f64 * STEP,
acceleration: 1.5,
};
let faulted = run_ensemble(
&faulted_process,
&nominal,
&sensor,
&prior,
0xfa17_0000,
false,
)?;
let delays = |threshold: f64| -> (Vec<usize>, usize) {
let mut delays = Vec::new();
let mut early = 0;
for stream in &faulted.streams {
if let Some(index) =
Detector::first_firing(threshold, PERSISTENCE, stream.iter().copied())
.expect("the detector is validly configured")
{
// Stream index 0 is step 1, so the onset sits at index ONSET - 1.
if index + 1 >= ONSET {
delays.push(index + 1 - ONSET);
} else {
early += 1;
}
}
}
delays.sort_unstable();
(delays, early)
};
let (calibrated, early) = delays(calibration.threshold);
println!(
"\non {} faulted runs, the fault starting at step {}:\n",
faulted.streams.len(),
ONSET
);
println!(
" detected in {:.0}% of runs, {} fired before the onset",
100.0 * calibrated.len() as f64 / faulted.streams.len() as f64,
early
);
if !calibrated.is_empty() {
println!(
" time to detect: fastest {}, lower quartile {}, median {}, upper quartile {} steps",
calibrated[0],
calibrated[calibrated.len() / 4],
calibrated[calibrated.len() / 2],
calibrated[3 * calibrated.len() / 4],
);
}
// And the point of calibrating at all: a lower threshold looks better and is
// not comparable.
println!("\nwhat a lower threshold buys, and what it costs:\n");
println!(" threshold median delay detected fired early false alarms");
for factor in [1.0, 0.5, 0.25] {
let threshold = calibration.threshold * factor;
let (d, fired_early) = delays(threshold);
let nominal_firings = good
.streams
.iter()
.filter(|stream| {
Detector::first_firing(threshold, PERSISTENCE, stream.iter().copied())
.expect("validly configured")
.is_some()
})
.count();
println!(
" {:>10.4} {:>14} {:>10.0}% {:>13.0}% {:>13.1}%",
threshold,
if d.is_empty() {
"never".to_string()
} else {
d[d.len() / 2].to_string()
},
100.0 * d.len() as f64 / faulted.streams.len() as f64,
100.0 * fired_early as f64 / faulted.streams.len() as f64,
100.0 * nominal_firings as f64 / good.streams.len() as f64,
);
}
println!(
"\n Quartering the threshold improves the median delay from 13 steps to 6 and\n takes the false-alarm rate from 1% to 88%. Either number alone can be made to\n look like progress, which is why two detectors are only comparable once their\n false-alarm rates are matched."
);
println!(
"\n The 'detected' column falling as the threshold drops is not a bug. At a low\n threshold most faulted runs fire before step {} — see 'fired early' — so the\n detector is not detecting the fault at all, it is reacting to ordinary noise\n that happens to arrive first. A detector that fires early on most runs has no\n meaningful time to detect, however good the surviving median looks.",
ONSET
);
// The calibrator does not know or care what the statistic is.
println!("\nthe calibrator is not tied to NIS:\n");
println!(" (per-step calibration this time, 1% of all steps, so the thresholds");
println!(" are not comparable with the per-run figure above)\n");
let mut traces = Vec::new();
let mut raw = Vec::new();
for stream in good.streams.iter().take(50) {
for value in stream {
raw.push(value * 2.5 + 7.0);
traces.push(value.sqrt());
}
}
for (name, samples) in [
("NIS", good.nis.clone()),
("a rescaling of it", raw),
("its square root", traces),
] {
let c = calibrate_per_step(samples, 0.01)?;
println!(
" {:<20} threshold {:>8.4} achieved rate {:.3}%",
name,
c.threshold,
100.0 * c.achieved
);
}
println!(
"\n Three statistics on three scales, all calibrated to the same 1% of steps\n from samples alone. Nothing about their distributions was assumed, which is\n what lets a detector with no known nominal distribution be compared against\n one that has a textbook threshold."
);
Ok(())
}
examples/equipped.rs
+300
View File
@@ -0,0 +1,300 @@
//! The same filter with every mechanism turned on, against a plain one.
//!
//! Three things happen to this target that a textbook UKF handles badly:
//!
//! - one position measurement in twenty is wild, by thirty standard deviations
//! - the target starts accelerating halfway through the arc, and no model says so
//! - a second, better sensor reports occasionally, with a different number of
//! components from the first
//!
//! The equipped filter gets a chi-squared residual gate for the first, adaptive
//! process noise for the second, and nothing at all for the third, because the
//! measurement dimension belongs to [`Ukf::update`] rather than to the filter and
//! two sensors need no special arrangement.
//!
//! Run with `cargo run --example equipped`.
use nalgebra::{Matrix1, Matrix2, SMatrix, SVector, Vector1, Vector2};
use nonlinear_filters::prelude::*;
use nonlinear_filters::sim::{simulate, GaussianSampler, Rng};
const STEP: f64 = 0.5;
const STEPS: usize = 120;
/// The step at which the target starts accelerating.
const ONSET: usize = 60;
/// How often the better sensor reports.
const FIX_EVERY: usize = 10;
/// What the filter believes: a target that coasts.
struct Coasting {
intensity: f64,
}
impl ProcessModel<2> for Coasting {
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,
)
}
}
/// What the target actually does: coasts, then accelerates from `onset`.
struct Maneuvering {
inner: Coasting,
onset: f64,
acceleration: f64,
}
impl ProcessModel<2> for Maneuvering {
fn propagate(&self, time: f64, step: f64, state: &Vector2<f64>) -> Vector2<f64> {
let a = if time >= self.onset {
self.acceleration
} else {
0.0
};
Vector2::new(
state[0] + step * state[1] + 0.5 * a * step * step,
state[1] + a * step,
)
}
fn noise(&self, time: f64, step: f64, state: &Vector2<f64>) -> Matrix2<f64> {
self.inner.noise(time, step, state)
}
}
/// The everyday sensor: position only, one component.
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)
}
}
/// The occasional good sensor: position and velocity, two components.
///
/// Nothing about the filter changes to accept this. `M` is a parameter of
/// `update`, so the same filter takes a one-component measurement at one step and
/// a two-component one at the next.
struct Fix {
position_deviation: f64,
velocity_deviation: f64,
}
impl MeasurementModel<2, 2> for Fix {
fn predict(&self, _time: f64, state: &Vector2<f64>) -> Vector2<f64> {
*state
}
fn noise(&self, _time: f64) -> Matrix2<f64> {
SMatrix::<f64, 2, 2>::new(
self.position_deviation * self.position_deviation,
0.0,
0.0,
self.velocity_deviation * self.velocity_deviation,
)
}
}
/// What one filter did over the whole arc.
#[derive(Default)]
struct Tally {
squared_error: f64,
samples: usize,
rejected: usize,
}
impl Tally {
fn record(&mut self, error: f64) {
self.squared_error += error * error;
self.samples += 1;
}
fn rms(&self) -> f64 {
(self.squared_error / self.samples as f64).sqrt()
}
}
fn main() -> Result<(), FilterError> {
let filter_process = Coasting { intensity: 0.05 };
let truth_process = Maneuvering {
inner: Coasting { intensity: 0.05 },
onset: ONSET as f64 * STEP,
acceleration: 0.6,
};
let position = Position { deviation: 1.0 };
let fix = Fix {
position_deviation: 0.3,
velocity_deviation: 0.1,
};
// Truth and the position sensor's stream.
let mut rng = Rng::new(20_261_003);
let mut run = simulate(
&truth_process,
&position,
&Vector2::new(0.0, 2.0),
0.0,
STEP,
STEPS,
&mut rng,
)?;
// One position measurement in twenty, displaced by thirty standard deviations.
let mut outliers = Vec::new();
for step in 1..run.len() {
if rng.uniform() < 0.05 {
let sign = if rng.uniform() < 0.5 { -1.0 } else { 1.0 };
run.measurements[step][0] += sign * 30.0 * position.deviation;
outliers.push(step);
}
}
// The good sensor's readings, synthesized from the same truth.
let fix_sampler = GaussianSampler::new(&fix.noise(0.0));
let fixes: Vec<SVector<f64, 2>> = run
.states
.iter()
.map(|truth| truth + fix_sampler.sample(&mut rng))
.collect();
let prior = Gaussian::from_deviations(Vector2::new(0.0, 0.0), Vector2::new(10.0, 5.0));
// The textbook filter: no gate, no adaptation, no forgetting.
let mut plain = Ukf::new(prior);
// The equipped one. Each addition answers one of the three faults above.
let mut equipped = Ukf::new(prior)
// Reject a residual too large to be this filter's own noise. 0.99 throws
// away one good measurement in a hundred to do it.
.with_gate(ResidualGate::new(0.99)?)
// Let the innovations set the size of Q. One free parameter, the window.
.with_adaptation(CovarianceMatching::inflating(20)?)
// And discount old measurements slightly, so the covariance keeps a floor
// under it and the filter never stops listening.
.with_fading_memory(1.02)?;
let mut plain_tally = Tally::default();
let mut equipped_tally = Tally::default();
println!("step position NIS gate fix Q scale plain err equipped err");
println!(" ------------------------------------------------------------------------------");
for (step, fix_reading) in fixes.iter().enumerate().skip(1) {
let time = run.times[step - 1];
let now = run.times[step];
plain.predict(&filter_process, time, STEP)?;
equipped.predict(&filter_process, time, STEP)?;
plain.update(&position, now, &run.measurements[step])?;
let report = equipped.update(&position, now, &run.measurements[step])?;
if !report.accepted {
equipped_tally.rejected += 1;
}
// Every tenth step the better sensor also reports. Two updates in one
// step, with different measurement dimensions, through one filter.
let fixed = step % FIX_EVERY == 0;
if fixed {
plain.update(&fix, now, fix_reading)?;
equipped.update(&fix, now, fix_reading)?;
}
let truth = run.states[step][0];
let plain_error = plain.estimate.mean[0] - truth;
let equipped_error = equipped.estimate.mean[0] - truth;
plain_tally.record(plain_error);
equipped_tally.record(equipped_error);
let interesting = !report.accepted || fixed || step == ONSET || step % 30 == 0;
if interesting {
println!(
"{:4} {:10.2} {:8.1} {:>6} {:>5} {:10.1} {:11.3} {:14.3}",
step,
run.measurements[step][0],
report.nis,
if report.accepted { "pass" } else { "REJECT" },
if fixed { "yes" } else { "" },
equipped.adaptation().scale(),
plain_error,
equipped_error,
);
}
}
println!();
println!(
"over {} steps, with {} outliers injected and the maneuver from step {}:",
STEPS,
outliers.len(),
ONSET
);
println!();
println!(" plain equipped");
println!(
" position RMS {:>8.3} {:>8.3}",
plain_tally.rms(),
equipped_tally.rms()
);
println!(
" measurements rejected {:>5} {:>5}",
plain_tally.rejected, equipped_tally.rejected
);
println!(
" final 1 sigma {:>8.3} {:>8.3}",
plain.estimate.covariance[(0, 0)].sqrt(),
equipped.estimate.covariance[(0, 0)].sqrt()
);
println!(
" final Q scale 1.0 {:>8.1}",
equipped.adaptation().scale()
);
// What to read in that table, and what not to.
//
// **The gate rejects eleven for ten injected outliers.** The ten are
// unmistakable: their NIS is in the hundreds against a threshold of 6.63. The
// eleventh is a perfectly good measurement thrown away, and it is not a bug —
// `ResidualGate::new(0.99)` promises to discard one good measurement in a
// hundred, and 120 steps bought about one. Tightening the gate to 0.999 would
// buy that measurement back and start letting smaller outliers through.
//
// **The Q scale does not glide up at the maneuver; it thrashes.** It is at the
// clamp of 1.0 most of the time and jumps into the hundreds for a few steps at
// a stretch. That is this mechanism's real behavior, and it is a feedback loop
// rather than noise: inflating the scale makes the next prediction's covariance
// larger, which makes the innovations look explicable again, which collapses
// the scale back to the clamp. A longer window damps the amplitude without
// removing the oscillation. It still works — see the RMS — but anyone plotting
// the scale expecting a tracking signal will be surprised.
//
// **The plain filter's final 1 sigma is the number to be suspicious of.** It is
// *smaller* than the equipped filter's while its RMS is nearly three times
// larger. A filter reporting more confidence than its errors justify is the
// failure that matters, because every decision downstream believes it. That is
// exactly what the diagnostics in `diagnostics.rs` are for, and no amount of
// staring at a single run reveals it.
//
// One thing deliberately not claimed: that adaptation beats any fixed process
// noise. It beats *this* filter's declared 0.05 by a wide margin. A fixed
// intensity of 1.0, had anyone known to choose it, beats the adaptive filter on
// this scenario. Adaptation buys robustness to a process noise that was set
// wrong, not accuracy beyond one that was set right.
Ok(())
}
examples/simple.rs
+126
View File
@@ -0,0 +1,126 @@
//! A bare unscented Kalman filter, start to finish.
//!
//! The smallest thing that is still a real filter: a one-dimensional target that
//! coasts, a sensor that reports its position badly, and a filter that works out
//! where it is. No gate, no adaptation, no fading memory — those are in
//! `equipped.rs`.
//!
//! Run with `cargo run --example simple`.
use nalgebra::{Matrix1, Matrix2, Vector1, Vector2};
use nonlinear_filters::prelude::*;
use nonlinear_filters::sim::{simulate, Rng};
/// How the state moves: position and velocity, with the velocity disturbed by a
/// white acceleration of intensity `intensity`.
struct ConstantVelocity {
/// Acceleration noise power, in (m/s^2)^2 per second.
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> {
// The exact integral of a continuous white acceleration over the step. The
// off-diagonal terms matter: an acceleration that moved the velocity also
// moved the position, and a diagonal Q claims otherwise.
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,
)
}
}
/// What the sensor reports: position, plus noise of the given standard deviation.
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)
}
}
fn main() -> Result<(), FilterError> {
let process = ConstantVelocity { intensity: 0.2 };
let sensor = Position { deviation: 1.0 };
let step = 0.5;
// Generate a truth trajectory and the measurements a sensor would have made of
// it. In a real application this is where the data comes in instead.
let truth_start = Vector2::new(0.0, 2.0);
let mut rng = Rng::new(20_261_003);
let run = simulate(&process, &sensor, &truth_start, 0.0, step, 40, &mut rng)?;
// The prior: roughly where we think it is, and how wrong that could be. The
// deviations are generous on purpose — 10 m and 5 m/s against a truth at 0 m
// and 2 m/s — because a filter that is lied to about its own uncertainty is
// worse off than one that admits it does not know.
let prior = Gaussian::from_deviations(Vector2::new(0.0, 0.0), Vector2::new(10.0, 5.0));
let mut filter = Ukf::new(prior);
println!(" t measured truth estimate 1 sigma error NIS");
println!(" ---------------------------------------------------------------------");
for k in 1..run.len() {
// Move the estimate forward to the time of the next measurement, then
// apply it. The two halves are separate calls because measurements do not
// always arrive: with no measurement, predict again and carry on.
filter.predict(&process, run.times[k - 1], step)?;
let report = filter.update(&sensor, run.times[k], &run.measurements[k])?;
if k % 5 == 0 || k == 1 {
let position = filter.estimate.mean[0];
let deviation = filter.estimate.covariance[(0, 0)].sqrt();
println!(
"{:5.1} {:12.3} {:10.3} {:10.3} {:10.3} {:10.3} {:7.2}",
run.times[k],
run.measurements[k][0],
run.states[k][0],
position,
deviation,
position - run.states[k][0],
report.nis,
);
}
}
println!();
println!("final estimate");
println!(
" position {:8.3} m +/- {:.3} m (truth {:.3})",
filter.estimate.mean[0],
filter.estimate.covariance[(0, 0)].sqrt(),
run.states[run.len() - 1][0],
);
println!(
" velocity {:8.3} m/s +/- {:.3} m/s (truth {:.3})",
filter.estimate.mean[1],
filter.estimate.covariance[(1, 1)].sqrt(),
run.states[run.len() - 1][1],
);
// Two things worth noticing in the output above.
//
// The velocity is estimated to a fraction of a meter per second, and no sensor
// ever measured it. That is the whole point of a filter rather than a smoother
// over the measurements: the process model says position and velocity are
// related, so measuring one constrains the other.
//
// The position uncertainty settles to well under the sensor's 1 m, because the
// filter is combining many measurements, and it settles rather than falling to
// zero, because the process noise keeps adding uncertainty as fast as the
// measurements remove it.
Ok(())
}
readme.md
+27 -1
View File
@@ -100,6 +100,17 @@ let estimate = filter.estimate;
# Ok::<(), nonlinear_filters::FilterError>(())
```
## Examples
```sh
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
@@ -191,6 +202,21 @@ constant-velocity state of the given width against a single range measurement:
| 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:
@@ -205,7 +231,7 @@ is the floor for reading anything into these:
- 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 these.
`cargo bench` re-runs all of it.
## License
src/adaptive.rs
+66 -3
View File
@@ -169,9 +169,20 @@ impl<const N: usize> AdaptiveProcessNoise<N> for FixedProcessNoise {
/// cannot find it; [`InnovationNoise`] can, at the cost of assuming nothing and
/// therefore estimating much more from the same data.
///
/// Mixing measurements of different dimensions inside one window averages traces
/// of different sizes and makes the result meaningless. Use one adapter per
/// measurement stream when a filter is fed more than one.
/// # Feeding it more than one sensor
///
/// Measurements of different dimensions can share one window. Each sample carries
/// its own `tr(R)`, `tr(H P_sigma H')` and `tr(H Q0 H')`, and averaging the whole
/// equation term by term leaves the solved scale unbiased whatever mix of
/// dimensions went in.
///
/// What the mix changes is the weighting. A sample whose measurement is more
/// sensitive to the nominal noise — a larger `tr(H Q0 H')` — has proportionally
/// more say. Where two streams imply different scales, the result is exactly the
/// `tr(H Q0 H')`-weighted mean of what each implies, which is the right
/// arrangement: the measurement that sees the process noise most clearly should
/// carry the most weight about its size. It does mean a weakly sensitive stream
/// contributes little even when it is the more numerous one.
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct CovarianceMatching {
window: usize,
@@ -550,6 +561,58 @@ mod tests {
assert_relative_eq!(matcher.scale(), 4.0, max_relative = 1e-12);
}
/// Measurements of different dimensions can share a window.
///
/// This test exists because the documentation originally claimed the opposite
/// — that mixing dimensions "averages traces of different sizes and makes the
/// result meaningless". That was wrong, and wrong in a way that only algebra
/// catches: each sample contributes its own three traces, so averaging the
/// whole moment-matching equation term by term is unbiased regardless of the
/// mix. The two sensors below have trace magnitudes an order of magnitude
/// apart and both imply a scale of 4.
#[test]
fn covariance_matching_pools_across_measurement_dimensions() {
let sample = |dof, squared, noise, propagated, template| InnovationRecord::<2> {
accepted: true,
dof,
nis: 0.0,
innovation_squared: squared,
innovation_covariance_trace: propagated + template + noise,
measurement_noise_trace: noise,
propagated_trace: propagated,
template_trace: template,
state_correction: Vector2::zeros(),
};
// Sensor A, one component: tr(HPH') 2, tr(R) 1, tr(HQ0H') 0.5, so a scale
// of 4 gives tr(S) = 5. Sensor B, two components, with every trace an
// order of magnitude larger, and the same scale of 4 giving tr(S) = 71.
let a = sample(1, 5.0, 1.0, 2.0, 0.5);
let b = sample(2, 71.0, 17.0, 30.0, 6.0);
let mut pooled = CovarianceMatching::new(40, 1e-9, 1e9).unwrap();
for _ in 0..20 {
pooled.observe(&a);
pooled.observe(&b);
}
assert_relative_eq!(pooled.scale(), 4.0, max_relative = 1e-12);
// Where the two streams imply different scales, the pooled answer is the
// template-trace-weighted mean of them, not an average of the two scales.
// Here A implies 4 and B implies 1, with weights 0.5 and 6.0.
let disagreeing = sample(2, 53.0, 17.0, 30.0, 6.0);
let mut mixed = CovarianceMatching::new(40, 1e-9, 1e9).unwrap();
for _ in 0..20 {
mixed.observe(&a);
mixed.observe(&disagreeing);
}
assert_relative_eq!(
mixed.scale(),
(4.0 * 0.5 + 1.0 * 6.0) / 6.5,
max_relative = 1e-12
);
}
#[test]
fn covariance_matching_clamps() {
let mut matcher = CovarianceMatching::inflating(5).unwrap();
src/consider.rs
+2 -2
View File
@@ -196,7 +196,7 @@ impl<const J: usize, const N: usize, S: SigmaPoints<J>> ConsiderUkf<J, N, S> {
///
/// [`FilterError::NotPositiveDefinite`] if the joint covariance admits no
/// Cholesky factor.
pub fn predict<P: ProcessModel<J>>(
pub fn predict<P: ProcessModel<J> + ?Sized>(
&mut self,
model: &P,
time: f64,
@@ -248,7 +248,7 @@ impl<const J: usize, const N: usize, S: SigmaPoints<J>> ConsiderUkf<J, N, S> {
///
/// [`FilterError::NotPositiveDefinite`] if either the joint covariance or the
/// innovation covariance admits no Cholesky factor.
pub fn update<const M: usize, H: MeasurementModel<J, M>>(
pub fn update<const M: usize, H: MeasurementModel<J, M> + ?Sized>(
&mut self,
model: &H,
time: f64,
src/imm.rs
+2 -2
View File
@@ -340,7 +340,7 @@ where
/// [`FilterError::ModelProbabilitiesUnderflowed`] if no mode found the
/// measurement possible at all, which means the bank does not cover what
/// happened.
pub fn update<const M: usize, H: MeasurementModel<N, M>>(
pub fn update<const M: usize, H: MeasurementModel<N, M> + ?Sized>(
&mut self,
model: &H,
time: f64,
@@ -409,7 +409,7 @@ where
/// # Errors
///
/// Whatever either half reports.
pub fn step<const M: usize, H: MeasurementModel<N, M>>(
pub fn step<const M: usize, H: MeasurementModel<N, M> + ?Sized>(
&mut self,
model: &H,
time: f64,
src/ukf.rs
+2 -2
View File
@@ -221,7 +221,7 @@ where
///
/// [`FilterError::NotPositiveDefinite`] if the current covariance admits no
/// Cholesky factor.
pub fn predict<P: ProcessModel<N>>(
pub fn predict<P: ProcessModel<N> + ?Sized>(
&mut self,
model: &P,
time: f64,
@@ -268,7 +268,7 @@ where
/// [`FilterError::NotPositiveDefinite`] if the state covariance admits no
/// Cholesky factor, or [`FilterError::Singular`] if the innovation covariance
/// cannot be inverted.
pub fn update<const M: usize, H: MeasurementModel<N, M>>(
pub fn update<const M: usize, H: MeasurementModel<N, M> + ?Sized>(
&mut self,
model: &H,
time: f64,