#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
use crate::srukf::unscented_predict;
use crate::unscented::UnscentedParams;
#[derive(Debug, Clone, Copy)]
pub struct GaussianComponent<const N: usize> {
pub weight: f64,
pub mean: [f64; N],
pub sqrt_cov: [[f64; N]; N],
}
#[derive(Debug, Clone)]
pub struct GaussianMixture<const N: usize, const C: usize, const L: usize> {
pub components: [GaussianComponent<N>; C],
params: UnscentedParams,
}
impl<const N: usize, const C: usize, const L: usize> GaussianMixture<N, C, L> {
#[must_use]
pub const fn new(components: [GaussianComponent<N>; C], params: UnscentedParams) -> Self {
assert!(L == 2 * N + 1, "sigma-point count L must equal 2N + 1");
Self { components, params }
}
pub fn normalize(&mut self) {
let total: f64 = self.components.iter().map(|c| c.weight).sum();
if total == 0.0 {
return;
}
for c in &mut self.components {
c.weight /= total;
}
}
pub fn propagate(
&mut self,
dynamics: impl Fn(&[f64; N]) -> [f64; N],
process_noise_sqrt: &[[f64; N]; N],
) {
for c in &mut self.components {
let (mean, s) = unscented_predict::<N, L>(
&self.params,
&c.mean,
&c.sqrt_cov,
&dynamics,
process_noise_sqrt,
);
c.mean = mean;
c.sqrt_cov = s;
}
}
#[must_use]
pub fn collapse(&self) -> ([f64; N], [[f64; N]; N]) {
let mut mean = [0.0; N];
for c in &self.components {
for i in 0..N {
mean[i] += c.weight * c.mean[i];
}
}
let mut cov = [[0.0; N]; N];
for c in &self.components {
for i in 0..N {
for j in 0..N {
let mut p_ij = 0.0;
for k in 0..N {
p_ij += c.sqrt_cov[i][k] * c.sqrt_cov[j][k];
}
let spread = (c.mean[i] - mean[i]) * (c.mean[j] - mean[j]);
cov[i][j] += c.weight * (p_ij + spread);
}
}
}
(mean, cov)
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp)]
use super::*;
use crate::unscented::cholesky;
fn component<const N: usize>(
weight: f64,
mean: [f64; N],
p: &[[f64; N]; N],
) -> GaussianComponent<N> {
GaussianComponent {
weight,
mean,
sqrt_cov: cholesky::<N>(p).unwrap(),
}
}
#[test]
fn normalizing_divides_by_the_total_and_leaves_an_all_zero_mixture_alone() {
let p = [[1.0, 0.0], [0.0, 1.0]];
let mut mix = GaussianMixture::<2, 3, 5>::new(
[
component(3.0, [1.0, 0.0], &p),
component(1.0, [0.0, 1.0], &p),
component(4.0, [-1.0, 2.0], &p),
],
UnscentedParams::gaussian(),
);
mix.normalize();
let got: [f64; 3] = core::array::from_fn(|c| mix.components[c].weight);
assert_eq!(got, [0.375, 0.125, 0.5], "weights 3,1,4 over a total of 8");
assert_eq!(
got.iter().sum::<f64>(),
1.0,
"the invariant collapse relies on"
);
let mut zeroed = GaussianMixture::<2, 2, 5>::new(
[
component(0.0, [1.0, 0.0], &p),
component(0.0, [0.0, 1.0], &p),
],
UnscentedParams::gaussian(),
);
zeroed.normalize();
for c in 0..2 {
assert_eq!(zeroed.components[c].weight, 0.0, "component {c}");
}
}
#[test]
fn collapse_matches_the_total_covariance() {
let p0 = [[0.4, 0.05], [0.05, 0.3]];
let p1 = [[0.2, -0.02], [-0.02, 0.5]];
let mix = GaussianMixture::<2, 2, 5>::new(
[
component(0.6, [1.0, -1.0], &p0),
component(0.4, [-0.5, 2.0], &p1),
],
UnscentedParams::gaussian(),
);
let (mean, cov) = mix.collapse();
let means = [[1.0, -1.0], [-0.5, 2.0]];
let ps = [p0, p1];
let ws = [0.6, 0.4];
let mut mean_ref = [0.0; 2];
for c in 0..2 {
for i in 0..2 {
mean_ref[i] += ws[c] * means[c][i];
}
}
let mut cov_ref = [[0.0; 2]; 2];
for c in 0..2 {
for i in 0..2 {
for j in 0..2 {
let spread = (means[c][i] - mean_ref[i]) * (means[c][j] - mean_ref[j]);
cov_ref[i][j] += ws[c] * (ps[c][i][j] + spread);
}
}
}
for i in 0..2 {
assert!((mean[i] - mean_ref[i]).abs() < 1e-12, "mean[{i}]");
for j in 0..2 {
assert!((cov[i][j] - cov_ref[i][j]).abs() < 1e-12, "cov[{i}][{j}]");
}
}
}
#[test]
fn linear_propagation_commutes_with_collapse() {
let a = [[1.0, 0.1], [-0.05, 0.98]];
let sq = [[0.05, 0.0], [0.01, 0.04]];
let p0 = [[0.4, 0.05], [0.05, 0.3]];
let p1 = [[0.2, -0.02], [-0.02, 0.5]];
let params = UnscentedParams::new(1.0, 2.0, 0.0);
let mut mix = GaussianMixture::<2, 2, 5>::new(
[
component(0.6, [1.0, -1.0], &p0),
component(0.4, [-0.5, 2.0], &p1),
],
params,
);
let (mean_before, cov_before) = mix.collapse();
mix.propagate(
|x| {
[
a[0][0] * x[0] + a[0][1] * x[1],
a[1][0] * x[0] + a[1][1] * x[1],
]
},
&sq,
);
let (mean_after, cov_after) = mix.collapse();
let mut mean_ref = [0.0; 2];
for i in 0..2 {
for j in 0..2 {
mean_ref[i] += a[i][j] * mean_before[j];
}
}
let mut ap = [[0.0; 2]; 2];
for i in 0..2 {
for j in 0..2 {
for k in 0..2 {
ap[i][j] += a[i][k] * cov_before[k][j];
}
}
}
let mut cov_ref = [[0.0; 2]; 2];
for i in 0..2 {
for j in 0..2 {
for k in 0..2 {
cov_ref[i][j] += ap[i][k] * a[j][k];
}
cov_ref[i][j] += sq[i][0] * sq[j][0] + sq[i][1] * sq[j][1];
}
}
for i in 0..2 {
assert!((mean_after[i] - mean_ref[i]).abs() < 1e-10, "mean[{i}]");
for j in 0..2 {
assert!(
(cov_after[i][j] - cov_ref[i][j]).abs() < 1e-10,
"cov[{i}][{j}]"
);
}
}
}
}