#![allow(clippy::needless_range_loop)]
use ndarray::{Array1, Array2};
use crate::switching::logsumexp;
pub(crate) struct FilterOutput {
pub llf: f64,
pub filtered_marginal: Array2<f64>,
pub predicted_joint_log: Array2<f64>,
pub filtered_joint_log: Array2<f64>,
}
fn n_states(k: usize, order: usize) -> usize {
k.pow((order + 1) as u32)
}
fn digits(mut idx: usize, k: usize, order: usize) -> Vec<usize> {
let mut d = vec![0usize; order + 1];
for pos in (0..=order).rev() {
d[pos] = idx % k;
idx /= k;
}
d
}
pub(crate) fn hamilton_filter(
init: &Array1<f64>,
transition: &Array2<f64>,
cond_loglik: &Array2<f64>,
order: usize,
) -> FilterOutput {
let k = init.len();
let ns = n_states(k, order);
let nobs = cond_loglik.ncols();
let log_init = init.mapv(|v| v.max(1e-20).ln());
let log_p = transition.mapv(|v| v.max(1e-20).ln());
let mut predicted_joint_log = Array2::<f64>::zeros((ns, nobs));
let mut filtered_joint_log = Array2::<f64>::zeros((ns, nobs));
let mut filtered_marginal = Array2::<f64>::zeros((nobs, k));
let mut llf = 0.0;
let mut prev_filtered_log = vec![0.0f64; ns];
for state in 0..ns {
let d = digits(state, k, order);
let mut acc = log_init[d[order]];
for m in 0..order {
acc += log_p[[d[m], d[m + 1]]];
}
prev_filtered_log[state] = acc;
}
let mut predicted_log = vec![0.0f64; ns];
for t in 0..nobs {
if order == 0 {
for i in 0..k {
let mut terms = Vec::with_capacity(k);
for j in 0..k {
terms.push(log_p[[i, j]] + prev_filtered_log[j]);
}
predicted_log[i] = logsumexp(&terms);
}
} else {
for state in 0..ns {
let d = digits(state, k, order);
let mut terms = Vec::with_capacity(k);
for last in 0..k {
let mut prev_idx = 0usize;
for pos in 1..=order {
prev_idx = prev_idx * k + d[pos];
}
prev_idx = prev_idx * k + last;
terms.push(prev_filtered_log[prev_idx]);
}
let marg = logsumexp(&terms);
predicted_log[state] = log_p[[d[0], d[1]]] + marg;
}
}
let mut joint_log = vec![0.0f64; ns];
for state in 0..ns {
joint_log[state] = predicted_log[state] + cond_loglik[[state, t]];
}
let lik_log = logsumexp(&joint_log);
llf += lik_log;
for state in 0..ns {
predicted_joint_log[[state, t]] = predicted_log[state];
let f = joint_log[state] - lik_log;
filtered_joint_log[[state, t]] = f;
}
for i in 0..k {
let mut terms = Vec::with_capacity(ns / k);
for state in 0..ns {
if digits(state, k, order)[0] == i {
terms.push(filtered_joint_log[[state, t]]);
}
}
filtered_marginal[[t, i]] = logsumexp(&terms).exp();
}
for state in 0..ns {
prev_filtered_log[state] = filtered_joint_log[[state, t]];
}
}
FilterOutput {
llf,
filtered_marginal,
predicted_joint_log,
filtered_joint_log,
}
}
pub(crate) fn kim_smoother(
transition: &Array2<f64>,
out: &FilterOutput,
k: usize,
order: usize,
) -> Array2<f64> {
let ns = n_states(k, order);
let nobs = out.filtered_joint_log.ncols();
let log_p = transition.mapv(|v| v.max(1e-20).ln());
let mut smoothed_joint_log = Array2::<f64>::zeros((ns, nobs));
for state in 0..ns {
smoothed_joint_log[[state, nobs - 1]] = out.filtered_joint_log[[state, nobs - 1]];
}
for t in (0..nobs - 1).rev() {
for state in 0..ns {
let d = digits(state, k, order); let mut terms = Vec::with_capacity(k);
for s_next in 0..k {
let mut next_idx = s_next;
for pos in 0..order {
next_idx = next_idx * k + d[pos];
}
let trans = log_p[[s_next, d[0]]];
let num = smoothed_joint_log[[next_idx, t + 1]] + trans;
let den = out.predicted_joint_log[[next_idx, t + 1]];
terms.push(num - den);
}
let ratio = logsumexp(&terms);
smoothed_joint_log[[state, t]] = out.filtered_joint_log[[state, t]] + ratio;
}
}
let mut smoothed_marginal = Array2::<f64>::zeros((nobs, k));
for t in 0..nobs {
for i in 0..k {
let mut terms = Vec::with_capacity(ns / k);
for state in 0..ns {
if digits(state, k, order)[0] == i {
terms.push(smoothed_joint_log[[state, t]]);
}
}
smoothed_marginal[[t, i]] = logsumexp(&terms).exp();
}
}
smoothed_marginal
}
#[cfg(test)]
mod tests {
use super::*;
use crate::switching::{steady_state, transition_matrix};
use ndarray::array;
fn naive_filter(init: &Array1<f64>, p: &Array2<f64>, dens: &Array2<f64>) -> (f64, Array2<f64>) {
let k = init.len();
let nobs = dens.ncols();
let mut filt = Array2::<f64>::zeros((nobs, k));
let mut prev = init.clone();
let mut llf = 0.0;
for t in 0..nobs {
let pred = p.dot(&prev);
let mut joint = vec![0.0; k];
let mut lik = 0.0;
for i in 0..k {
joint[i] = pred[i] * dens[[i, t]];
lik += joint[i];
}
llf += lik.ln();
for i in 0..k {
filt[[t, i]] = joint[i] / lik;
prev[i] = filt[[t, i]];
}
}
(llf, filt)
}
#[test]
fn order0_filter_matches_naive() {
let p = transition_matrix(&[0.9, 0.2], 2); let init = steady_state(&p).unwrap();
let dens = array![[0.30, 0.10, 0.25, 0.40], [0.05, 0.35, 0.20, 0.02]];
let cll = dens.mapv(f64::ln);
let out = hamilton_filter(&init, &p, &cll, 0);
let (llf_naive, filt_naive) = naive_filter(&init, &p, &dens);
assert!(
(out.llf - llf_naive).abs() < 1e-12,
"{} vs {}",
out.llf,
llf_naive
);
for t in 0..4 {
for i in 0..2 {
assert!((out.filtered_marginal[[t, i]] - filt_naive[[t, i]]).abs() < 1e-12);
}
}
}
#[test]
fn filtered_and_smoothed_are_distributions() {
let p = transition_matrix(&[0.85, 0.15], 2);
let init = steady_state(&p).unwrap();
let dens = array![[0.4, 0.1, 0.3], [0.1, 0.5, 0.2]];
let cll = dens.mapv(f64::ln);
let out = hamilton_filter(&init, &p, &cll, 0);
let smoothed = kim_smoother(&p, &out, 2, 0);
for t in 0..3 {
let fs: f64 = (0..2).map(|i| out.filtered_marginal[[t, i]]).sum();
let ss: f64 = (0..2).map(|i| smoothed[[t, i]]).sum();
assert!((fs - 1.0).abs() < 1e-12, "filtered sum {fs}");
assert!((ss - 1.0).abs() < 1e-12, "smoothed sum {ss}");
}
for i in 0..2 {
assert!((smoothed[[2, i]] - out.filtered_marginal[[2, i]]).abs() < 1e-12);
}
}
#[test]
fn order1_filter_sums_to_one() {
let p = transition_matrix(&[0.8, 0.3], 2);
let init = steady_state(&p).unwrap();
let cll = Array2::from_shape_vec(
(4, 3),
vec![
-1.0, -1.2, -0.9, -2.0, -1.5, -1.1, -1.3, -1.0, -1.4, -0.8, -1.1, -1.6,
],
)
.unwrap();
let out = hamilton_filter(&init, &p, &cll, 1);
for t in 0..3 {
let s: f64 = (0..2).map(|i| out.filtered_marginal[[t, i]]).sum();
assert!((s - 1.0).abs() < 1e-12, "order-1 filtered sum {s}");
}
let smoothed = kim_smoother(&p, &out, 2, 1);
for t in 0..3 {
let s: f64 = (0..2).map(|i| smoothed[[t, i]]).sum();
assert!((s - 1.0).abs() < 1e-12, "order-1 smoothed sum {s}");
}
}
}