#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
#[derive(Debug, Clone)]
pub struct SquareRootKalman<const N: usize> {
pub x: [f64; N],
s: [[f64; N]; N],
}
impl<const N: usize> SquareRootKalman<N> {
#[must_use]
pub const fn new(x0: [f64; N], covariance_sqrt: [[f64; N]; N]) -> Self {
Self {
x: x0,
s: covariance_sqrt,
}
}
#[must_use]
pub fn from_covariance(x0: [f64; N], p0: &[[f64; N]; N]) -> Option<Self> {
let mut l = [[0.0; N]; N];
for i in 0..N {
for j in 0..=i {
let mut sum = p0[i][j];
for k in 0..j {
sum -= l[i][k] * l[j][k];
}
if i == j {
if sum <= 0.0 {
return None;
}
l[i][j] = libm::sqrt(sum);
} else {
l[i][j] = sum / l[j][j];
}
}
}
Some(Self { x: x0, s: l })
}
pub fn time_update(&mut self, stm: &[[f64; N]; N], process_noise_sqrt: &[[f64; N]; N]) {
let mut 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 += stm[i][k] * self.s[k][j];
}
left[i][j] = acc;
}
}
let mut right = *process_noise_sqrt;
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);
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;
}
}
}
}
self.s = left;
}
pub fn measurement_update(
&mut self,
h: &[f64; N],
innovation: f64,
noise_variance: f64,
) -> f64 {
let mut alpha = libm::sqrt(noise_variance);
let mut top = [0.0; N];
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += h[k] * self.s[k][j];
}
top[j] = acc;
}
let mut col = [0.0; N];
let mut block = self.s;
for j in 0..N {
let denom = libm::hypot(alpha, top[j]);
if denom == 0.0 {
continue;
}
let (c, s) = (alpha / denom, top[j] / denom);
alpha = denom;
top[j] = 0.0;
for r in 0..N {
let a0 = col[r];
let aj = block[r][j];
col[r] = c * a0 + s * aj;
block[r][j] = -s * a0 + c * aj;
}
}
for i in 0..N {
for j in (i + 1)..N {
let denom = libm::hypot(block[i][i], block[i][j]);
if denom == 0.0 {
continue;
}
let (c, s) = (block[i][i] / denom, block[i][j] / denom);
for r in 0..N {
let b_i = block[r][i];
let b_j = block[r][j];
block[r][i] = c * b_i + s * b_j;
block[r][j] = -s * b_i + c * b_j;
}
}
}
self.s = block;
if alpha != 0.0 {
for i in 0..N {
self.x[i] += (col[i] / alpha) * innovation;
}
}
alpha * alpha
}
#[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
}
#[must_use]
pub fn covariance_sqrt(&self) -> [[f64; N]; N] {
self.s
}
}