#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
#[derive(Clone, Copy, Debug)]
pub struct SmoothingStep<const N: usize> {
pub filtered: [f64; N],
pub filtered_sqrt: [[f64; N]; N],
pub stm: [[f64; N]; N],
pub process_noise_sqrt: [[f64; N]; N],
pub predicted: [f64; N],
}
#[derive(Clone, Debug)]
pub struct RtsSmoother<const N: usize> {
x: [f64; N],
s: [[f64; N]; N],
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum RtsError {
SingularPrediction {
direction: usize,
},
}
impl core::fmt::Display for RtsError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::SingularPrediction { direction } => write!(
f,
"predicted covariance is singular in direction {direction}; the smoother gain does not exist"
),
}
}
}
impl core::error::Error for RtsError {}
impl<const N: usize> RtsSmoother<N> {
#[must_use]
pub const fn new(x: [f64; N], covariance_sqrt: [[f64; N]; N]) -> Self {
Self {
x,
s: covariance_sqrt,
}
}
pub fn step(&mut self, step: &SmoothingStep<N>) -> Result<(), RtsError> {
let mut top_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 += step.stm[i][k] * step.filtered_sqrt[k][j];
}
top_left[i][j] = acc;
}
}
let mut top_right = step.process_noise_sqrt;
let mut bottom_left = step.filtered_sqrt;
let mut bottom_right = [[0.0; N]; N];
for i in 0..N {
for j in (i + 1)..(2 * N) {
let p = top_left[i][i];
let q = if j < N {
top_left[i][j]
} else {
top_right[i][j - N]
};
let denom = libm::hypot(p, q);
if denom == 0.0 {
continue;
}
let (c, s) = (p / denom, q / denom);
rotate_columns(&mut top_left, &mut top_right, i, j, c, s);
rotate_columns(&mut bottom_left, &mut bottom_right, i, j, c, s);
}
}
let mut gain = [[0.0; N]; N];
for r in 0..N {
for i in (0..N).rev() {
if top_left[i][i] == 0.0 {
return Err(RtsError::SingularPrediction { direction: i });
}
let mut acc = bottom_left[r][i];
for j in (i + 1)..N {
acc -= gain[r][j] * top_left[j][i];
}
gain[r][i] = acc / top_left[i][i];
}
}
let mut lookahead = [0.0; N];
for i in 0..N {
lookahead[i] = self.x[i] - step.predicted[i];
}
let mut x = step.filtered;
for i in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += gain[i][k] * lookahead[k];
}
x[i] += acc;
}
let mut left = bottom_right;
let mut right = [[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 += gain[i][k] * self.s[k][j];
}
right[i][j] = acc;
}
}
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);
rotate_columns(&mut left, &mut right, i, j, c, s);
}
}
self.x = x;
self.s = left;
Ok(())
}
#[must_use]
pub const fn state(&self) -> &[f64; N] {
&self.x
}
#[must_use]
pub const fn covariance_sqrt(&self) -> [[f64; N]; N] {
self.s
}
#[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
}
}
fn rotate_columns<const N: usize>(
left: &mut [[f64; N]; N],
right: &mut [[f64; N]; N],
i: usize,
j: usize,
c: f64,
s: f64,
) {
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;
}
}
}