#![allow(clippy::many_single_char_names)]
use crate::SquareRootKalman;
use crate::consistency::chi_square_mean_interval;
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum RobustPolicy {
Plain,
ChiSquareGate {
threshold: f64,
},
ChiSquareInflation {
threshold: f64,
},
Huber {
tuning: f64,
},
StudentT {
degrees_of_freedom: f64,
},
}
#[must_use]
pub fn chi_square_gate(dof: usize, z: f64) -> f64 {
chi_square_mean_interval(dof, 1, z).1
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RobustOutcome {
pub accepted: bool,
pub weight: f64,
pub nominal_normalized_squared: f64,
pub innovation_variance: f64,
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct RobustTally {
pub seen: u32,
pub rejected: u32,
pub downweighted: u32,
pub longest_rejection_run: u32,
current_rejection_run: u32,
weight_sum: f64,
}
impl RobustTally {
pub fn record(&mut self, outcome: &RobustOutcome) {
self.seen += 1;
if outcome.accepted {
self.weight_sum += outcome.weight;
if outcome.weight < 1.0 {
self.downweighted += 1;
}
self.current_rejection_run = 0;
} else {
self.rejected += 1;
self.current_rejection_run += 1;
self.longest_rejection_run = self.longest_rejection_run.max(self.current_rejection_run);
}
}
#[must_use]
pub fn mean_weight(&self) -> f64 {
if self.seen == 0 {
return 0.0;
}
self.weight_sum / f64::from(self.seen)
}
#[must_use]
pub fn rejected_fraction(&self) -> f64 {
if self.seen == 0 {
return 0.0;
}
f64::from(self.rejected) / f64::from(self.seen)
}
}
fn huber_weight(prior: f64, noise: f64, innovation: f64, tuning: f64) -> f64 {
let nominal = prior + noise;
let squared = innovation * innovation;
if squared <= tuning * tuning * nominal {
return 1.0;
}
let c2 = tuning * tuning;
let residual = |w: f64| w * w * w * squared - c2 * prior * w - c2 * noise;
let (mut lo, mut hi) = (0.0_f64, 1.0_f64);
for _ in 0..60 {
let mid = f64::midpoint(lo, hi);
if residual(mid) < 0.0 {
lo = mid;
} else {
hi = mid;
}
}
f64::midpoint(lo, hi)
}
fn student_t_variance(prior: f64, noise: f64, innovation: f64, dof: f64) -> f64 {
let a = dof + 1.0;
let b = a * prior + noise * dof;
let discriminant = b * b + 4.0 * a * noise * innovation * innovation;
let f = (b + libm::sqrt(discriminant)) / (2.0 * a);
f - prior
}
impl RobustPolicy {
#[must_use]
pub fn effective_variance(
&self,
prior_variance: f64,
noise_variance: f64,
innovation: f64,
) -> Option<f64> {
if !prior_variance.is_finite()
|| !noise_variance.is_finite()
|| !innovation.is_finite()
|| prior_variance < 0.0
|| noise_variance <= 0.0
{
return None;
}
let nominal = prior_variance + noise_variance;
match *self {
Self::Plain => Some(noise_variance),
Self::ChiSquareGate { threshold } => {
(innovation * innovation <= threshold * nominal).then_some(noise_variance)
}
Self::ChiSquareInflation { threshold } => (threshold > 0.0).then(|| {
let normalized = innovation * innovation / nominal;
noise_variance
* if normalized > threshold {
normalized / threshold
} else {
1.0
}
}),
Self::Huber { tuning } => (tuning > 0.0).then(|| {
noise_variance / huber_weight(prior_variance, noise_variance, innovation, tuning)
}),
Self::StudentT { degrees_of_freedom } => (degrees_of_freedom > 0.0).then(|| {
student_t_variance(
prior_variance,
noise_variance,
innovation,
degrees_of_freedom,
)
}),
}
}
#[must_use]
pub fn variance_scale(&self, normalized_squared: f64, dof: usize) -> Option<f64> {
if !normalized_squared.is_finite() || normalized_squared < 0.0 || dof == 0 {
return None;
}
#[allow(clippy::cast_precision_loss)]
let d = dof as f64;
match *self {
Self::Plain => Some(1.0),
Self::ChiSquareGate { threshold } => (normalized_squared <= threshold).then_some(1.0),
Self::ChiSquareInflation { threshold } => {
(threshold > 0.0).then(|| (normalized_squared / threshold).max(1.0))
}
Self::Huber { tuning } => {
(tuning > 0.0).then(|| (libm::sqrt(normalized_squared / d) / tuning).max(1.0))
}
Self::StudentT { degrees_of_freedom } => (degrees_of_freedom > 0.0)
.then(|| (degrees_of_freedom + normalized_squared) / (degrees_of_freedom + d)),
}
}
}
pub(crate) const fn rejected(normalized_squared: f64) -> RobustOutcome {
RobustOutcome {
accepted: false,
weight: 0.0,
nominal_normalized_squared: normalized_squared,
innovation_variance: 0.0,
}
}
pub fn robust_update<const N: usize>(
filter: &mut SquareRootKalman<N>,
policy: RobustPolicy,
h: &[f64; N],
innovation: f64,
noise_variance: f64,
) -> RobustOutcome {
let s = filter.covariance_sqrt();
let mut projected = [0.0; N];
for (row, &weight) in s.iter().zip(h.iter()) {
for (component, &value) in projected.iter_mut().zip(row.iter()) {
*component += weight * value;
}
}
let prior: f64 = projected.iter().map(|v| v * v).sum();
let nominal = prior + noise_variance;
let normalized = if nominal > 0.0 {
innovation * innovation / nominal
} else {
f64::INFINITY
};
match policy.effective_variance(prior, noise_variance, innovation) {
Some(effective) => {
let used = filter.measurement_update(h, innovation, effective);
RobustOutcome {
accepted: true,
weight: noise_variance / effective,
nominal_normalized_squared: normalized,
innovation_variance: used,
}
}
None => RobustOutcome {
accepted: false,
weight: 0.0,
nominal_normalized_squared: normalized,
innovation_variance: 0.0,
},
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::expect_used, clippy::float_cmp)]
use super::*;
#[test]
fn the_gate_threshold_is_the_tabulated_chi_square_quantile() {
for (z, exact) in [(1.96, 5.023_886), (2.576, 7.879_400), (3.0, 10.276_618)] {
let gate = chi_square_gate(1, z);
assert!(
(gate / exact - 1.0).abs() < 0.03,
"χ²₁ at z = {z}: {gate} against {exact}"
);
}
assert!((chi_square_gate(3, 1.96) / 9.348_404 - 1.0).abs() < 0.005);
assert!(chi_square_gate(1, 1.96) < chi_square_gate(1, 3.0));
assert!(chi_square_gate(1, 1.96) < chi_square_gate(3, 1.96));
}
#[test]
fn a_clean_observation_is_admitted_at_full_weight() {
let (prior, noise) = (4.0, 1.0);
let innovation = 0.5 * libm::sqrt(prior + noise);
for policy in [
RobustPolicy::Plain,
RobustPolicy::ChiSquareGate { threshold: 9.0 },
RobustPolicy::ChiSquareInflation { threshold: 9.0 },
RobustPolicy::Huber { tuning: 1.345 },
] {
let effective = policy
.effective_variance(prior, noise, innovation)
.expect("a clean observation is not gated");
assert!((effective - noise).abs() < 1e-12, "{policy:?} moved R");
}
let t = RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
};
let at_zero = t.effective_variance(prior, noise, 0.0).unwrap();
assert!((at_zero - noise * 5.0 / 6.0).abs() < 1e-12, "{at_zero}");
}
#[test]
fn the_huber_weight_satisfies_its_own_defining_equation() {
let (prior, noise, tuning) = (4.0, 1.0, 1.345);
for innovation in [4.0, 5.0, 10.0, 100.0, 1e4] {
let effective = RobustPolicy::Huber { tuning }
.effective_variance(prior, noise, innovation)
.unwrap();
let w = noise / effective;
assert!(w < 1.0, "ν = {innovation} should be down-weighted");
let delta = innovation / libm::sqrt(prior + effective);
assert!(
(w - tuning / delta.abs()).abs() < 1e-12,
"ν = {innovation}: w = {w}, c/|δ| = {}",
tuning / delta.abs()
);
}
}
#[test]
fn the_huber_weight_leaves_one_exactly_at_its_tuning_constant() {
for (prior, noise, tuning) in [
(4.0, 1.0, 1.345),
(0.25, 4.0, 1.0),
(10.0, 0.1, 2.5),
(0.1, 0.2, 0.5),
(0.05, 0.4, 0.8),
] {
let boundary = tuning * libm::sqrt(prior + noise);
for scale in [0.5, 0.999] {
let w = huber_weight(prior, noise, scale * boundary, tuning);
assert_eq!(
w, 1.0,
"p = {prior}, R = {noise}, c = {tuning} at {scale}× the boundary"
);
}
for scale in [1.001, 1.5, 20.0] {
let innovation = scale * boundary;
let w = huber_weight(prior, noise, innovation, tuning);
assert!(
w > 0.0 && w < 1.0,
"p = {prior}, R = {noise}, c = {tuning} at {scale}× the boundary: w = {w}"
);
let c2 = tuning * tuning;
let residual = w * w * w * innovation * innovation - c2 * prior * w - c2 * noise;
assert!(
residual.abs() < 1e-12 * c2 * (prior + noise),
"w³ν² − c²pw − c²R = {residual} at w = {w}"
);
}
}
}
#[test]
fn with_no_prior_uncertainty_the_huber_weight_is_a_cube_root() {
for (noise, tuning, innovation, pinned) in [
(0.25, 1.0, 5.0, 0.215_443_469_003_188_34),
(1.0, 2.0, 10.0, 0.341_995_189_335_339_34),
(4.0, 1.0, 20.0, 0.215_443_469_003_188_34),
] {
let w = huber_weight(0.0, noise, innovation, tuning);
let closed_form = libm::cbrt(tuning * tuning * noise / (innovation * innovation));
assert!(
(w / closed_form - 1.0).abs() < 1e-15,
"R = {noise}, c = {tuning}, ν = {innovation}: {w} against ∛(c²R/ν²) = {closed_form}"
);
assert_eq!(w, pinned, "R = {noise}, c = {tuning}, ν = {innovation}");
}
for policy in [
RobustPolicy::Plain,
RobustPolicy::ChiSquareGate { threshold: 9.0 },
RobustPolicy::ChiSquareInflation { threshold: 9.0 },
RobustPolicy::Huber { tuning: 1.345 },
RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
},
] {
let effective = policy
.effective_variance(0.0, 2.0, 1.0)
.expect("a certain prior is a legal prior");
assert!(effective > 0.0 && effective.is_finite(), "{policy:?}");
}
}
#[test]
fn the_student_t_variance_satisfies_its_own_defining_equation() {
let (prior, noise, dof) = (4.0, 1.0, 5.0);
for innovation in [0.0, 1.0, 5.0, 50.0, 1e4] {
let effective = RobustPolicy::StudentT {
degrees_of_freedom: dof,
}
.effective_variance(prior, noise, innovation)
.unwrap();
let lambda = noise / effective;
let f = prior + effective;
let expected = (dof + 1.0) / (dof + innovation * innovation / f);
assert!(
(lambda / expected - 1.0).abs() < 1e-12,
"ν = {innovation}: λ = {lambda}, (n+1)/(n+δ²) = {expected}"
);
}
}
#[test]
fn only_the_student_t_has_a_bounded_influence_function() {
let (prior, noise) = (4.0, 1.0);
let increment = |policy: RobustPolicy, innovation: f64| {
policy
.effective_variance(prior, noise, innovation)
.map_or(0.0, |effective| prior * innovation / (prior + effective))
};
let gate = RobustPolicy::ChiSquareGate { threshold: 9.0 };
assert_eq!(increment(gate, 1e3), 0.0);
let t = RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
};
let saturated = increment(t, 1e5);
assert!(
(increment(t, 1e7) / saturated - 1.0).abs() < 1e-3,
"Student-t saturates: {} then {}",
saturated,
increment(t, 1e7)
);
let bound = prior * libm::sqrt(6.0 / noise);
assert!(
(saturated / bound - 1.0).abs() < 1e-4,
"{saturated} against p√((n+1)/R) = {bound}"
);
let inflate = RobustPolicy::ChiSquareInflation { threshold: 9.0 };
let at_threshold = increment(inflate, 3.0 * libm::sqrt(prior + noise));
assert!(
increment(inflate, 1e3) < at_threshold / 20.0,
"{at_threshold} then {}",
increment(inflate, 1e3)
);
assert!(
increment(inflate, 1e6) < increment(inflate, 1e3),
"not descending"
);
for nu in [1e3, 1e6] {
let asymptote = prior * 9.0 * (prior + noise) / (noise * nu);
assert!(
(increment(inflate, nu) / asymptote - 1.0).abs() < 1e-3,
"ν = {nu}: {} against {asymptote}",
increment(inflate, nu)
);
}
let epsilon = 3.0 * libm::sqrt(prior + noise) * (1.0 + 1e-9);
assert!((increment(inflate, epsilon) / at_threshold - 1.0).abs() < 1e-6);
let huber = RobustPolicy::Huber { tuning: 1.345 };
let (near, far) = (increment(huber, 1e2), increment(huber, 1e5));
assert!(far > 10.0 * near, "Huber's influence grows: {near} → {far}");
assert!(
far > 2.0 * bound,
"and past the Student-t's ceiling: {far} > {bound}"
);
let (deep, deeper) = (increment(huber, 1e6), increment(huber, 1e9));
assert!(
(deeper / deep / 10.0 - 1.0).abs() < 0.01,
"|ν|^(1/3): {deep} → {deeper}"
);
}
#[test]
fn a_gated_observation_leaves_the_filter_exactly_as_it_was() {
let mut filter = SquareRootKalman::new([0.0, 0.0], [[2.0, 0.0], [0.0, 1.0]]);
let before = (filter.x, filter.covariance_sqrt());
let outcome = robust_update(
&mut filter,
RobustPolicy::ChiSquareGate { threshold: 9.0 },
&[1.0, 0.0],
100.0,
1.0,
);
assert!(!outcome.accepted);
assert_eq!(outcome.weight, 0.0);
assert_eq!(outcome.innovation_variance, 0.0);
assert_eq!(before, (filter.x, filter.covariance_sqrt()));
assert!(outcome.nominal_normalized_squared > 9.0);
}
#[test]
fn the_nominal_residual_is_reported_on_both_branches() {
let seed = SquareRootKalman::new([0.0, 0.0], [[2.0, 0.0], [0.5, 1.0]]);
let h = [1.0, -0.5];
let (innovation, noise) = (1.5, 1.0);
let p = seed.covariance();
let prior: f64 = (0..2)
.map(|i| h[i] * (0..2).map(|j| p[i][j] * h[j]).sum::<f64>())
.sum();
assert_eq!(prior, 3.3125, "the geometry this test is pinned to moved");
let expected = innovation * innovation / (prior + noise);
let mut applied = seed.clone();
let accepted = robust_update(&mut applied, RobustPolicy::Plain, &h, innovation, noise);
assert!(accepted.accepted);
assert_eq!(accepted.nominal_normalized_squared, expected);
let mut refused = seed;
let outcome = robust_update(
&mut refused,
RobustPolicy::ChiSquareGate { threshold: 1e-6 },
&h,
innovation,
noise,
);
assert!(!outcome.accepted);
assert_eq!(outcome.nominal_normalized_squared, expected);
}
#[test]
fn a_measurement_with_no_variance_at_all_reports_an_infinite_residual() {
let mut filter = SquareRootKalman::new([0.0, 0.0], [[2.0, 0.0], [0.5, 1.0]]);
let before = (filter.x, filter.covariance_sqrt());
let outcome = robust_update(&mut filter, RobustPolicy::Plain, &[0.0, 0.0], 0.0, 0.0);
assert!(!outcome.accepted);
assert!(
outcome.nominal_normalized_squared.is_infinite(),
"{}",
outcome.nominal_normalized_squared
);
assert_eq!(before, (filter.x, filter.covariance_sqrt()));
}
#[test]
fn the_plain_policy_is_the_plain_update() {
let seed = SquareRootKalman::new([0.5, -0.25], [[2.0, 0.0], [0.3, 1.0]]);
let (mut robust, mut plain) = (seed.clone(), seed);
let outcome = robust_update(&mut robust, RobustPolicy::Plain, &[1.0, 0.5], 0.7, 1.5);
let variance = plain.measurement_update(&[1.0, 0.5], 0.7, 1.5);
assert_eq!(robust.x, plain.x);
assert_eq!(robust.covariance_sqrt(), plain.covariance_sqrt());
assert_eq!(outcome.innovation_variance, variance);
assert_eq!(outcome.weight, 1.0);
}
#[test]
fn the_tally_counts_what_the_filter_did() {
let mut tally = RobustTally::default();
let outcome = |accepted, weight| RobustOutcome {
accepted,
weight,
nominal_normalized_squared: 0.0,
innovation_variance: 1.0,
};
tally.record(&outcome(true, 1.0));
tally.record(&outcome(true, 0.5));
tally.record(&outcome(false, 0.0));
tally.record(&outcome(true, 1.0));
assert_eq!(tally.seen, 4);
assert_eq!(tally.rejected, 1);
assert_eq!(tally.downweighted, 1);
assert_eq!(tally.mean_weight(), 2.5 / 4.0);
assert_eq!(tally.longest_rejection_run, 1);
assert_eq!(tally.rejected_fraction(), 0.25);
let mut runs = RobustTally::default();
for accepted in [false, false, true, false, false, false, true, false] {
runs.record(&outcome(accepted, 1.0));
}
assert_eq!(runs.longest_rejection_run, 3);
assert_eq!(runs.rejected, 6);
assert_eq!(RobustTally::default().longest_rejection_run, 0);
assert_eq!(RobustTally::default().mean_weight(), 0.0);
assert_eq!(RobustTally::default().rejected_fraction(), 0.0);
}
#[test]
fn an_unposeable_update_is_refused_rather_than_guessed_at() {
for policy in [
RobustPolicy::Plain,
RobustPolicy::ChiSquareGate { threshold: 9.0 },
RobustPolicy::ChiSquareInflation { threshold: 9.0 },
RobustPolicy::Huber { tuning: 1.345 },
RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
},
] {
assert_eq!(policy.effective_variance(1.0, 0.0, 1.0), None);
assert_eq!(policy.effective_variance(1.0, -1.0, 1.0), None);
assert_eq!(policy.effective_variance(-1.0, 1.0, 1.0), None);
assert_eq!(policy.effective_variance(1.0, 1.0, f64::NAN), None);
assert_eq!(policy.effective_variance(f64::INFINITY, 1.0, 1.0), None);
}
assert_eq!(
RobustPolicy::Huber { tuning: 0.0 }.effective_variance(1.0, 1.0, 1.0),
None
);
assert_eq!(
RobustPolicy::ChiSquareInflation { threshold: 0.0 }.effective_variance(1.0, 1.0, 1.0),
None
);
assert_eq!(
RobustPolicy::StudentT {
degrees_of_freedom: 0.0
}
.effective_variance(1.0, 1.0, 1.0),
None
);
}
#[test]
fn one_degree_of_freedom_reproduces_the_scalar_threshold_policies() {
for delta_squared in [0.0, 0.25, 1.0, 4.0, 9.0, 100.0] {
let nu = libm::sqrt(delta_squared);
for policy in [
RobustPolicy::Plain,
RobustPolicy::ChiSquareGate { threshold: 9.0 },
RobustPolicy::ChiSquareInflation { threshold: 9.0 },
] {
let scalar = policy.effective_variance(0.5, 0.5, nu);
let vector = policy.variance_scale(delta_squared, 1).map(|s| 0.5 * s);
match (scalar, vector) {
(None, None) => {}
(Some(a), Some(b)) => assert!(
(a - b).abs() < 1e-12 * a.max(1.0),
"{policy:?} at δ² = {delta_squared}: scalar {a}, vector {b}"
),
_ => {
panic!("{policy:?} at δ² = {delta_squared}: {scalar:?} against {vector:?}")
}
}
}
}
}
#[test]
fn the_one_step_forms_discount_harder_than_the_fixed_points_they_approximate() {
for policy in [
RobustPolicy::Huber { tuning: 1.345 },
RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
},
] {
for delta_squared in [4.0, 9.0, 25.0, 100.0, 1e4] {
let nu = libm::sqrt(delta_squared);
let fixed = policy.effective_variance(0.5, 0.5, nu).unwrap();
let one_step = 0.5 * policy.variance_scale(delta_squared, 1).unwrap();
assert!(
one_step > fixed,
"{policy:?} at δ² = {delta_squared}: one-step R_eff {one_step} against fixed \
point {fixed}"
);
}
}
let boundary = 1.345 * 1.345;
assert_eq!(
RobustPolicy::Huber { tuning: 1.345 }.variance_scale(boundary, 1),
Some(1.0)
);
assert_eq!(
RobustPolicy::Huber { tuning: 1.345 }.effective_variance(0.5, 0.5, 1.345),
Some(0.5)
);
}
#[test]
fn a_typical_observation_is_undiscounted_at_every_dimension() {
for dof in 1_u32..=6 {
let expectation = f64::from(dof);
let dof = dof as usize;
assert_eq!(
RobustPolicy::Huber { tuning: 1.345 }.variance_scale(expectation, dof),
Some(1.0),
"Huber discounts a typical {dof}-vector"
);
let student = RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
}
.variance_scale(expectation, dof)
.unwrap();
assert_eq!(student, 1.0, "Student-t at δ² = {dof} gave {student}");
}
}
#[test]
fn the_gate_thresholds_the_whole_normalised_innovation_and_not_one_axis_of_it() {
let threshold = chi_square_gate(3, 2.576);
let policy = RobustPolicy::ChiSquareGate { threshold };
assert_eq!(policy.variance_scale(threshold, 3), Some(1.0));
assert_eq!(policy.variance_scale(threshold * (1.0 + 1e-12), 3), None);
assert_eq!(policy.variance_scale(3.0 * threshold, 3), None);
assert_eq!(policy.variance_scale(threshold, 1), Some(1.0));
assert!(chi_square_gate(1, 2.576) < threshold);
}
#[test]
fn a_scale_cannot_be_produced_for_an_input_that_is_not_one() {
for policy in [
RobustPolicy::Plain,
RobustPolicy::ChiSquareInflation { threshold: 9.0 },
RobustPolicy::Huber { tuning: 1.345 },
RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
},
] {
assert_eq!(policy.variance_scale(f64::NAN, 3), None);
assert_eq!(policy.variance_scale(f64::INFINITY, 3), None);
assert_eq!(policy.variance_scale(-1.0, 3), None);
assert_eq!(policy.variance_scale(1.0, 0), None);
}
assert_eq!(
RobustPolicy::Huber { tuning: 0.0 }.variance_scale(4.0, 3),
None
);
assert_eq!(
RobustPolicy::ChiSquareInflation { threshold: 0.0 }.variance_scale(4.0, 3),
None
);
assert_eq!(
RobustPolicy::StudentT {
degrees_of_freedom: 0.0
}
.variance_scale(4.0, 3),
None
);
}
#[test]
fn inflation_and_the_student_t_are_asymptotically_quadratic_and_huber_is_not() {
let (a, b) = (1e6, 4e6);
for policy in [
RobustPolicy::ChiSquareInflation { threshold: 9.0 },
RobustPolicy::StudentT {
degrees_of_freedom: 5.0,
},
] {
let ratio = policy.variance_scale(b, 3).unwrap() / policy.variance_scale(a, 3).unwrap();
assert!((ratio - 4.0).abs() < 1e-3, "{policy:?} scaled by {ratio}");
}
let huber = RobustPolicy::Huber { tuning: 1.345 };
let ratio = huber.variance_scale(b, 3).unwrap() / huber.variance_scale(a, 3).unwrap();
assert!(
(ratio - 2.0).abs() < 1e-9,
"Huber scaled by {ratio}, not by √4"
);
}
}