#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
#[derive(Debug, Clone)]
pub struct Srif<const N: usize> {
r: [[f64; N]; N],
d: [f64; N],
residual_ss: f64,
rows: u32,
}
impl<const N: usize> Default for Srif<N> {
fn default() -> Self {
Self::new()
}
}
impl<const N: usize> Srif<N> {
#[must_use]
pub const fn new() -> Self {
Self {
r: [[0.0; N]; N],
d: [0.0; N],
residual_ss: 0.0,
rows: 0,
}
}
#[must_use]
pub fn with_prior(x0: &[f64; N], r0: &[[f64; N]; N]) -> Self {
let mut d = [0.0; N];
for i in 0..N {
let mut acc = 0.0;
for j in i..N {
acc += r0[i][j] * x0[j];
}
d[i] = acc;
}
Self {
r: *r0,
d,
residual_ss: 0.0,
rows: 0,
}
}
pub fn add_row(&mut self, h: &[f64; N], z: f64, weight: f64) {
let w = libm::sqrt(weight);
let mut a = *h;
for v in &mut a {
*v *= w;
}
let mut zz = z * w;
for k in 0..N {
if a[k] == 0.0 {
continue;
}
let rkk = self.r[k][k];
let denom = libm::hypot(rkk, a[k]);
if denom == 0.0 {
continue;
}
let c = rkk / denom;
let s = a[k] / denom;
self.r[k][k] = denom;
for j in (k + 1)..N {
let temp = c * self.r[k][j] + s * a[j];
a[j] = c * a[j] - s * self.r[k][j];
self.r[k][j] = temp;
}
let temp = c * self.d[k] + s * zz;
zz = c * zz - s * self.d[k];
self.d[k] = temp;
}
self.residual_ss += zz * zz;
self.rows += 1;
}
pub fn solve(&self) -> Result<[f64; N], SrifError> {
let mut x = [0.0; N];
for i in (0..N).rev() {
if self.r[i][i] == 0.0 {
return Err(SrifError::RankDeficient { direction: i });
}
let mut acc = self.d[i];
for j in (i + 1)..N {
acc -= self.r[i][j] * x[j];
}
x[i] = acc / self.r[i][i];
}
Ok(x)
}
pub fn covariance(&self) -> Result<[[f64; N]; N], SrifError> {
let mut rinv = [[0.0; N]; N];
for col in 0..N {
for i in (0..N).rev() {
if self.r[i][i] == 0.0 {
return Err(SrifError::RankDeficient { direction: i });
}
let mut acc = f64::from(u8::from(i == col));
for j in (i + 1)..N {
acc -= self.r[i][j] * rinv[j][col];
}
rinv[i][col] = acc / self.r[i][i];
}
}
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 += rinv[i][k] * rinv[j][k];
}
p[i][j] = acc;
}
}
Ok(p)
}
#[must_use]
pub fn information_sqrt(&self) -> [[f64; N]; N] {
self.r
}
#[must_use]
pub fn residual_stats(&self) -> (f64, u32) {
(self.residual_ss, self.rows)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum SrifError {
RankDeficient {
direction: usize,
},
}
impl core::fmt::Display for SrifError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::RankDeficient { direction } => {
write!(
f,
"state direction {direction} is unobserved (singular information matrix)"
)
}
}
}
}
impl core::error::Error for SrifError {}