#![allow(clippy::many_single_char_names, clippy::needless_range_loop)]
#[must_use]
pub fn consider_sensitivity<const X: usize, const C: usize>(
estimated_covariance: &[[f64; X]; X],
cross_information: &[[f64; C]; X],
) -> [[f64; C]; X] {
let mut s = [[0.0; C]; X];
for i in 0..X {
for j in 0..C {
let mut acc = 0.0;
for k in 0..X {
acc += estimated_covariance[i][k] * cross_information[k][j];
}
s[i][j] = -acc;
}
}
s
}
#[must_use]
pub fn consider_covariance<const X: usize, const C: usize>(
estimated_covariance: &[[f64; X]; X],
sensitivity: &[[f64; C]; X],
consider_covariance: &[[f64; C]; C],
) -> [[f64; X]; X] {
let mut m = [[0.0; C]; X];
for i in 0..X {
for j in 0..C {
let mut acc = 0.0;
for k in 0..C {
acc += sensitivity[i][k] * consider_covariance[k][j];
}
m[i][j] = acc;
}
}
let mut out = *estimated_covariance;
for i in 0..X {
for j in 0..X {
let mut acc = 0.0;
for k in 0..C {
acc += m[i][k] * sensitivity[j][k];
}
out[i][j] += acc;
}
}
out
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn scalar_consider_matches_the_closed_form_error_variance() {
let (hx, hc, r, pcc) = (2.0, 3.0, 0.5, 4.0);
let p = [[r / (hx * hx)]]; let cross = [[hx * hc / r]]; let s = consider_sensitivity(&p, &cross);
assert!((s[0][0] - (-hc / hx)).abs() < 1e-12, "S {}", s[0][0]); let pc = consider_covariance(&p, &s, &[[pcc]]);
let closed = (hc * hc * pcc + r) / (hx * hx);
assert!(
(pc[0][0] - closed).abs() < 1e-12,
"Pc {} vs {closed}",
pc[0][0]
);
assert!(pc[0][0] > p[0][0]); }
#[test]
fn two_state_one_consider_matches_the_hand_computed_matrix() {
let p = [[0.1, 0.0], [0.0, 0.2]];
let cross = [[5.0], [-3.0]]; let s = consider_sensitivity(&p, &cross);
assert!(
(s[0][0] + 0.5).abs() < 1e-12 && (s[1][0] - 0.6).abs() < 1e-12,
"{s:?}"
);
let pc = consider_covariance(&p, &s, &[[2.0]]);
let expected = [[0.6, -0.6], [-0.6, 0.92]];
for i in 0..2 {
for j in 0..2 {
assert!(
(pc[i][j] - expected[i][j]).abs() < 1e-12,
"Pc[{i}][{j}] {}",
pc[i][j]
);
}
}
assert!((pc[0][1] - pc[1][0]).abs() < 1e-15);
assert!(pc[0][0] > p[0][0] && pc[1][1] > p[1][1]);
}
#[test]
fn consider_sensitivity_matches_the_direct_information_route() {
let (hx0, hx1, hc, r, pcc) = (1.0, 0.5, 0.8, 0.25, 3.0);
let p = [[0.4, 0.1], [0.1, 0.5]];
let cross = [[hx0 * hc / r], [hx1 * hc / r]]; let s = consider_sensitivity(&p, &cross);
let phx = [p[0][0] * hx0 + p[0][1] * hx1, p[1][0] * hx0 + p[1][1] * hx1]; for i in 0..2 {
assert!(
(s[i][0] - (-phx[i] * hc / r)).abs() < 1e-12,
"S[{i}] {}",
s[i][0]
);
}
let pc = consider_covariance(&p, &s, &[[pcc]]);
for i in 0..2 {
for j in 0..2 {
let expected = p[i][j] + (phx[i] * hc / r) * pcc * (phx[j] * hc / r);
assert!(
(pc[i][j] - expected).abs() < 1e-12,
"Pc[{i}][{j}] {}",
pc[i][j]
);
}
}
}
}