1use 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#[derive(Debug, Clone)]
18pub struct KalmanFilter {
19 f: Vec<f64>, h: Vec<f64>, q: Vec<f64>, r: Vec<f64>, x: Vec<f64>, p: Vec<f64>, 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 gemm(Transpose::No, Transpose::Yes, m, n, k, 1.0, a, b, 0.0, out).map_err(Into::into)
49}
50
51impl KalmanFilter {
52 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 pub fn state(&self) -> &[f64] {
88 &self.x
89 }
90 pub fn covariance(&self) -> &[f64] {
92 &self.p
93 }
94
95 pub fn predict(&mut self) -> Result<(), LearningError> {
97 let nx = self.nx;
98 let mut xn = vec![0.0; nx];
100 matvec(Transpose::No, nx, nx, &self.f, &self.x, &mut xn)?;
101 self.x = xn;
102 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 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 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 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 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 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 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 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 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 let mut kf = KalmanFilter::new(
190 vec![1.0], vec![1.0], vec![1e-4], vec![1.0], vec![0.0], vec![10.0], 1,
197 1,
198 )
199 .unwrap();
200 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 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 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..]); 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}