#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
use crate::unscented::cholesky;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ProcessNoiseError {
NonFinite,
NegativeStep,
NonPositiveCorrelationTime,
NegativeSigma,
ExponentialFailed,
}
pub type DiscreteStep<const N: usize> = ([[f64; N]; N], [[f64; N]; N]);
const PADE_ORDER: u8 = 6;
const PADE_THETA: f64 = 0.5;
const MAX_SQUARINGS: u32 = 1024;
#[must_use]
pub fn matrix_exp<const N: usize>(a: &[[f64; N]; N]) -> Option<[[f64; N]; N]> {
let mut norm = 0.0f64;
for i in 0..N {
let mut row = 0.0;
for j in 0..N {
if !a[i][j].is_finite() {
return None;
}
row += libm::fabs(a[i][j]);
}
if row > norm {
norm = row;
}
}
let mut squarings: u32 = 0;
let mut reduced = norm;
while reduced > PADE_THETA {
reduced *= 0.5;
squarings += 1;
if squarings > MAX_SQUARINGS {
return None;
}
}
let scale = libm::exp2(-f64::from(squarings));
let mut scaled = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
scaled[i][j] = a[i][j] * scale;
}
}
let mut numerator = identity::<N>();
let mut denominator = identity::<N>();
let mut power = identity::<N>();
let mut c = 1.0;
let q = f64::from(PADE_ORDER);
for k in 1..=PADE_ORDER {
let kf = f64::from(k);
c *= (q - kf + 1.0) / ((2.0 * q - kf + 1.0) * kf);
power = matmul(&power, &scaled);
let sign = if k % 2 == 0 { 1.0 } else { -1.0 };
for i in 0..N {
for j in 0..N {
numerator[i][j] += c * power[i][j];
denominator[i][j] += sign * c * power[i][j];
}
}
}
let mut result = solve(&denominator, &numerator)?;
for _ in 0..squarings {
result = matmul(&result, &result);
}
Some(result)
}
pub fn van_loan<const N: usize, const M: usize>(
f: &[[f64; N]; N],
q_continuous: &[[f64; N]; N],
dt: f64,
) -> Result<DiscreteStep<N>, ProcessNoiseError> {
assert!(M == 2 * N, "van_loan: M must equal 2 * N");
if !dt.is_finite() {
return Err(ProcessNoiseError::NonFinite);
}
if dt < 0.0 {
return Err(ProcessNoiseError::NegativeStep);
}
for i in 0..N {
for j in 0..N {
if !f[i][j].is_finite() || !q_continuous[i][j].is_finite() {
return Err(ProcessNoiseError::NonFinite);
}
}
}
let mut c = [[0.0; M]; M];
for i in 0..N {
for j in 0..N {
c[i][j] = -f[i][j] * dt;
c[i][j + N] = q_continuous[i][j] * dt;
c[i + N][j + N] = f[j][i] * dt;
}
}
let x = matrix_exp::<M>(&c).ok_or(ProcessNoiseError::ExponentialFailed)?;
let mut phi = [[0.0; N]; N];
let mut upper_right = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
phi[i][j] = x[j + N][i + N];
upper_right[i][j] = x[i][j + N];
}
}
let mut q_discrete = matmul(&phi, &upper_right);
for i in 0..N {
for j in 0..i {
let mean = f64::midpoint(q_discrete[i][j], q_discrete[j][i]);
q_discrete[i][j] = mean;
q_discrete[j][i] = mean;
}
}
Ok((phi, q_discrete))
}
pub fn state_noise_compensation(
dt: f64,
sigma_acceleration: [f64; 3],
) -> Result<[[f64; 6]; 6], ProcessNoiseError> {
if !dt.is_finite() || !sigma_acceleration.iter().all(|s| s.is_finite()) {
return Err(ProcessNoiseError::NonFinite);
}
if dt < 0.0 {
return Err(ProcessNoiseError::NegativeStep);
}
if sigma_acceleration.iter().any(|&s| s < 0.0) {
return Err(ProcessNoiseError::NegativeSigma);
}
let (rr, rv, vv) = (dt * dt * dt / 3.0, 0.5 * dt * dt, dt);
let mut q = [[0.0; 6]; 6];
for axis in 0..3 {
let density = sigma_acceleration[axis] * sigma_acceleration[axis];
q[axis][axis] = density * rr;
q[axis][axis + 3] = density * rv;
q[axis + 3][axis] = density * rv;
q[axis + 3][axis + 3] = density * vv;
}
Ok(q)
}
#[must_use]
pub fn rotate_position_velocity(q: &[[f64; 6]; 6], rotation: &[[f64; 3]; 3]) -> [[f64; 6]; 6] {
let mut a = [[0.0; 6]; 6];
for i in 0..3 {
for j in 0..3 {
a[i][j] = rotation[i][j];
a[i + 3][j + 3] = rotation[i][j];
}
}
let aq = matmul(&a, q);
let mut out = [[0.0; 6]; 6];
for i in 0..6 {
for j in 0..6 {
let mut acc = 0.0;
for k in 0..6 {
acc += aq[i][k] * a[j][k];
}
out[i][j] = acc;
}
}
out
}
pub fn dynamic_model_compensation(
dt: f64,
correlation_time: [f64; 3],
sigma_acceleration: [f64; 3],
) -> Result<DiscreteStep<9>, ProcessNoiseError> {
if !correlation_time.iter().all(|t| t.is_finite())
|| !sigma_acceleration.iter().all(|s| s.is_finite())
{
return Err(ProcessNoiseError::NonFinite);
}
if correlation_time.iter().any(|&t| t <= 0.0) {
return Err(ProcessNoiseError::NonPositiveCorrelationTime);
}
if sigma_acceleration.iter().any(|&s| s < 0.0) {
return Err(ProcessNoiseError::NegativeSigma);
}
let mut f = [[0.0; 9]; 9];
let mut q_continuous = [[0.0; 9]; 9];
for axis in 0..3 {
f[axis][axis + 3] = 1.0; f[axis + 3][axis + 6] = 1.0; f[axis + 6][axis + 6] = -1.0 / correlation_time[axis]; let sigma = sigma_acceleration[axis];
q_continuous[axis + 6][axis + 6] = 2.0 * sigma * sigma / correlation_time[axis];
}
van_loan::<9, 18>(&f, &q_continuous, dt)
}
#[must_use]
pub fn process_noise_sqrt<const N: usize>(q: &[[f64; N]; N]) -> Option<[[f64; N]; N]> {
cholesky(q)
}
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 matmul<const N: usize>(a: &[[f64; N]; N], b: &[[f64; N]; N]) -> [[f64; N]; N] {
let mut c = [[0.0; N]; N];
for i in 0..N {
for k in 0..N {
let aik = a[i][k];
if aik == 0.0 {
continue;
}
for j in 0..N {
c[i][j] += aik * b[k][j];
}
}
}
c
}
fn solve<const N: usize>(a: &[[f64; N]; N], b: &[[f64; N]; N]) -> Option<[[f64; N]; N]> {
let mut lu = *a;
let mut x = *b;
for col in 0..N {
let mut pivot = col;
let mut best = libm::fabs(lu[col][col]);
for row in (col + 1)..N {
let candidate = libm::fabs(lu[row][col]);
if candidate > best {
best = candidate;
pivot = row;
}
}
if best == 0.0 {
return None;
}
if pivot != col {
lu.swap(pivot, col);
x.swap(pivot, col);
}
for row in (col + 1)..N {
let factor = lu[row][col] / lu[col][col];
if factor == 0.0 {
continue;
}
for k in col..N {
lu[row][k] -= factor * lu[col][k];
}
for k in 0..N {
x[row][k] -= factor * x[col][k];
}
}
}
for col in (0..N).rev() {
for k in 0..N {
let mut acc = x[col][k];
for j in (col + 1)..N {
acc -= lu[col][j] * x[j][k];
}
x[col][k] = acc / lu[col][col];
}
}
Some(x)
}
#[cfg(test)]
mod tests {
#![allow(clippy::expect_used, clippy::unwrap_used, clippy::float_cmp)]
use super::*;
fn max_abs_diff<const N: usize>(a: &[[f64; N]; N], b: &[[f64; N]; N]) -> f64 {
let mut worst = 0.0f64;
for i in 0..N {
for j in 0..N {
let d = libm::fabs(a[i][j] - b[i][j]);
if d > worst {
worst = d;
}
}
}
worst
}
#[test]
fn a_non_finite_input_is_rejected_whichever_argument_carries_it() {
let sigma = [1.0, 2.0, 3.0];
let tau = [100.0; 3];
for bad in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
assert_eq!(
state_noise_compensation(bad, sigma).unwrap_err(),
ProcessNoiseError::NonFinite,
"SNC accepted a step of {bad} beside finite sigmas"
);
assert_eq!(
state_noise_compensation(10.0, [bad, 2.0, 3.0]).unwrap_err(),
ProcessNoiseError::NonFinite,
"SNC accepted a sigma of {bad} beside a finite step"
);
assert_eq!(
dynamic_model_compensation(10.0, [bad, 100.0, 100.0], sigma).unwrap_err(),
ProcessNoiseError::NonFinite,
"DMC accepted a correlation time of {bad} beside finite sigmas"
);
assert_eq!(
dynamic_model_compensation(10.0, tau, [bad, 2.0, 3.0]).unwrap_err(),
ProcessNoiseError::NonFinite,
"DMC accepted a sigma of {bad} beside a finite correlation time"
);
}
}
#[test]
fn zero_process_noise_is_a_request_and_not_an_error() {
let snc = state_noise_compensation(10.0, [0.0; 3]).unwrap();
assert_eq!(max_abs_diff(&snc, &[[0.0; 6]; 6]), 0.0);
let (transition, noise) = dynamic_model_compensation(10.0, [100.0; 3], [0.0; 3]).unwrap();
assert_eq!(max_abs_diff(&noise, &[[0.0; 9]; 9]), 0.0);
assert!(transition[6][6] < 1.0, "the DMC decay went missing");
}
#[test]
fn solve_pivots_and_breaks_ties_toward_the_first_row() {
let eye = [[1.0, 0.0], [0.0, 1.0]];
let swapped = solve(&[[0.0, 1.0], [1.0, 0.0]], &eye).expect("a permutation is invertible");
assert_eq!(swapped, [[0.0, 1.0], [1.0, 0.0]]);
let tied = solve(&[[1.0, 3.0], [-1.0, 7.0]], &eye).expect("determinant 10");
assert_eq!(tied[0][0].to_bits(), 0.7_f64.to_bits());
assert_eq!(
tied[0][1].to_bits(),
(-0.300_000_000_000_000_04_f64).to_bits()
);
assert_eq!(tied[1][0].to_bits(), 0.1_f64.to_bits());
assert_eq!(tied[1][1].to_bits(), 0.1_f64.to_bits());
}
#[test]
fn the_squaring_ladder_runs_to_both_of_its_documented_boundaries() {
let at_theta = matrix_exp(&[[PADE_THETA, 0.0], [0.0, -PADE_THETA]]).expect("finite");
assert_eq!(
at_theta[0][0].to_bits(),
1.648_721_270_700_128_f64.to_bits()
);
assert_eq!(
at_theta[1][1].to_bits(),
6.065_306_597_126_335e-1_f64.to_bits()
);
for (got, want) in [
(at_theta[0][0], libm::exp(PADE_THETA)),
(at_theta[1][1], libm::exp(-PADE_THETA)),
] {
let ulp = libm::fabs(want) * f64::EPSILON;
assert!(libm::fabs(got - want) <= ulp, "{got:e} against {want:e}");
}
let n = libm::exp2(1023.0);
let nilpotent = matrix_exp(&[[0.0, n], [0.0, 0.0]]).expect("exp(N) = I + N is finite");
assert_eq!(nilpotent, [[1.0, n], [0.0, 1.0]]);
}
#[test]
fn the_exponential_of_a_diagonal_is_diagonal() {
let a = [[0.3, 0.0, 0.0], [0.0, -1.7, 0.0], [0.0, 0.0, 5.0]];
let e = matrix_exp(&a).unwrap();
let want = [
[libm::exp(0.3), 0.0, 0.0],
[0.0, libm::exp(-1.7), 0.0],
[0.0, 0.0, libm::exp(5.0)],
];
assert!(max_abs_diff(&e, &want) < 1e-12 * libm::exp(5.0));
}
#[test]
fn the_exponential_of_a_jordan_block_is_the_closed_form() {
let a = 2.5;
let m = [[a, 1.0], [0.0, a]];
let e = matrix_exp(&m).unwrap();
let ea = libm::exp(a);
let want = [[ea, ea], [0.0, ea]];
assert!(max_abs_diff(&e, &want) < 1e-12 * ea);
}
#[test]
fn the_exponential_survives_widely_separated_eigenvalues() {
let (l1, l2) = (-30.0, 1e-4);
let v = [[1.0, 1.0], [0.0, 1.0]];
let v_inv = [[1.0, -1.0], [0.0, 1.0]];
let d = [[l1, 0.0], [0.0, l2]];
let m = matmul(&matmul(&v, &d), &v_inv);
let e = matrix_exp(&m).unwrap();
let exp_d = [[libm::exp(l1), 0.0], [0.0, libm::exp(l2)]];
let want = matmul(&matmul(&v, &exp_d), &v_inv);
assert!(max_abs_diff(&e, &want) < 1e-13);
}
#[test]
fn the_exponential_of_zero_is_the_identity_and_inverses_compose() {
let zero = [[0.0; 4]; 4];
assert_eq!(
max_abs_diff(&matrix_exp(&zero).unwrap(), &identity::<4>()),
0.0
);
let a = [
[0.1, -0.4, 2.0, 0.0],
[1.3, 0.0, -0.2, 0.7],
[0.0, 0.9, -1.1, 0.3],
[-0.5, 0.2, 0.0, 0.4],
];
let mut neg = [[0.0; 4]; 4];
for i in 0..4 {
for j in 0..4 {
neg[i][j] = -a[i][j];
}
}
let product = matmul(&matrix_exp(&a).unwrap(), &matrix_exp(&neg).unwrap());
assert!(max_abs_diff(&product, &identity::<4>()) < 1e-13);
}
#[test]
fn a_non_finite_exponential_argument_is_rejected() {
assert!(matrix_exp(&[[f64::NAN, 0.0], [0.0, 1.0]]).is_none());
assert!(matrix_exp(&[[f64::INFINITY, 0.0], [0.0, 1.0]]).is_none());
}
#[test]
fn van_loan_reproduces_the_double_integrator_closed_form() {
let (q, dt) = (1e-12, 60.0);
let f = [[0.0, 1.0], [0.0, 0.0]];
let qc = [[0.0, 0.0], [0.0, q]];
let (phi, qd) = van_loan::<2, 4>(&f, &qc, dt).unwrap();
assert!(max_abs_diff(&phi, &[[1.0, dt], [0.0, 1.0]]) < 1e-13 * dt);
let want = [
[q * dt * dt * dt / 3.0, q * dt * dt / 2.0],
[q * dt * dt / 2.0, q * dt],
];
assert!(max_abs_diff(&qd, &want) < 1e-14 * want[0][0]);
}
#[test]
fn van_loan_reproduces_the_gauss_markov_closed_form() {
let (tau, q, dt) = (200.0, 1e-12, 60.0);
let f = [[-1.0 / tau]];
let qc = [[q]];
let (phi, qd) = van_loan::<1, 2>(&f, &qc, dt).unwrap();
assert!(libm::fabs(phi[0][0] - libm::exp(-dt / tau)) < 1e-15);
let want = 0.5 * q * tau * (1.0 - libm::exp(-2.0 * dt / tau));
assert!(libm::fabs(qd[0][0] - want) < 1e-15 * want);
let (_, far) = van_loan::<1, 2>(&f, &qc, 200.0 * tau).unwrap();
assert!(libm::fabs(far[0][0] - 0.5 * q * tau) < 5e-14 * q * tau);
}
#[test]
fn van_loan_reproduces_the_dmc_closed_form() {
let (tau, q, dt) = (200.0, 1e-12, 60.0);
let f = [[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [0.0, 0.0, -1.0 / tau]];
let qc = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, q]];
let (phi, qd) = van_loan::<3, 6>(&f, &qc, dt).unwrap();
let want_phi = [
[1.0, 60.0, 1_632.728_827_268_714_7],
[0.0, 1.0, 51.836_355_863_656_43],
[0.0, 0.0, 0.740_818_220_681_717_9],
];
let want_q = [
[
3.303_985_406_594_049_5e-5,
1.332_901_711_697_136e-6,
2.678_972_598_777_139e-8,
],
[
1.332_901_711_697_136e-6,
5.784_498_653_138_012_6e-8,
1.343_503_894_611_814_1e-9,
],
[
2.678_972_598_777_139e-8,
1.343_503_894_611_814_1e-9,
4.511_883_639_059_735_6e-11,
],
];
for i in 0..3 {
for j in 0..3 {
assert!(
libm::fabs(phi[i][j] - want_phi[i][j]) <= 1e-12 * libm::fabs(want_phi[i][j])
);
let scale = libm::fabs(want_q[i][j]);
assert!(libm::fabs(qd[i][j] - want_q[i][j]) <= 1e-9 * scale);
}
}
}
#[test]
fn the_van_loan_asymmetry_is_rounding_sized_before_it_is_averaged() {
let (tau, q, dt) = (150.0, 4e-11, 30.0);
let f = [[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [0.0, 0.0, -1.0 / tau]];
let qc = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, q]];
let mut c = [[0.0; 6]; 6];
for i in 0..3 {
for j in 0..3 {
c[i][j] = -f[i][j] * dt;
c[i][j + 3] = qc[i][j] * dt;
c[i + 3][j + 3] = f[j][i] * dt;
}
}
let x = matrix_exp::<6>(&c).unwrap();
let mut phi = [[0.0; 3]; 3];
let mut upper = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
phi[i][j] = x[j + 3][i + 3];
upper[i][j] = x[i][j + 3];
}
}
let raw = matmul(&phi, &upper);
let scale = raw[0][0];
for i in 0..3 {
for j in 0..3 {
assert!(libm::fabs(raw[i][j] - raw[j][i]) < 1e-12 * scale);
}
}
}
#[test]
fn state_noise_compensation_is_the_double_integrator_per_axis() {
let dt = 45.0;
let sigma = [3e-9, 1e-8, 5e-9];
let q = state_noise_compensation(dt, sigma).unwrap();
for axis in 0..3 {
let density = sigma[axis] * sigma[axis];
let f = [[0.0, 1.0], [0.0, 0.0]];
let qc = [[0.0, 0.0], [0.0, density]];
let (_, want) = van_loan::<2, 4>(&f, &qc, dt).unwrap();
let got = [
[q[axis][axis], q[axis][axis + 3]],
[q[axis + 3][axis], q[axis + 3][axis + 3]],
];
assert!(max_abs_diff(&got, &want) < 1e-14 * want[0][0]);
}
for i in 0..6 {
for j in 0..6 {
if i % 3 != j % 3 {
assert_eq!(q[i][j], 0.0);
}
}
}
}
#[test]
fn the_position_velocity_rotation_is_an_orthogonal_congruence() {
let q = state_noise_compensation(60.0, [2e-9, 7e-9, 4e-9]).unwrap();
let eye = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
assert_eq!(max_abs_diff(&rotate_position_velocity(&q, &eye), &q), 0.0);
let (c, s) = (libm::cos(0.7), libm::sin(0.7));
let r = [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]];
let rt = [[c, s, 0.0], [-s, c, 0.0], [0.0, 0.0, 1.0]];
let round_trip = rotate_position_velocity(&rotate_position_velocity(&q, &r), &rt);
assert!(max_abs_diff(&round_trip, &q) < 1e-16);
let iso = state_noise_compensation(60.0, [5e-9; 3]).unwrap();
assert!(max_abs_diff(&rotate_position_velocity(&iso, &r), &iso) < 1e-24);
assert!(max_abs_diff(&rotate_position_velocity(&q, &r), &q) > 1e-20);
}
#[test]
fn dynamic_model_compensation_is_three_independent_axes() {
let dt = 60.0;
let tau = [200.0, 400.0, 150.0];
let sigma = [1e-8, 3e-8, 2e-8];
let (phi, q) = dynamic_model_compensation(dt, tau, sigma).unwrap();
for axis in 0..3 {
let density = 2.0 * sigma[axis] * sigma[axis] / tau[axis];
let f = [
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0],
[0.0, 0.0, -1.0 / tau[axis]],
];
let qc = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, density]];
let (want_phi, want_q) = van_loan::<3, 6>(&f, &qc, dt).unwrap();
let idx = [axis, axis + 3, axis + 6];
for i in 0..3 {
for j in 0..3 {
let got_phi = phi[idx[i]][idx[j]];
let got_q = q[idx[i]][idx[j]];
assert!(
libm::fabs(got_phi - want_phi[i][j])
< 1e-12 * (1.0 + libm::fabs(want_phi[i][j]))
);
assert!(libm::fabs(got_q - want_q[i][j]) < 1e-10 * libm::fabs(want_q[0][0]));
}
}
}
let (_, far) = dynamic_model_compensation(500.0 * tau[0], tau, sigma).unwrap();
assert!(libm::fabs(far[6][6] - sigma[0] * sigma[0]) < 1e-9 * sigma[0] * sigma[0]);
}
#[test]
fn van_loan_accuracy_degrades_only_with_the_dynamic_range_of_the_block() {
let (tau, q) = (200.0, 1e-12);
for ratio in [0.05, 0.3, 1.0, 5.0, 20.0, 200.0] {
let dt = ratio * tau;
let (_, qd) = van_loan::<1, 2>(&[[-1.0 / tau]], &[[q]], dt).unwrap();
let want = 0.5 * q * tau * (1.0 - libm::exp(-2.0 * dt / tau));
assert!(
libm::fabs(qd[0][0] - want) / want < 5e-14,
"scalar Gauss-Markov at dt/tau = {ratio}"
);
}
for ratio in [0.05, 1.0, 20.0] {
let dt = ratio * tau;
let f = [[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [0.0, 0.0, -1.0 / tau]];
let qc = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, q]];
let (_, qd) = van_loan::<3, 6>(&f, &qc, dt).unwrap();
let want = 0.5 * q * tau * (1.0 - libm::exp(-2.0 * dt / tau));
assert!(
libm::fabs(qd[2][2] - want) / want < 1e-12,
"three-state Q_ww at dt/tau = {ratio}"
);
}
}
#[test]
fn the_dmc_acceleration_stays_correlated_across_a_short_step() {
let tau = [600.0; 3];
let sigma = [1e-8; 3];
let (phi, _) = dynamic_model_compensation(30.0, tau, sigma).unwrap();
assert!(phi[6][6] > 0.95);
let (phi_long, _) = dynamic_model_compensation(6000.0, tau, sigma).unwrap();
assert!(phi_long[6][6] < 0.01);
}
#[test]
fn the_preconditions_are_reported_separately() {
let f = [[0.0, 1.0], [0.0, 0.0]];
let qc = [[0.0, 0.0], [0.0, 1e-12]];
assert_eq!(
van_loan::<2, 4>(&f, &qc, -1.0),
Err(ProcessNoiseError::NegativeStep)
);
assert_eq!(
van_loan::<2, 4>(&f, &qc, f64::NAN),
Err(ProcessNoiseError::NonFinite)
);
assert_eq!(
van_loan::<2, 4>(&[[f64::NAN, 0.0], [0.0, 0.0]], &qc, 1.0),
Err(ProcessNoiseError::NonFinite)
);
assert_eq!(
dynamic_model_compensation(60.0, [200.0, 0.0, 200.0], [1e-8; 3]),
Err(ProcessNoiseError::NonPositiveCorrelationTime)
);
assert_eq!(
dynamic_model_compensation(60.0, [200.0; 3], [1e-8, -1e-8, 1e-8]),
Err(ProcessNoiseError::NegativeSigma)
);
assert_eq!(
state_noise_compensation(-1.0, [1e-9; 3]),
Err(ProcessNoiseError::NegativeStep)
);
assert_eq!(
state_noise_compensation(60.0, [1e-9, -1.0, 1e-9]),
Err(ProcessNoiseError::NegativeSigma)
);
}
#[test]
fn a_zero_step_accumulates_no_noise() {
let q = state_noise_compensation(0.0, [1e-8; 3]).unwrap();
assert!(q.iter().flatten().all(|&e| e == 0.0));
let (phi, qd) = dynamic_model_compensation(0.0, [200.0; 3], [1e-8; 3]).unwrap();
assert_eq!(max_abs_diff(&phi, &identity::<9>()), 0.0);
assert!(qd.iter().flatten().all(|&e| libm::fabs(e) < 1e-30));
}
#[test]
fn the_discrete_noise_factors_into_the_square_root_the_filters_take() {
let q = state_noise_compensation(60.0, [1e-8, 2e-8, 5e-9]).unwrap();
let s = process_noise_sqrt(&q).unwrap();
let mut reconstructed = [[0.0; 6]; 6];
for i in 0..6 {
for j in 0..6 {
let mut acc = 0.0;
for k in 0..6 {
acc += s[i][k] * s[j][k];
}
reconstructed[i][j] = acc;
}
}
assert!(max_abs_diff(&reconstructed, &q) < 1e-14 * q[0][0]);
let mut singular = q;
for j in 0..6 {
singular[2][j] = 0.0;
singular[j][2] = 0.0;
}
assert!(process_noise_sqrt(&singular).is_none());
}
}