#![allow(clippy::many_single_char_names, clippy::needless_range_loop)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum HInfinityError {
ExistenceConditionViolated {
direction: usize,
},
SingularCovariance {
direction: usize,
},
MeasurementNoiseNotPositiveDefinite {
direction: usize,
},
UnattainableBound {
direction: usize,
},
}
impl core::fmt::Display for HInfinityError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::ExistenceConditionViolated { direction } => write!(
f,
"H-infinity existence condition fails in direction {direction}: reduce gamma"
),
Self::SingularCovariance { direction } => {
write!(f, "covariance is singular in direction {direction}")
}
Self::MeasurementNoiseNotPositiveDefinite { direction } => write!(
f,
"measurement noise covariance is not positive-definite in direction {direction}"
),
Self::UnattainableBound { direction } => write!(
f,
"gamma is unattainable for this prior covariance in direction {direction}"
),
}
}
}
impl core::error::Error for HInfinityError {}
#[derive(Debug, Clone)]
pub struct HInfinityFilter<const N: usize> {
pub x: [f64; N],
s: [[f64; N]; N],
performance_weight: [[f64; N]; N],
gamma: f64,
}
impl<const N: usize> HInfinityFilter<N> {
pub fn new(
x0: [f64; N],
covariance_sqrt: [[f64; N]; N],
performance_weight: [[f64; N]; N],
gamma: f64,
) -> Result<Self, HInfinityError> {
if !(gamma.is_finite() && gamma >= 0.0) {
return Err(HInfinityError::UnattainableBound { direction: 0 });
}
let inverse = inverse_from_factor(&covariance_sqrt)
.map_err(|direction| HInfinityError::SingularCovariance { direction })?;
let mut worst = inverse;
for i in 0..N {
for j in 0..N {
worst[i][j] -= gamma * performance_weight[i][j];
}
}
cholesky(&symmetrised(&worst))
.map_err(|direction| HInfinityError::UnattainableBound { direction })?;
Ok(Self {
x: x0,
s: covariance_sqrt,
performance_weight,
gamma,
})
}
pub fn from_covariance(
x0: [f64; N],
p0: &[[f64; N]; N],
performance_weight: [[f64; N]; N],
gamma: f64,
) -> Result<Self, HInfinityError> {
let s =
cholesky(p0).map_err(|direction| HInfinityError::SingularCovariance { direction })?;
Self::new(x0, s, performance_weight, gamma)
}
pub fn step<const D: usize>(
&mut self,
transition: &[[f64; N]; N],
measurement: &[[f64; N]; D],
z: &[f64; D],
noise_covariance: &[[f64; D]; D],
process_noise_sqrt: &[[f64; N]; N],
) -> Result<[f64; D], HInfinityError> {
let noise_factor = cholesky(noise_covariance).map_err(|direction| {
HInfinityError::MeasurementNoiseNotPositiveDefinite { direction }
})?;
let noise_inverse = inverse_from_factor(&noise_factor).map_err(|direction| {
HInfinityError::MeasurementNoiseNotPositiveDefinite { direction }
})?;
let mut weighted = [[0.0; N]; D];
for i in 0..D {
for j in 0..N {
let mut acc = 0.0;
for k in 0..D {
acc += noise_inverse[i][k] * measurement[k][j];
}
weighted[i][j] = acc;
}
}
let mut m = inverse_from_factor(&self.s)
.map_err(|direction| HInfinityError::SingularCovariance { direction })?;
for i in 0..N {
for j in 0..N {
let mut information = 0.0;
for k in 0..D {
information += measurement[k][i] * weighted[k][j];
}
m[i][j] += information - self.gamma * self.performance_weight[i][j];
}
}
let m_factor = cholesky(&symmetrised(&m))
.map_err(|direction| HInfinityError::ExistenceConditionViolated { direction })?;
let m_inverse_factor = invert_lower(&m_factor)
.map_err(|direction| HInfinityError::ExistenceConditionViolated { direction })?;
let mut m_inverse = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += m_inverse_factor[k][i] * m_inverse_factor[k][j];
}
m_inverse[i][j] = acc;
}
}
let mut transformed = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += transition[i][k] * m_inverse[k][j];
}
transformed[i][j] = acc;
}
}
let mut gain = [[0.0; D]; N];
for i in 0..N {
for j in 0..D {
let mut acc = 0.0;
for k in 0..N {
acc += transformed[i][k] * weighted[j][k];
}
gain[i][j] = acc;
}
}
let mut innovation = *z;
for i in 0..D {
for k in 0..N {
innovation[i] -= measurement[i][k] * self.x[k];
}
}
let mut next = [0.0; N];
for i in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += transition[i][k] * self.x[k];
}
for j in 0..D {
acc += gain[i][j] * innovation[j];
}
next[i] = acc;
}
self.x = next;
let mut left = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += transition[i][k] * m_inverse_factor[j][k];
}
left[i][j] = acc;
}
}
self.s = triangularise(left, *process_noise_sqrt);
Ok(innovation)
}
#[must_use]
pub fn covariance(&self) -> [[f64; N]; N] {
let mut p = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += self.s[i][k] * self.s[j][k];
}
p[i][j] = acc;
}
}
p
}
#[must_use]
pub const fn covariance_sqrt(&self) -> [[f64; N]; N] {
self.s
}
#[must_use]
pub const fn gamma(&self) -> f64 {
self.gamma
}
}
fn triangularise<const N: usize>(
mut left: [[f64; N]; N],
mut right: [[f64; N]; N],
) -> [[f64; N]; N] {
for i in 0..N {
for j in (i + 1)..(2 * N) {
let p = left[i][i];
let q = if j < N { left[i][j] } else { right[i][j - N] };
let denom = libm::hypot(p, q);
if denom == 0.0 {
continue;
}
let (c, s) = (p / denom, q / denom);
for r in 0..N {
let a_i = left[r][i];
let a_j = if j < N { left[r][j] } else { right[r][j - N] };
left[r][i] = c * a_i + s * a_j;
let updated = -s * a_i + c * a_j;
if j < N {
left[r][j] = updated;
} else {
right[r][j - N] = updated;
}
}
}
}
left
}
fn cholesky<const N: usize>(a: &[[f64; N]; N]) -> Result<[[f64; N]; N], usize> {
let mut l = [[0.0; N]; N];
for i in 0..N {
for j in 0..=i {
let mut sum = a[i][j];
for k in 0..j {
sum -= l[i][k] * l[j][k];
}
if i == j {
if sum.is_nan() || sum <= 0.0 {
return Err(i);
}
l[i][j] = libm::sqrt(sum);
} else {
l[i][j] = sum / l[j][j];
}
}
}
Ok(l)
}
fn invert_lower<const N: usize>(l: &[[f64; N]; N]) -> Result<[[f64; N]; N], usize> {
let mut inverse = [[0.0; N]; N];
for i in 0..N {
if l[i][i] == 0.0 {
return Err(i);
}
inverse[i][i] = 1.0 / l[i][i];
for j in 0..i {
let mut acc = 0.0;
for k in j..i {
acc += l[i][k] * inverse[k][j];
}
inverse[i][j] = -acc / l[i][i];
}
}
Ok(inverse)
}
fn inverse_from_factor<const N: usize>(l: &[[f64; N]; N]) -> Result<[[f64; N]; N], usize> {
let factor = invert_lower(l)?;
let mut inverse = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += factor[k][i] * factor[k][j];
}
inverse[i][j] = acc;
}
}
Ok(inverse)
}
fn symmetrised<const N: usize>(a: &[[f64; N]; N]) -> [[f64; N]; N] {
let mut out = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
out[i][j] = f64::midpoint(a[i][j], a[j][i]);
}
}
out
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp)]
use super::*;
use crate::kalman::SquareRootKalman;
const F: [[f64; 2]; 2] = [[1.0, 1.0], [0.0, 1.0]];
const H: [[f64; 2]; 1] = [[1.0, 0.0]];
const SQ: [[f64; 2]; 2] = [[0.02, 0.0], [0.0, 0.05]];
const P0: [[f64; 2]; 2] = [[1.0, 0.0], [0.0, 0.5]];
const R: f64 = 0.09;
fn identity<const N: usize>() -> [[f64; N]; N] {
let mut m = [[0.0; N]; N];
for i in 0..N {
m[i][i] = 1.0;
}
m
}
fn observations(count: usize, manoeuvre_at: Option<usize>) -> ([Vec<f64>; 2], Vec<f64>) {
let (mut truth, mut rate) = (0.0_f64, 1.0_f64);
let mut seed = 12_345_u64;
let (mut states, mut rates, mut z) = (Vec::new(), Vec::new(), Vec::new());
for k in 0..count {
if manoeuvre_at == Some(k) {
rate += 3.0;
}
truth += rate;
seed = seed.wrapping_mul(6_364_136_223_846_793_005).wrapping_add(1);
#[allow(clippy::cast_precision_loss)]
let jitter = ((seed >> 33) as f64 / f64::from(1_u32 << 31) - 0.5) * 0.6;
states.push(truth);
rates.push(rate);
z.push(truth + jitter);
}
([states, rates], z)
}
#[test]
fn the_zero_gamma_filter_is_the_kalman_filter() {
let (_, z) = observations(12, None);
let mut hinf =
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), 0.0).unwrap();
let mut kalman = SquareRootKalman::<2>::from_covariance([0.0, 0.0], &P0).unwrap();
let (mut worst_x, mut worst_p) = (0.0_f64, 0.0_f64);
for measurement in &z {
hinf.step::<1>(&F, &H, &[*measurement], &[[R]], &SQ)
.unwrap();
let predicted = kalman.x[0];
kalman.measurement_update(&H[0], measurement - predicted, R);
let updated = kalman.x;
for i in 0..2 {
kalman.x[i] = F[i][0] * updated[0] + F[i][1] * updated[1];
}
kalman.time_update(&F, &SQ);
let (ph, pk) = (hinf.covariance(), kalman.covariance());
for i in 0..2 {
worst_x = worst_x.max((hinf.x[i] - kalman.x[i]).abs());
for j in 0..2 {
worst_p = worst_p.max((ph[i][j] - pk[i][j]).abs());
}
}
}
println!("H-infinity(gamma=0) vs SR-KF: max |dx| {worst_x:.3e}, max |dP| {worst_p:.3e}");
assert!(
worst_x < 1e-12,
"state departed from the Kalman filter: {worst_x:.3e}"
);
assert!(worst_p < 1e-12, "covariance departed: {worst_p:.3e}");
}
#[test]
fn the_recursion_holds_in_three_dimensions_with_no_symmetry_to_hide_behind() {
const F3: [[f64; 3]; 3] = [[1.0, 1.0, 0.5], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]];
const H3: [[f64; 3]; 1] = [[1.0, 0.5, 0.0]];
const SQ3: [[f64; 3]; 3] = [[0.02, 0.0, 0.0], [0.01, 0.05, 0.0], [0.0, 0.01, 0.03]];
const P3: [[f64; 3]; 3] = [[1.0, 0.3, -0.2], [0.3, 0.5, 0.15], [-0.2, 0.15, 0.25]];
let (_, z) = observations(12, None);
let mut hinf =
HInfinityFilter::<3>::from_covariance([0.1, -0.2, 0.05], &P3, identity(), 0.0).unwrap();
let mut kalman = SquareRootKalman::<3>::from_covariance([0.1, -0.2, 0.05], &P3).unwrap();
let (mut worst_x, mut worst_p) = (0.0_f64, 0.0_f64);
for measurement in &z {
hinf.step::<1>(&F3, &H3, &[*measurement], &[[R]], &SQ3)
.unwrap();
let predicted: f64 = (0..3).map(|k| H3[0][k] * kalman.x[k]).sum();
kalman.measurement_update(&H3[0], measurement - predicted, R);
let updated = kalman.x;
for i in 0..3 {
kalman.x[i] = (0..3).map(|k| F3[i][k] * updated[k]).sum();
}
kalman.time_update(&F3, &SQ3);
let (ph, pk) = (hinf.covariance(), kalman.covariance());
for i in 0..3 {
worst_x = worst_x.max((hinf.x[i] - kalman.x[i]).abs());
for j in 0..3 {
worst_p = worst_p.max((ph[i][j] - pk[i][j]).abs());
}
}
}
println!(
"3-D H-infinity(gamma=0) vs SR-KF: max |dx| {worst_x:.3e}, max |dP| {worst_p:.3e}"
);
assert!(worst_x < 1e-12, "state departed: {worst_x:.3e}");
assert!(worst_p < 1e-12, "covariance departed: {worst_p:.3e}");
}
#[test]
fn the_minimax_filter_is_bounded_where_the_kalman_filter_is_optimistic() {
let manoeuvre = monte_carlo(0.9, Some(20));
let quiet = monte_carlo(0.9, None);
println!("manoeuvre: {manoeuvre:?}\nmatched: {quiet:?}");
assert!(
manoeuvre.rms_hinf < 0.96 * manoeuvre.rms_kalman,
"the minimax filter must bound the unmodelled step: RMS {:.4} vs {:.4}",
manoeuvre.rms_hinf,
manoeuvre.rms_kalman
);
assert!(
manoeuvre.peak_hinf < manoeuvre.peak_kalman,
"peak {:.3} vs {:.3}",
manoeuvre.peak_hinf,
manoeuvre.peak_kalman
);
assert!(
manoeuvre.rms_hinf < monte_carlo(0.3, Some(20)).rms_hinf,
"the protection must grow with gamma"
);
assert!(
quiet.rms_kalman < quiet.rms_hinf,
"on data that matches the model the Kalman filter must not be beaten: {:.4} vs {:.4}",
quiet.rms_kalman,
quiet.rms_hinf
);
}
#[test]
fn an_unattainable_bound_is_refused_rather_than_answered() {
let refused = HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), 3.0);
assert_eq!(
refused.err(),
Some(HInfinityError::UnattainableBound { direction: 0 })
);
assert!(HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), -1.0).is_err());
assert!(
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), f64::NAN).is_err()
);
let loud = [[0.1, 0.0], [0.0, 2.0]];
let mut filter =
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), 0.9).unwrap();
let mut failure = None;
for _ in 0..50 {
if let Err(error) = filter.step::<1>(&F, &H, &[0.0], &[[R]], &loud) {
failure = Some(error);
break;
}
}
assert_eq!(
failure,
Some(HInfinityError::ExistenceConditionViolated { direction: 1 }),
"the recursion must report infeasibility rather than returning a gain"
);
}
#[test]
fn the_degenerate_inputs_are_named_rather_than_inverted() {
let singular = [[1.0, 0.0], [0.0, 0.0]];
assert_eq!(
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &singular, identity(), 0.0).err(),
Some(HInfinityError::SingularCovariance { direction: 1 })
);
let mut filter =
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), 0.0).unwrap();
assert_eq!(
filter.step::<1>(&F, &H, &[0.0], &[[0.0]], &SQ).err(),
Some(HInfinityError::MeasurementNoiseNotPositiveDefinite { direction: 0 })
);
}
#[test]
fn the_reported_factor_multiplies_out_to_the_reported_covariance() {
let mut filter =
HInfinityFilter::<2>::from_covariance([0.4, -0.2], &P0, identity(), 0.05).unwrap();
let innovation = filter.step::<1>(&F, &H, &[1.25], &[[R]], &SQ).unwrap();
assert!((innovation[0] - (1.25 - 0.4)).abs() < 1e-15, "z − H x̂");
let (s, p) = (filter.covariance_sqrt(), filter.covariance());
for i in 0..2 {
for j in 0..2 {
if j > i {
assert_eq!(s[i][j], 0.0, "S[{i}][{j}] is above the diagonal");
}
let sst: f64 = (0..2).map(|k| s[i][k] * s[j][k]).sum();
assert!((sst - p[i][j]).abs() < 1e-14, "S Sᵀ[{i}][{j}]");
}
}
assert_eq!(filter.gamma(), 0.05);
}
#[test]
fn every_refusal_names_its_own_direction() {
let messages = [
(
HInfinityError::ExistenceConditionViolated { direction: 2 },
"H-infinity existence condition fails in direction 2: reduce gamma",
),
(
HInfinityError::SingularCovariance { direction: 1 },
"covariance is singular in direction 1",
),
(
HInfinityError::MeasurementNoiseNotPositiveDefinite { direction: 0 },
"measurement noise covariance is not positive-definite in direction 0",
),
(
HInfinityError::UnattainableBound { direction: 3 },
"gamma is unattainable for this prior covariance in direction 3",
),
];
for (error, expected) in messages {
assert_eq!(format!("{error}"), expected);
let dynamic: &dyn core::error::Error = &error;
assert_eq!(format!("{dynamic}"), expected);
}
}
#[test]
fn a_larger_bound_keeps_a_larger_covariance() {
let (_, z) = observations(15, None);
let mut trace = Vec::new();
for gamma in [0.0, 0.05, 0.15] {
let mut filter =
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), gamma).unwrap();
for measurement in &z {
filter
.step::<1>(&F, &H, &[*measurement], &[[R]], &SQ)
.unwrap();
}
let p = filter.covariance();
trace.push(p[0][0] + p[1][1]);
}
assert!(
trace[0] < trace[1] && trace[1] < trace[2],
"trace P must grow with gamma: {trace:?}"
);
}
fn gaussians(seed: &mut u64) -> (f64, f64) {
let mut uniform = || {
*seed = seed
.wrapping_mul(6_364_136_223_846_793_005)
.wrapping_add(1_442_695_040_888_963_407);
#[allow(clippy::cast_precision_loss)]
let u = ((*seed >> 11) as f64) / ((1_u64 << 53) as f64);
u.mul_add(0.999_999_9, 5e-8)
};
let (u1, u2) = (uniform(), uniform());
let r = libm::sqrt(-2.0 * libm::log(u1));
(
r * libm::cos(core::f64::consts::TAU * u2),
r * libm::sin(core::f64::consts::TAU * u2),
)
}
#[derive(Debug)]
struct Score {
rms_hinf: f64,
rms_kalman: f64,
peak_hinf: f64,
peak_kalman: f64,
}
fn monte_carlo(gamma: f64, manoeuvre_at: Option<usize>) -> Score {
const RUNS: usize = 200;
const STEPS: usize = 60;
const SETTLED: usize = 5;
let (mut sse_hinf, mut sse_kalman) = (0.0_f64, 0.0_f64);
let (mut peak_hinf, mut peak_kalman) = (0.0_f64, 0.0_f64);
for run in 0..RUNS {
let mut seed = 1 + u64::try_from(run).unwrap() * 7919;
let (mut position, mut rate) = (0.0_f64, 1.0_f64);
let mut hinf =
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), gamma).unwrap();
let mut kalman = SquareRootKalman::<2>::from_covariance([0.0, 0.0], &P0).unwrap();
for k in 0..STEPS {
let (drive_position, drive_rate) = gaussians(&mut seed);
let (jitter, _) = gaussians(&mut seed);
if k >= SETTLED {
let error_hinf = (hinf.x[0] - position).abs();
let error_kalman = (kalman.x[0] - position).abs();
sse_hinf += error_hinf * error_hinf;
sse_kalman += error_kalman * error_kalman;
peak_hinf = peak_hinf.max(error_hinf);
peak_kalman = peak_kalman.max(error_kalman);
}
let measurement = position + libm::sqrt(R) * jitter;
hinf.step::<1>(&F, &H, &[measurement], &[[R]], &SQ).unwrap();
let predicted = kalman.x[0];
kalman.measurement_update(&H[0], measurement - predicted, R);
let updated = kalman.x;
for i in 0..2 {
kalman.x[i] = F[i][0] * updated[0] + F[i][1] * updated[1];
}
kalman.time_update(&F, &SQ);
if manoeuvre_at == Some(k) {
rate += 3.0;
}
position += rate + SQ[0][0] * drive_position;
rate += SQ[1][1] * drive_rate;
}
}
#[allow(clippy::cast_precision_loss)]
let samples = (RUNS * (STEPS - SETTLED)) as f64;
Score {
rms_hinf: libm::sqrt(sse_hinf / samples),
rms_kalman: libm::sqrt(sse_kalman / samples),
peak_hinf,
peak_kalman,
}
}
#[test]
fn probe_negated_left() {
let (_, z) = observations(6, None);
let mut a =
HInfinityFilter::<2>::from_covariance([0.0, 0.0], &P0, identity(), 0.1).unwrap();
for m in &z {
a.step::<1>(&F, &H, &[*m], &[[R]], &SQ).unwrap();
}
let (pa, sa) = (a.covariance(), a.covariance_sqrt());
println!("P {pa:?}\nS {sa:?}");
}
}