#![allow(
clippy::many_single_char_names,
clippy::needless_range_loop,
clippy::cast_precision_loss
)]
use crate::sampling::Rng;
use crate::unscented::cholesky;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum EnsembleError {
InnovationCovarianceNotPositiveDefinite,
}
#[derive(Debug, Clone)]
pub struct EnsembleKalman<const N: usize, const M: usize> {
members: [[f64; N]; M],
}
impl<const N: usize, const M: usize> EnsembleKalman<N, M> {
#[must_use]
pub const fn from_members(members: [[f64; N]; M]) -> Self {
const { assert!(M >= 2, "an ensemble needs at least two members") };
Self { members }
}
#[must_use]
pub fn from_mean_and_covariance_sqrt(
mean: &[f64; N],
covariance_sqrt: &[[f64; N]; N],
rng: &mut Rng,
) -> Self {
const { assert!(M >= 2, "an ensemble needs at least two members") };
let mut members = [[0.0; N]; M];
for member in &mut members {
let draw = rng.correlated_normal(covariance_sqrt);
for i in 0..N {
member[i] = mean[i] + draw[i];
}
}
Self { members }
}
#[must_use]
pub const fn members(&self) -> &[[f64; N]; M] {
&self.members
}
#[must_use]
pub fn mean(&self) -> [f64; N] {
let mut mean = [0.0; N];
for member in &self.members {
for i in 0..N {
mean[i] += member[i];
}
}
for value in &mut mean {
*value /= M as f64;
}
mean
}
#[must_use]
pub fn covariance(&self) -> [[f64; N]; N] {
let mean = self.mean();
let mut cov = [[0.0; N]; N];
for member in &self.members {
for i in 0..N {
let di = member[i] - mean[i];
for j in 0..N {
cov[i][j] += di * (member[j] - mean[j]);
}
}
}
let scale = 1.0 / (M as f64 - 1.0);
for row in &mut cov {
for value in row {
*value *= scale;
}
}
cov
}
pub fn predict(
&mut self,
dynamics: impl Fn(&[f64; N]) -> [f64; N],
process_noise_sqrt: &[[f64; N]; N],
rng: &mut Rng,
) {
for member in &mut self.members {
let propagated = dynamics(member);
let noise = rng.correlated_normal(process_noise_sqrt);
for i in 0..N {
member[i] = propagated[i] + noise[i];
}
}
}
pub fn update<const D: usize>(
&mut self,
observation: &[f64; D],
measurement: impl Fn(&[f64; N]) -> [f64; D],
measurement_noise_sqrt: &[[f64; D]; D],
rng: &mut Rng,
) -> Result<(), EnsembleError> {
let mut forecast = [[0.0; D]; M];
let mut y_mean = [0.0; D];
for (slot, member) in forecast.iter_mut().zip(&self.members) {
*slot = measurement(member);
for k in 0..D {
y_mean[k] += slot[k];
}
}
for value in &mut y_mean {
*value /= M as f64;
}
let x_mean = self.mean();
let scale = 1.0 / (M as f64 - 1.0);
let mut pxy = [[0.0; D]; N];
let mut pyy = [[0.0; D]; D];
for (member, y) in self.members.iter().zip(&forecast) {
for k in 0..D {
let dk = y[k] - y_mean[k];
for i in 0..N {
pxy[i][k] += (member[i] - x_mean[i]) * dk;
}
for l in 0..D {
pyy[k][l] += dk * (y[l] - y_mean[l]);
}
}
}
for i in 0..N {
for k in 0..D {
pxy[i][k] *= scale;
}
}
for k in 0..D {
for l in 0..D {
pyy[k][l] *= scale;
let mut r = 0.0;
for m in 0..D {
r += measurement_noise_sqrt[k][m] * measurement_noise_sqrt[l][m];
}
pyy[k][l] += r;
}
}
let factor =
cholesky(&pyy).ok_or(EnsembleError::InnovationCovarianceNotPositiveDefinite)?;
for (member, y) in self.members.iter_mut().zip(&forecast) {
let perturbation = rng.correlated_normal(measurement_noise_sqrt);
let mut innovation = [0.0; D];
for k in 0..D {
innovation[k] = observation[k] + perturbation[k] - y[k];
}
let weighted = solve_spd(&factor, &innovation);
for i in 0..N {
let mut delta = 0.0;
for k in 0..D {
delta += pxy[i][k] * weighted[k];
}
member[i] += delta;
}
}
Ok(())
}
}
fn solve_spd<const D: usize>(factor: &[[f64; D]; D], b: &[f64; D]) -> [f64; D] {
let mut y = [0.0; D];
for i in 0..D {
let mut sum = b[i];
for k in 0..i {
sum -= factor[i][k] * y[k];
}
y[i] = sum / factor[i][i];
}
let mut u = [0.0; D];
for i in (0..D).rev() {
let mut sum = y[i];
for k in (i + 1)..D {
sum -= factor[k][i] * u[k];
}
u[i] = sum / factor[i][i];
}
u
}
#[cfg(test)]
mod tests {
#![allow(clippy::float_cmp, clippy::expect_used)]
use super::*;
#[test]
fn the_triangular_solve_inverts_the_factorisation() {
let a = [[4.0, 2.0, 0.6], [2.0, 5.0, 1.0], [0.6, 1.0, 3.0]];
let l = cholesky(&a).expect("positive definite");
let want = [1.5, -2.0, 0.25];
let mut b = [0.0; 3];
for i in 0..3 {
for j in 0..3 {
b[i] += a[i][j] * want[j];
}
}
let got = solve_spd(&l, &b);
for i in 0..3 {
assert!((got[i] - want[i]).abs() < 1e-12, "component {i}: {got:?}");
}
}
#[test]
fn a_singular_innovation_covariance_is_reported_and_changes_nothing() {
let members = [[1.0, 0.0], [1.0, 1.0], [1.0, 2.0], [1.0, 3.0]];
let mut filter = EnsembleKalman::<2, 4>::from_members(members);
let mut rng = Rng::new(5);
let outcome = filter.update(&[1.0], |x| [x[0]], &[[0.0]], &mut rng);
assert_eq!(
outcome,
Err(EnsembleError::InnovationCovarianceNotPositiveDefinite)
);
assert_eq!(
*filter.members(),
members,
"a failed update must not mutate"
);
}
#[test]
fn the_analysis_gain_is_the_one_the_sample_moments_imply() {
let members = [[1.0, 0.5], [2.0, -0.5], [3.0, 1.5], [6.0, 2.5]];
let want = [56.0 / 59.0, 28.0 / 59.0];
let r_sqrt = [[0.5]];
let shifted = |observation: f64| {
let mut filter = EnsembleKalman::<2, 4>::from_members(members);
let mut rng = Rng::new(2024);
filter
.update(&[observation], |x| [x[0]], &r_sqrt, &mut rng)
.expect("R is positive definite");
filter.mean()
};
let high = shifted(10.0);
let low = shifted(4.0);
for i in 0..2 {
let response = (high[i] - low[i]) / 6.0;
assert!(
(response - want[i]).abs() < 1e-12,
"component {i}: gain {response}, want {}",
want[i]
);
}
}
#[test]
fn the_moments_are_the_sample_moments() {
let filter = EnsembleKalman::<1, 4>::from_members([[1.0], [2.0], [3.0], [6.0]]);
assert_eq!(filter.mean()[0], 3.0);
assert!((filter.covariance()[0][0] - 14.0 / 3.0).abs() < 1e-12);
}
}