#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
use crate::rts::{RtsError, RtsSmoother, SmoothingStep};
#[derive(Clone, Debug)]
pub struct FixedLagSmoother<const N: usize, const LAG: usize> {
window: [SmoothingStep<N>; LAG],
next: usize,
depth: usize,
}
impl<const N: usize, const LAG: usize> Default for FixedLagSmoother<N, LAG> {
fn default() -> Self {
Self::new()
}
}
impl<const N: usize, const LAG: usize> FixedLagSmoother<N, LAG> {
const EMPTY: SmoothingStep<N> = SmoothingStep {
filtered: [0.0; N],
filtered_sqrt: [[0.0; N]; N],
stm: [[0.0; N]; N],
process_noise_sqrt: [[0.0; N]; N],
predicted: [0.0; N],
};
#[must_use]
pub const fn new() -> Self {
Self {
window: [Self::EMPTY; LAG],
next: 0,
depth: 0,
}
}
pub fn push(&mut self, step: SmoothingStep<N>) {
if LAG == 0 {
return;
}
self.window[self.next] = step;
self.next = (self.next + 1) % LAG;
if self.depth < LAG {
self.depth += 1;
}
}
#[must_use]
pub const fn depth(&self) -> usize {
self.depth
}
pub fn smooth(
&self,
filtered: &[f64; N],
filtered_sqrt: &[[f64; N]; N],
) -> Result<([f64; N], [[f64; N]; N]), RtsError> {
let mut smoother = RtsSmoother::new(*filtered, *filtered_sqrt);
for back in 1..=self.depth {
let index = (self.next + LAG - back) % LAG;
smoother.step(&self.window[index])?;
}
Ok((*smoother.state(), smoother.covariance_sqrt()))
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp, clippy::cast_precision_loss)]
use super::*;
fn interval(index: usize) -> SmoothingStep<2> {
let t = index as f64;
SmoothingStep {
filtered: [t, 0.5 * t],
filtered_sqrt: [[1.0 + 0.1 * t, 0.0], [0.2, 0.7]],
stm: [[1.0, 1.0], [0.0, 1.0]],
process_noise_sqrt: [[0.1, 0.0], [0.0, 0.05]],
predicted: [t + 0.5 * t, 0.5 * t],
}
}
#[test]
fn a_zero_lag_smoother_is_the_filter() {
let mut smoother = FixedLagSmoother::<2, 0>::new();
for i in 0..5 {
smoother.push(interval(i));
}
assert_eq!(smoother.depth(), 0);
let seed = ([3.0, -1.0], [[2.0, 0.0], [0.5, 1.0]]);
let (x, s) = smoother.smooth(&seed.0, &seed.1).unwrap();
assert_eq!((x, s), seed);
}
#[test]
fn the_depth_is_the_history_not_the_constant() {
let mut smoother = FixedLagSmoother::<2, 3>::new();
assert_eq!(smoother.depth(), 0);
for (pushed, expected) in [(0, 1), (1, 2), (2, 3), (3, 3), (4, 3)] {
smoother.push(interval(pushed));
assert_eq!(smoother.depth(), expected);
}
}
#[test]
fn the_window_replays_the_newest_intervals_newest_first() {
let mut smoother = FixedLagSmoother::<2, 3>::new();
for i in 0..6 {
smoother.push(interval(i));
}
let seed = ([7.0, 1.5], [[1.5, 0.0], [0.3, 0.9]]);
let (x, s) = smoother.smooth(&seed.0, &seed.1).unwrap();
let mut expected = RtsSmoother::new(seed.0, seed.1);
for i in [5, 4, 3] {
expected.step(&interval(i)).unwrap();
}
assert_eq!(x, *expected.state());
assert_eq!(s, expected.covariance_sqrt());
}
#[test]
fn a_singular_prediction_is_reported_rather_than_smoothed_over() {
let mut degenerate = interval(0);
degenerate.filtered_sqrt = [[0.0; 2]; 2];
degenerate.process_noise_sqrt = [[0.0; 2]; 2];
let mut smoother = FixedLagSmoother::<2, 2>::new();
smoother.push(degenerate);
let error = smoother
.smooth(&[0.0, 0.0], &[[1.0, 0.0], [0.0, 1.0]])
.unwrap_err();
assert!(matches!(error, RtsError::SingularPrediction { .. }));
}
}