Skip to main content

qualia_core_db/solvers/learning/sequential/
kalman.rs

1//! Kalman filter (PRML ch 13.3) — exact inference for a linear-Gaussian state-space
2//! model. Recursively estimates the hidden state `x` and its covariance `P` from
3//! noisy linear observations. The matrix products reuse `linear_algebra::gemm` and
4//! the innovation-covariance inverse reuses `linear_algebra::cholesky` (no new
5//! solver). Kernel-class `DenseLinear`.
6//!
7//! Model: `xₜ = F xₜ₋₁ + w` (`w ~ N(0, Q)`), `zₜ = H xₜ + v` (`v ~ N(0, R)`).
8//! Predict: `x ← Fx`, `P ← FPFᵀ + Q`.
9//! Update with `z`: `S = HPHᵀ + R`, `K = PHᵀS⁻¹`, `x ← x + K(z − Hx)`,
10//! `P ← (I − KH)P`.
11
12use crate::solvers::learning::LearningError;
13use crate::solvers::linear_algebra::cholesky::{cholesky_factor, cholesky_solve};
14use crate::solvers::linear_algebra::gemm::{gemm, matvec, Transpose};
15
16/// A linear-Gaussian Kalman filter with current state estimate.
17#[derive(Debug, Clone)]
18pub struct KalmanFilter {
19    f: Vec<f64>, // n_x × n_x transition
20    h: Vec<f64>, // n_z × n_x observation
21    q: Vec<f64>, // n_x × n_x process noise
22    r: Vec<f64>, // n_z × n_z measurement noise
23    x: Vec<f64>, // n_x state estimate
24    p: Vec<f64>, // n_x × n_x state covariance
25    nx: usize,
26    nz: usize,
27}
28
29fn no(
30    m: usize,
31    n: usize,
32    k: usize,
33    a: &[f64],
34    b: &[f64],
35    out: &mut [f64],
36) -> Result<(), LearningError> {
37    gemm(Transpose::No, Transpose::No, m, n, k, 1.0, a, b, 0.0, out).map_err(Into::into)
38}
39fn no_t(
40    m: usize,
41    n: usize,
42    k: usize,
43    a: &[f64],
44    b: &[f64],
45    out: &mut [f64],
46) -> Result<(), LearningError> {
47    // out(m×n) = a(m×k) · b(n×k)ᵀ
48    gemm(Transpose::No, Transpose::Yes, m, n, k, 1.0, a, b, 0.0, out).map_err(Into::into)
49}
50
51impl KalmanFilter {
52    /// Construct with model matrices and an initial state estimate `(x0, p0)`.
53    pub fn new(
54        f: Vec<f64>,
55        h: Vec<f64>,
56        q: Vec<f64>,
57        r: Vec<f64>,
58        x0: Vec<f64>,
59        p0: Vec<f64>,
60        nx: usize,
61        nz: usize,
62    ) -> Result<Self, LearningError> {
63        if nx == 0
64            || nz == 0
65            || f.len() != nx * nx
66            || h.len() != nz * nx
67            || q.len() != nx * nx
68            || r.len() != nz * nz
69            || x0.len() != nx
70            || p0.len() != nx * nx
71        {
72            return Err(LearningError::InvalidDimension);
73        }
74        Ok(Self {
75            f,
76            h,
77            q,
78            r,
79            x: x0,
80            p: p0,
81            nx,
82            nz,
83        })
84    }
85
86    /// Current state.
87    pub fn state(&self) -> &[f64] {
88        &self.x
89    }
90    /// `KalmanFilter::covariance`.
91    pub fn covariance(&self) -> &[f64] {
92        &self.p
93    }
94
95    /// Time update (predict): advance the state and inflate the covariance.
96    pub fn predict(&mut self) -> Result<(), LearningError> {
97        let nx = self.nx;
98        // x ← F x.
99        let mut xn = vec![0.0; nx];
100        matvec(Transpose::No, nx, nx, &self.f, &self.x, &mut xn)?;
101        self.x = xn;
102        // P ← F P Fᵀ + Q.
103        let mut fp = vec![0.0; nx * nx];
104        no(nx, nx, nx, &self.f, &self.p, &mut fp)?;
105        let mut pn = vec![0.0; nx * nx];
106        no_t(nx, nx, nx, &fp, &self.f, &mut pn)?;
107        for i in 0..nx * nx {
108            pn[i] += self.q[i];
109        }
110        self.p = pn;
111        Ok(())
112    }
113
114    /// Measurement update (correct) with observation `z`.
115    pub fn update(&mut self, z: &[f64]) -> Result<(), LearningError> {
116        let (nx, nz) = (self.nx, self.nz);
117        if z.len() != nz {
118            return Err(LearningError::InvalidDimension);
119        }
120        // Innovation y = z − H x.
121        let mut hx = vec![0.0; nz];
122        matvec(Transpose::No, nz, nx, &self.h, &self.x, &mut hx)?;
123        let y: Vec<f64> = z.iter().zip(&hx).map(|(zi, hxi)| zi - hxi).collect();
124        // HP (nz×nx) and S = HP Hᵀ + R (nz×nz).
125        let mut hp = vec![0.0; nz * nx];
126        no(nz, nx, nx, &self.h, &self.p, &mut hp)?;
127        let mut s = vec![0.0; nz * nz];
128        no_t(nz, nz, nx, &hp, &self.h, &mut s)?;
129        for i in 0..nz * nz {
130            s[i] += self.r[i];
131        }
132        // S⁻¹ via Cholesky.
133        let mut l = vec![0.0; nz * nz];
134        cholesky_factor(nz, &s, &mut l).map_err(|_| LearningError::Singular)?;
135        let mut s_inv = vec![0.0; nz * nz];
136        let mut ej = vec![0.0; nz];
137        let mut cj = vec![0.0; nz];
138        for j in 0..nz {
139            ej.iter_mut().for_each(|v| *v = 0.0);
140            ej[j] = 1.0;
141            cholesky_solve(nz, &l, &ej, &mut cj)?;
142            for i in 0..nz {
143                s_inv[i * nz + j] = cj[i];
144            }
145        }
146        // P Hᵀ (nx×nz), then K = P Hᵀ S⁻¹ (nx×nz).
147        let mut pht = vec![0.0; nx * nz];
148        no_t(nx, nz, nx, &self.p, &self.h, &mut pht)?;
149        let mut kgain = vec![0.0; nx * nz];
150        no(nx, nz, nz, &pht, &s_inv, &mut kgain)?;
151        // x ← x + K y.
152        let mut ky = vec![0.0; nx];
153        matvec(Transpose::No, nx, nz, &kgain, &y, &mut ky)?;
154        for i in 0..nx {
155            self.x[i] += ky[i];
156        }
157        // P ← P − K (H P) = P − K·HP.
158        let mut khp = vec![0.0; nx * nx];
159        no(nx, nx, nz, &kgain, &hp, &mut khp)?;
160        for i in 0..nx * nx {
161            self.p[i] -= khp[i];
162        }
163        Ok(())
164    }
165
166    /// Filter a sequence of observations (row-major `t × nz`), returning the state
167    /// estimate after each (predict→update) step, row-major `t × nx`.
168    pub fn filter(&mut self, observations: &[f64], t: usize) -> Result<Vec<f64>, LearningError> {
169        if observations.len() != t * self.nz {
170            return Err(LearningError::InvalidDimension);
171        }
172        let mut out = vec![0.0; t * self.nx];
173        for step in 0..t {
174            self.predict()?;
175            self.update(&observations[step * self.nz..(step + 1) * self.nz])?;
176            out[step * self.nx..(step + 1) * self.nx].copy_from_slice(&self.x);
177        }
178        Ok(out)
179    }
180}
181
182#[cfg(test)]
183mod tests {
184    use super::*;
185
186    #[test]
187    fn tracks_a_constant_with_noisy_measurements() {
188        // 1-D random-walk model tracking a constant true value 5, noisy obs.
189        let mut kf = KalmanFilter::new(
190            vec![1.0],  // F
191            vec![1.0],  // H
192            vec![1e-4], // Q (nearly constant state)
193            vec![1.0],  // R (noisy measurement)
194            vec![0.0],  // x0
195            vec![10.0], // P0 (weak prior → the data dominates)
196            1,
197            1,
198        )
199        .unwrap();
200        // Measurements jitter around 5.
201        let obs = [5.3, 4.7, 5.1, 4.9, 5.2, 4.8, 5.0, 5.1, 4.9, 5.0];
202        let est = kf.filter(&obs, 10).unwrap();
203        // The final estimate is close to 5 and the covariance has shrunk.
204        assert!((est[9] - 5.0).abs() < 0.3, "estimate {}", est[9]);
205        assert!(kf.covariance()[0] < 1.0, "covariance should shrink");
206    }
207
208    #[test]
209    fn smooths_better_than_raw_measurements() {
210        // The filtered estimate has lower variance than the raw noisy obs.
211        let mut kf = KalmanFilter::new(
212            vec![1.0],
213            vec![1.0],
214            vec![1e-3],
215            vec![1.0],
216            vec![10.0],
217            vec![1.0],
218            1,
219            1,
220        )
221        .unwrap();
222        let obs = [10.5, 9.4, 10.6, 9.5, 10.4, 9.6, 10.5, 9.5];
223        let est = kf.filter(&obs, 8).unwrap();
224        let var = |v: &[f64]| {
225            let m = v.iter().sum::<f64>() / v.len() as f64;
226            v.iter().map(|x| (x - m) * (x - m)).sum::<f64>() / v.len() as f64
227        };
228        let obs_var = var(&obs);
229        let est_var = var(&est[2..]); // skip the warm-up
230        assert!(
231            est_var < obs_var,
232            "filter should smooth: {est_var} !< {obs_var}"
233        );
234    }
235
236    #[test]
237    fn guards() {
238        assert_eq!(
239            KalmanFilter::new(
240                vec![1.0],
241                vec![1.0],
242                vec![1.0],
243                vec![1.0],
244                vec![0.0],
245                vec![1.0, 0.0],
246                1,
247                1
248            )
249            .unwrap_err(),
250            LearningError::InvalidDimension
251        );
252    }
253}