#![allow(clippy::many_single_char_names, clippy::needless_range_loop)]
#[derive(Debug, Clone)]
pub struct LinearCovariance<const N: usize, const M: usize> {
map: [[f64; N]; M],
dispersion: [[f64; N]; N],
cross: [[f64; N]; M],
error: [[f64; M]; M],
filter: [[f64; M]; M],
}
impl<const N: usize, const M: usize> LinearCovariance<N, M> {
#[must_use]
pub const fn new(map: [[f64; N]; M], dispersion: [[f64; N]; N], filter: [[f64; M]; M]) -> Self {
Self {
map,
dispersion,
cross: [[0.0; N]; M],
error: filter,
filter,
}
}
#[must_use]
pub const fn with_initial_error(mut self, error: [[f64; M]; M]) -> Self {
self.error = error;
self
}
pub fn time_update(
&mut self,
truth_stm: &[[f64; N]; N],
truth_process_noise: &[[f64; N]; N],
filter_stm: &[[f64; M]; M],
filter_process_noise: &[[f64; M]; M],
) {
let mut d = [[0.0; N]; M];
for i in 0..M {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += self.map[i][k] * truth_stm[k][j];
}
for k in 0..M {
acc -= filter_stm[i][k] * self.map[k][j];
}
d[i][j] = acc;
}
}
let phi_cxx = mul(truth_stm, &self.dispersion);
let mut dispersion = mul_transpose(&phi_cxx, truth_stm);
add_in_place(&mut dispersion, truth_process_noise);
let mut inner = mul(&d, &self.dispersion);
let phi_hat_cex = mul(filter_stm, &self.cross);
add_in_place(&mut inner, &phi_hat_cex);
let mut cross = mul_transpose(&inner, truth_stm);
let tq = mul(&self.map, truth_process_noise);
add_in_place(&mut cross, &tq);
let mut error = mul_transpose(&inner, &d);
let cross_dt = mul_transpose(&self.cross, &d); for i in 0..M {
for j in 0..M {
let mut acc = 0.0;
for k in 0..M {
acc += filter_stm[j][k] * cross_dt[k][i];
}
error[i][j] += acc;
}
}
let carried = mul(filter_stm, &self.error);
let propagated = mul_transpose(&carried, filter_stm);
add_in_place(&mut error, &propagated);
let tqt = mul_transpose(&tq, &self.map);
add_in_place(&mut error, &tqt);
self.dispersion = dispersion;
self.cross = cross;
self.error = error;
let phi_p = mul(filter_stm, &self.filter);
let mut filter = mul_transpose(&phi_p, filter_stm);
add_in_place(&mut filter, filter_process_noise);
self.filter = filter;
}
pub fn measurement_update(
&mut self,
truth_partials: &[f64; N],
filter_partials: &[f64; M],
true_noise_variance: f64,
filter_noise_variance: f64,
) -> f64 {
let mut p_ht = [0.0; M];
for i in 0..M {
let mut acc = 0.0;
for k in 0..M {
acc += self.filter[i][k] * filter_partials[k];
}
p_ht[i] = acc;
}
let mut innovation_variance = filter_noise_variance;
for i in 0..M {
innovation_variance += filter_partials[i] * p_ht[i];
}
let mut gain = [0.0; M];
if innovation_variance > 0.0 {
for i in 0..M {
gain[i] = p_ht[i] / innovation_variance;
}
}
let mut delta = *truth_partials;
for j in 0..N {
let mut acc = 0.0;
for k in 0..M {
acc += filter_partials[k] * self.map[k][j];
}
delta[j] -= acc;
}
let mut a = [[0.0; M]; M];
for i in 0..M {
a[i][i] = 1.0;
for j in 0..M {
a[i][j] -= gain[i] * filter_partials[j];
}
}
let mut cross = [[0.0; N]; M];
for i in 0..M {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc -= gain[i] * delta[k] * self.dispersion[k][j];
}
for k in 0..M {
acc += a[i][k] * self.cross[k][j];
}
cross[i][j] = acc;
}
}
let a_cee = mul(&a, &self.error);
let mut error = mul_transpose(&a_cee, &a);
let mut cxx_dt = [0.0; N];
for i in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += self.dispersion[i][k] * delta[k];
}
cxx_dt[i] = acc;
}
let mut d_cxx_dt = 0.0;
for i in 0..N {
d_cxx_dt += delta[i] * cxx_dt[i];
}
let mut correlated = [0.0; M];
for i in 0..M {
let mut acc = 0.0;
for k in 0..N {
acc += self.cross[i][k] * delta[k];
}
correlated[i] = acc;
}
let mut a_cex_dt = [0.0; M];
for i in 0..M {
let mut acc = 0.0;
for k in 0..M {
acc += a[i][k] * correlated[k];
}
a_cex_dt[i] = acc;
}
for i in 0..M {
for j in 0..M {
error[i][j] +=
gain[i] * d_cxx_dt * gain[j] - gain[i] * a_cex_dt[j] - a_cex_dt[i] * gain[j]
+ gain[i] * true_noise_variance * gain[j];
}
}
let a_p = mul(&a, &self.filter);
let mut filter = mul_transpose(&a_p, &a);
for i in 0..M {
for j in 0..M {
filter[i][j] += gain[i] * filter_noise_variance * gain[j];
}
}
self.cross = cross;
self.error = error;
self.filter = filter;
innovation_variance
}
#[must_use]
pub const fn true_covariance(&self) -> &[[f64; M]; M] {
&self.error
}
#[must_use]
pub const fn filter_covariance(&self) -> &[[f64; M]; M] {
&self.filter
}
#[must_use]
pub const fn dispersion(&self) -> &[[f64; N]; N] {
&self.dispersion
}
#[must_use]
pub const fn cross_covariance(&self) -> &[[f64; N]; M] {
&self.cross
}
#[must_use]
pub fn optimism(&self) -> [f64; M] {
let mut out = [0.0; M];
for i in 0..M {
out[i] = if self.filter[i][i] > 0.0 {
libm::sqrt(self.error[i][i] / self.filter[i][i])
} else {
f64::INFINITY
};
}
out
}
}
fn mul<const R: usize, const K: usize, const C: usize>(
a: &[[f64; K]; R],
b: &[[f64; C]; K],
) -> [[f64; C]; R] {
let mut out = [[0.0; C]; R];
for i in 0..R {
for j in 0..C {
let mut acc = 0.0;
for k in 0..K {
acc += a[i][k] * b[k][j];
}
out[i][j] = acc;
}
}
out
}
fn mul_transpose<const R: usize, const K: usize, const C: usize>(
a: &[[f64; K]; R],
b: &[[f64; K]; C],
) -> [[f64; C]; R] {
let mut out = [[0.0; C]; R];
for i in 0..R {
for j in 0..C {
let mut acc = 0.0;
for k in 0..K {
acc += a[i][k] * b[j][k];
}
out[i][j] = acc;
}
}
out
}
fn add_in_place<const R: usize, const C: usize>(a: &mut [[f64; C]; R], b: &[[f64; C]; R]) {
for i in 0..R {
for j in 0..C {
a[i][j] += b[i][j];
}
}
}
#[cfg(test)]
mod tests {
use super::*;
fn selection<const N: usize, const M: usize>() -> [[f64; N]; M] {
let mut t = [[0.0; N]; M];
for i in 0..M {
t[i][i] = 1.0;
}
t
}
#[test]
fn the_two_covariances_are_identical_when_the_filter_is_right() {
let phi = [[1.0, 0.5], [0.0, 1.0]];
let q = [[1e-4, 0.0], [0.0, 4e-4]];
let p0 = [[9.0, 0.0], [0.0, 0.25]];
let mut analysis =
LinearCovariance::<2, 2>::new(selection(), [[100.0, 0.0], [0.0, 1.0]], p0);
for step in 0..40 {
analysis.time_update(&phi, &q, &phi, &q);
let h = [1.0, 0.0];
analysis.measurement_update(&h, &h, 0.04, 0.04);
if step % 7 == 0 {
let h = [0.0, 1.0];
analysis.measurement_update(&h, &h, 0.01, 0.01);
}
}
for i in 0..2 {
for j in 0..2 {
let (truth, claimed) = (
analysis.true_covariance()[i][j],
analysis.filter_covariance()[i][j],
);
assert!(
(truth - claimed).abs() <= 1e-12 * claimed.abs().max(1.0),
"Cee[{i}][{j}] {truth} vs P̂ {claimed}"
);
}
}
for ratio in analysis.optimism() {
assert!((ratio - 1.0).abs() < 1e-9, "optimism {ratio}");
}
assert!(
analysis.cross_covariance()[0][0].abs() > 1e-4,
"Cex {:?}",
analysis.cross_covariance()
);
}
#[test]
fn a_perfect_filter_with_no_process_noise_never_correlates_with_the_dispersion() {
let phi = [[1.0, 0.25], [0.0, 1.0]];
let zero = [[0.0; 2]; 2];
let mut analysis = LinearCovariance::<2, 2>::new(
selection(),
[[36.0, 0.0], [0.0, 4.0]],
[[9.0, 0.0], [0.0, 1.0]],
);
for _ in 0..25 {
analysis.time_update(&phi, &zero, &phi, &zero);
analysis.measurement_update(&[1.0, 0.0], &[1.0, 0.0], 0.04, 0.04);
}
for row in analysis.cross_covariance() {
for value in row {
assert!(value.abs() < 1e-14, "Cex {:?}", analysis.cross_covariance());
}
}
}
#[test]
fn understating_the_measurement_noise_makes_the_filter_optimistic() {
let phi = [[1.0]];
let q = [[1e-6]];
let mut analysis = LinearCovariance::<1, 1>::new([[1.0]], [[4.0]], [[1.0]]);
for _ in 0..30 {
analysis.time_update(&phi, &q, &phi, &q);
analysis.measurement_update(&[1.0], &[1.0], 0.25, 0.01);
}
let (truth, claimed) = (
analysis.true_covariance()[0][0],
analysis.filter_covariance()[0][0],
);
assert!(truth > claimed, "achieved {truth} vs claimed {claimed}");
assert!(analysis.optimism()[0] > 2.0, "{:?}", analysis.optimism());
assert!(truth.is_finite() && truth > 0.0);
}
#[test]
fn an_unmodelled_state_biases_the_error_through_the_dispersion() {
let truth_stm = [[1.0, 0.0], [0.0, 1.0]];
let truth_q = [[1e-6, 0.0], [0.0, 0.0]];
let filter_stm = [[1.0]];
let filter_q = [[1e-6]];
let bias_variance = 0.09;
let mut analysis = LinearCovariance::<2, 1>::new(
[[1.0, 0.0]],
[[1.0, 0.0], [0.0, bias_variance]],
[[1.0]],
);
for _ in 0..200 {
analysis.time_update(&truth_stm, &truth_q, &filter_stm, &filter_q);
analysis.measurement_update(&[1.0, 1.0], &[1.0], 0.04, 0.04);
}
let truth = analysis.true_covariance()[0][0];
let claimed = analysis.filter_covariance()[0][0];
assert!(
claimed < 1e-3,
"the filter should think it has converged: {claimed}"
);
assert!(
(truth - bias_variance).abs() < 0.01 * bias_variance,
"achieved {truth} against the bias variance {bias_variance}"
);
assert!(
analysis.cross_covariance()[0][1].abs() > 0.5 * bias_variance,
"Cex {:?}",
analysis.cross_covariance()
);
}
#[test]
fn the_consider_covariance_is_the_one_measurement_limit_of_this_recursion() {
let (hx, hc, r, pcc) = (2.0, 3.0, 0.5, 4.0);
let diffuse = 1e12;
let mut analysis =
LinearCovariance::<2, 1>::new([[1.0, 0.0]], [[diffuse, 0.0], [0.0, pcc]], [[diffuse]]);
analysis.measurement_update(&[hx, hc], &[hx], r, r);
let p = [[r / (hx * hx)]];
let cross = [[hx * hc / r]];
let sensitivity = crate::consider::consider_sensitivity(&p, &cross);
let expected = crate::consider::consider_covariance(&p, &sensitivity, &[[pcc]]);
let achieved = analysis.true_covariance()[0][0];
assert!(
(achieved / expected[0][0] - 1.0).abs() < 1e-6,
"LinCov {achieved} against consider {}",
expected[0][0]
);
assert!(
(analysis.filter_covariance()[0][0] / p[0][0] - 1.0).abs() < 1e-6,
"claimed {}",
analysis.filter_covariance()[0][0]
);
}
#[test]
fn a_mismodelled_transition_feeds_the_dispersion_into_the_error() {
let truth_stm = [[1.02]];
let filter_stm = [[1.0]];
let zero = [[0.0]];
let dispersion = 4.0;
let mut analysis = LinearCovariance::<1, 1>::new([[1.0]], [[dispersion]], [[0.25]]);
analysis.time_update(&truth_stm, &zero, &filter_stm, &zero);
let expected = 0.02_f64.powi(2) * dispersion + 0.25;
assert!(
(analysis.true_covariance()[0][0] - expected).abs() < 1e-15,
"{} vs {expected}",
analysis.true_covariance()[0][0]
);
assert!((analysis.filter_covariance()[0][0] - 0.25).abs() < 1e-15);
assert!(
(analysis.cross_covariance()[0][0] - 0.02 * dispersion * 1.02).abs() < 1e-15,
"{}",
analysis.cross_covariance()[0][0]
);
}
#[test]
fn both_covariances_stay_symmetric_and_positive_on_the_diagonal() {
let truth_stm = [[1.0, 0.3], [0.0, 0.99]];
let filter_stm = [[1.0, 0.31], [0.0, 1.0]];
let truth_q = [[2e-4, 1e-5], [1e-5, 3e-4]];
let filter_q = [[1e-4, 0.0], [0.0, 1e-4]];
let mut analysis = LinearCovariance::<2, 2>::new(
selection(),
[[16.0, 0.0], [0.0, 4.0]],
[[4.0, 0.0], [0.0, 1.0]],
);
for step in 0..50 {
analysis.time_update(&truth_stm, &truth_q, &filter_stm, &filter_q);
let truth_h = [1.0, 0.05];
let filter_h = [1.0, 0.0];
analysis.measurement_update(&truth_h, &filter_h, 0.09, 0.04);
assert!(
analysis.true_covariance()[0][0] > 0.0 && analysis.true_covariance()[1][1] > 0.0,
"step {step}: {:?}",
analysis.true_covariance()
);
let c = analysis.true_covariance();
assert!(
(c[0][1] - c[1][0]).abs() <= 1e-12 * c[0][1].abs().max(1.0),
"step {step}: Cee asymmetric {c:?}"
);
let p = analysis.filter_covariance();
assert!(
(p[0][1] - p[1][0]).abs() <= 1e-12 * p[0][1].abs().max(1.0),
"step {step}: P̂ asymmetric {p:?}"
);
}
for ratio in analysis.optimism() {
assert!(ratio > 1.0, "{:?}", analysis.optimism());
}
}
#[allow(clippy::float_cmp)]
#[test]
fn a_filter_told_a_zero_innovation_variance_takes_no_gain() {
let mut analysis = LinearCovariance::<1, 1>::new([[1.0]], [[1.0]], [[0.0]]);
let variance = analysis.measurement_update(&[1.0], &[1.0], 1.0, 0.0);
assert_eq!(variance, 0.0, "innovation variance");
assert_eq!(analysis.true_covariance()[0][0], 0.0);
assert_eq!(analysis.filter_covariance()[0][0], 0.0);
assert!(analysis.optimism()[0].is_infinite());
}
}