#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
use crate::srukf::SquareRootUnscentedKalman;
use crate::unscented::UnscentedParams;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ImmError {
TransitionRowNotStochastic {
row: usize,
},
UnreachableModel {
column: usize,
},
ProbabilitiesNotNormalized,
SingularPriorCovariance {
model: usize,
},
SingularMixedCovariance {
model: usize,
},
}
impl core::fmt::Display for ImmError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::TransitionRowNotStochastic { row } => {
write!(f, "transition matrix row {row} is not a distribution")
}
Self::UnreachableModel { column } => {
write!(f, "no transition enters model {column}")
}
Self::ProbabilitiesNotNormalized => {
write!(f, "model probabilities do not sum to one")
}
Self::SingularPriorCovariance { model } => {
write!(
f,
"prior covariance of model {model} is not positive-definite"
)
}
Self::SingularMixedCovariance { model } => {
write!(f, "mixed prior covariance of model {model} is singular")
}
}
}
}
impl core::error::Error for ImmError {}
#[derive(Debug, Clone)]
pub struct InteractingMultipleModel<const N: usize, const M: usize, const L: usize> {
estimate: [[f64; N]; M],
sqrt_cov: [[[f64; N]; N]; M],
probability: [f64; M],
transition: [[f64; M]; M],
params: UnscentedParams,
}
impl<const N: usize, const M: usize, const L: usize> InteractingMultipleModel<N, M, L> {
pub fn new(
estimate: [f64; N],
covariance: &[[f64; N]; N],
probability: [f64; M],
transition: [[f64; M]; M],
params: UnscentedParams,
) -> Result<Self, ImmError> {
assert!(L == 2 * N + 1, "sigma-point count L must equal 2N + 1");
validate_distribution(&probability).map_err(|()| ImmError::ProbabilitiesNotNormalized)?;
for i in 0..M {
validate_distribution(&transition[i])
.map_err(|()| ImmError::TransitionRowNotStochastic { row: i })?;
}
for j in 0..M {
if (0..M).all(|i| transition[i][j] == 0.0) {
return Err(ImmError::UnreachableModel { column: j });
}
}
let seed = SquareRootUnscentedKalman::<N, L>::from_covariance(estimate, covariance, params)
.ok_or(ImmError::SingularPriorCovariance { model: 0 })?;
Ok(Self {
estimate: [estimate; M],
sqrt_cov: [seed.covariance_sqrt(); M],
probability,
transition,
params,
})
}
pub fn step<const D: usize>(
&mut self,
dynamics: impl Fn(usize, &[f64; N]) -> [f64; N],
process_noise_sqrt: &[[[f64; N]; N]; M],
measurement: impl Fn(&[f64; N]) -> [f64; D],
z: &[f64; D],
noise_sqrt: &[[f64; D]; D],
) -> Result<[f64; M], ImmError> {
let predicted = self.predicted_probabilities();
let mut log_likelihood = [0.0; M];
for j in 0..M {
let (mixed_estimate, mixed_covariance) = self.mix_into(j, &predicted);
let mut filter = SquareRootUnscentedKalman::<N, L>::from_covariance(
mixed_estimate,
&mixed_covariance,
self.params,
)
.ok_or(ImmError::SingularMixedCovariance { model: j })?;
filter.predict(|x| dynamics(j, x), &process_noise_sqrt[j]);
log_likelihood[j] = filter.update_with_log_likelihood(&measurement, z, noise_sqrt);
self.estimate[j] = filter.x;
self.sqrt_cov[j] = filter.covariance_sqrt();
}
self.reweigh(&predicted, &log_likelihood);
Ok(self.probability)
}
#[must_use]
pub const fn probabilities(&self) -> [f64; M] {
self.probability
}
#[must_use]
pub fn most_likely(&self) -> usize {
let mut best = 0;
for j in 1..M {
if self.probability[j] > self.probability[best] {
best = j;
}
}
best
}
#[must_use]
pub const fn model(&self, index: usize) -> ([f64; N], [[f64; N]; N]) {
(self.estimate[index], self.sqrt_cov[index])
}
#[must_use]
pub fn combined(&self) -> ([f64; N], [[f64; N]; N]) {
let mut mean = [0.0; N];
for j in 0..M {
for i in 0..N {
mean[i] += self.probability[j] * self.estimate[j][i];
}
}
let mut covariance = [[0.0; N]; N];
for j in 0..M {
for a in 0..N {
for b in 0..N {
let mut p_ab = 0.0;
for k in 0..N {
p_ab += self.sqrt_cov[j][a][k] * self.sqrt_cov[j][b][k];
}
let spread = (self.estimate[j][a] - mean[a]) * (self.estimate[j][b] - mean[b]);
covariance[a][b] += self.probability[j] * (p_ab + spread);
}
}
}
(mean, covariance)
}
fn predicted_probabilities(&self) -> [f64; M] {
let mut predicted = [0.0; M];
for j in 0..M {
for i in 0..M {
predicted[j] += self.transition[i][j] * self.probability[i];
}
}
predicted
}
fn mix_into(&self, j: usize, predicted: &[f64; M]) -> ([f64; N], [[f64; N]; N]) {
let mut weight = [0.0; M];
for i in 0..M {
weight[i] = self.transition[i][j] * self.probability[i] / predicted[j];
}
let mut mean = [0.0; N];
for i in 0..M {
for a in 0..N {
mean[a] += weight[i] * self.estimate[i][a];
}
}
let mut covariance = [[0.0; N]; N];
for i in 0..M {
for a in 0..N {
for b in 0..N {
let mut p_ab = 0.0;
for k in 0..N {
p_ab += self.sqrt_cov[i][a][k] * self.sqrt_cov[i][b][k];
}
let spread = (self.estimate[i][a] - mean[a]) * (self.estimate[i][b] - mean[b]);
covariance[a][b] += weight[i] * (p_ab + spread);
}
}
}
(mean, covariance)
}
fn reweigh(&mut self, predicted: &[f64; M], log_likelihood: &[f64; M]) {
let mut best = log_likelihood[0];
for j in 1..M {
if log_likelihood[j] > best {
best = log_likelihood[j];
}
}
let mut unnormalized = [0.0; M];
let mut total = 0.0;
for j in 0..M {
let ratio = libm::exp(log_likelihood[j] - best).max(f64::MIN_POSITIVE);
unnormalized[j] = predicted[j] * ratio;
total += unnormalized[j];
}
for j in 0..M {
self.probability[j] = unnormalized[j] / total;
}
}
}
fn validate_distribution<const M: usize>(p: &[f64; M]) -> Result<(), ()> {
let mut total = 0.0;
for value in p {
if !value.is_finite() || *value < 0.0 {
return Err(());
}
total += value;
}
#[allow(clippy::cast_precision_loss)] let tolerance = 8.0 * f64::EPSILON * M as f64;
if libm::fabs(total - 1.0) > tolerance {
return Err(());
}
Ok(())
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp)]
use super::*;
fn dynamics(_model: usize, x: &[f64; 2]) -> [f64; 2] {
[x[0] + x[1], x[1]]
}
fn measure(x: &[f64; 2]) -> [f64; 1] {
[x[0]]
}
const Q: [[f64; 2]; 2] = [[0.02, 0.0], [0.0, 0.05]];
const P0: [[f64; 2]; 2] = [[1.0, 0.0], [0.0, 0.5]];
const R: [[f64; 1]; 1] = [[0.3]];
fn observations(count: usize) -> Vec<f64> {
let (mut truth, mut seed) = (0.0_f64, 12_345_u64);
(0..count)
.map(|_| {
truth += 1.0;
seed = seed.wrapping_mul(6_364_136_223_846_793_005).wrapping_add(1);
#[allow(clippy::cast_precision_loss)]
let jitter = ((seed >> 33) as f64 / f64::from(1_u32 << 31) - 0.5) * 0.6;
truth + jitter
})
.collect()
}
#[test]
fn a_bank_of_one_model_is_the_filter_it_wraps() {
let params = UnscentedParams::gaussian();
let mut bank =
InteractingMultipleModel::<2, 1, 5>::new([0.0, 0.0], &P0, [1.0], [[1.0]], params)
.unwrap();
let mut filter =
SquareRootUnscentedKalman::<2, 5>::from_covariance([0.0, 0.0], &P0, params).unwrap();
let mut worst = 0.0_f64;
for z in observations(15) {
let probabilities = bank.step(dynamics, &[Q], measure, &[z], &R).unwrap();
assert_eq!(
probabilities,
[1.0],
"a single model owns all the probability"
);
filter.predict(|x| dynamics(0, x), &Q);
filter.update(measure, &[z], &R);
let (mean, _) = bank.combined();
for i in 0..2 {
worst = worst.max((mean[i] - filter.x[i]).abs());
}
}
println!("IMM(M=1) vs SR-UKF: worst |dx| {worst:.3e}");
assert!(
worst < 1e-12,
"the bank departed from the filter: {worst:.3e}"
);
}
#[test]
fn the_model_that_explains_the_data_takes_the_probability() {
let params = UnscentedParams::gaussian();
let quiet = Q;
let loud = [[2.0, 0.0], [0.0, 2.0]];
let mut bank = InteractingMultipleModel::<2, 2, 5>::new(
[0.0, 0.0],
&P0,
[0.5, 0.5],
[[0.95, 0.05], [0.05, 0.95]],
params,
)
.unwrap();
let mut probabilities = [0.5, 0.5];
for z in observations(30) {
probabilities = bank
.step(dynamics, &[quiet, loud], measure, &[z], &R)
.unwrap();
let total: f64 = probabilities.iter().sum();
assert!(
(total - 1.0).abs() < 1e-12,
"probabilities must stay a distribution"
);
}
println!("quiet/loud posterior: {probabilities:?}");
assert!(
probabilities[0] > 0.5,
"the model matching the data must win: {probabilities:?}"
);
assert_eq!(bank.most_likely(), 0);
}
#[test]
fn an_impossible_observation_does_not_zero_a_model_forever() {
let params = UnscentedParams::gaussian();
let loud = [[1e6, 0.0], [0.0, 1e6]];
let mut bank = InteractingMultipleModel::<2, 2, 5>::new(
[0.0, 1.0],
&P0,
[0.5, 0.5],
[[0.99, 0.01], [0.01, 0.99]],
params,
)
.unwrap();
bank.step(dynamics, &[Q, loud], measure, &[1e6], &R)
.unwrap();
let after_shock = bank.probabilities();
println!("after the shock: {after_shock:?}");
assert!(
after_shock[0] > 0.0 && after_shock[0].is_finite(),
"the quiet model must survive the shock as a positive probability: {after_shock:?}"
);
assert!(
after_shock[1] > 0.99,
"the loud model must take the shock: {after_shock:?}"
);
let mut epochs = 0;
let mut truth = 1e6;
while bank.probabilities()[0] <= 0.5 {
truth += 1.0;
bank.step(dynamics, &[Q, loud], measure, &[truth], &R)
.unwrap();
epochs += 1;
assert!(
epochs < 50,
"the quiet model never recovered: {:?}",
bank.probabilities()
);
}
println!("quiet model recovered after {epochs} epochs");
}
#[test]
fn the_combined_covariance_carries_the_spread_of_the_means() {
let params = UnscentedParams::gaussian();
let ambiguous = [[10.0; 1]; 1];
let split = |model: usize, x: &[f64; 2]| {
let push = if model == 0 { -10.0 } else { 10.0 };
[x[0] + x[1] + push, x[1]]
};
let mut bank = InteractingMultipleModel::<2, 2, 5>::new(
[0.0, 1.0],
&P0,
[0.5, 0.5],
[[0.99, 0.01], [0.01, 0.99]],
params,
)
.unwrap();
for z in [1.0, 2.0, 3.0] {
bank.step(split, &[Q, Q], measure, &[z], &ambiguous)
.unwrap();
}
let probabilities = bank.probabilities();
let (_, combined) = bank.combined();
let (mean0, s0) = bank.model(0);
let (mean1, _) = bank.model(1);
let within: f64 = (0..2).map(|k| s0[0][k] * s0[0][k]).sum();
let separation = (mean0[0] - mean1[0]).abs();
println!(
"probabilities {probabilities:?}, combined P00 {:.4}, within-model P00 {within:.4}, separation {separation:.4}",
combined[0][0]
);
assert!(
(0.3..0.7).contains(&probabilities[0]),
"the bank must stay undecided for this to measure anything: {probabilities:?}"
);
assert!(
separation > 10.0,
"the fixture must actually split the models"
);
assert!(
combined[0][0] > 10.0 * within,
"the spread of means is missing from the combined covariance: {:.4} vs {within:.4}",
combined[0][0]
);
}
#[test]
fn a_bank_that_cannot_be_a_bank_is_refused() {
let params = UnscentedParams::gaussian();
let build = |probability: [f64; 2], transition: [[f64; 2]; 2]| {
InteractingMultipleModel::<2, 2, 5>::new(
[0.0, 0.0],
&P0,
probability,
transition,
params,
)
};
let good = [[0.9, 0.1], [0.1, 0.9]];
assert_eq!(
build([0.5, 0.6], good).err(),
Some(ImmError::ProbabilitiesNotNormalized)
);
assert_eq!(
build([0.5, 0.5], [[0.9, 0.1], [0.2, 0.9]]).err(),
Some(ImmError::TransitionRowNotStochastic { row: 1 })
);
assert_eq!(
build([0.5, 0.5], [[0.9, 0.1], [f64::NAN, 0.1]]).err(),
Some(ImmError::TransitionRowNotStochastic { row: 1 })
);
assert_eq!(
build([0.5, 0.5], [[1.0, 0.0], [1.0, 0.0]]).err(),
Some(ImmError::UnreachableModel { column: 1 })
);
assert_eq!(
InteractingMultipleModel::<2, 2, 5>::new(
[0.0, 0.0],
&[[1.0, 0.0], [0.0, 0.0]],
[0.5, 0.5],
good,
params
)
.err(),
Some(ImmError::SingularPriorCovariance { model: 0 })
);
}
#[test]
fn every_refusal_names_the_row_column_or_model() {
let messages = [
(
ImmError::TransitionRowNotStochastic { row: 2 },
"transition matrix row 2 is not a distribution",
),
(
ImmError::UnreachableModel { column: 1 },
"no transition enters model 1",
),
(
ImmError::ProbabilitiesNotNormalized,
"model probabilities do not sum to one",
),
(
ImmError::SingularPriorCovariance { model: 3 },
"prior covariance of model 3 is not positive-definite",
),
(
ImmError::SingularMixedCovariance { model: 0 },
"mixed prior covariance of model 0 is singular",
),
];
for (error, expected) in messages {
assert_eq!(format!("{error}"), expected);
let dynamic: &dyn core::error::Error = &error;
assert_eq!(format!("{dynamic}"), expected);
}
}
#[test]
fn the_most_likely_model_is_the_argmax_and_a_tie_goes_to_the_lower_index() {
let params = UnscentedParams::gaussian();
let mut bank = InteractingMultipleModel::<2, 3, 5>::new(
[0.0, 0.0],
&P0,
[0.2, 0.5, 0.3],
[[0.8, 0.1, 0.1], [0.1, 0.8, 0.1], [0.1, 0.1, 0.8]],
params,
)
.unwrap();
assert_eq!(bank.most_likely(), 1);
bank.probability = [0.3, 0.2, 0.5];
assert_eq!(bank.most_likely(), 2);
bank.probability = [0.5, 0.5, 0.0];
assert_eq!(bank.most_likely(), 0, "a tie must go to the lower index");
}
#[test]
fn the_combination_and_the_mixing_are_the_same_moment_match() {
let params = UnscentedParams::gaussian();
let mut bank = InteractingMultipleModel::<2, 2, 5>::new(
[0.0, 0.0],
&P0,
[0.5, 0.5],
[[0.5, 0.5], [0.5, 0.5]],
params,
)
.unwrap();
bank.probability = [0.25, 0.75];
bank.estimate = [[1.0, 2.0], [5.0, 6.0]];
bank.sqrt_cov = [[[2.0, 0.0], [0.5, 1.0]], [[1.0, 0.0], [0.0, 3.0]]];
let expected_mean = [4.0, 5.0];
let expected_covariance = [[4.75, 3.25], [3.25, 10.0625]];
let (mean, covariance) = bank.combined();
assert_eq!(mean, expected_mean);
assert_eq!(covariance, expected_covariance);
let predicted = bank.predicted_probabilities();
assert_eq!(predicted, [0.5, 0.5]);
let (mixed_mean, mixed_covariance) = bank.mix_into(0, &predicted);
assert_eq!(mixed_mean, expected_mean);
assert_eq!(mixed_covariance, expected_covariance);
}
#[test]
fn the_tolerance_on_a_distribution_scales_with_its_length_and_includes_its_bound() {
assert!(validate_distribution(&[0.5, 0.5 + 16.0 * f64::EPSILON]).is_ok());
assert!(validate_distribution(&[0.5, 0.25, 0.25 + 2e-15]).is_ok());
assert!(validate_distribution(&[0.5, 0.25, 0.25 + 1e-13]).is_err());
}
}