1#![no_std]
2#![doc = include_str!("../README.md")]
3
4extern crate alloc;
5
6use alloc::format;
7
8mod error;
9mod extended;
10mod kalman;
11mod particle;
12mod static_filters;
13mod unscented;
14
15pub use error::FilterError;
16pub use extended::ExtendedKalmanFilter;
17pub use kalman::KalmanFilter;
18pub use particle::{ParticleFilter, SParticleFilter};
19pub use static_filters::{SExtendedKalmanFilter, SKalmanFilter, SUnscentedKalmanFilter};
20pub use unscented::{MerweScaledSigmaPoints, UnscentedKalmanFilter};
21
22use nalgebra::{DMatrix, DVector, RealField};
23
24#[derive(Debug, Clone)]
25pub(crate) struct FilterWorkspace<T: RealField> {
26 predicted_x: DVector<T>,
27 covariance: DMatrix<T>,
28 product: DMatrix<T>,
29 transpose: DMatrix<T>,
30 hp: DMatrix<T>,
31 pht: DMatrix<T>,
32 innovation_covariance: DMatrix<T>,
33}
34
35impl<T: RealField + Copy> FilterWorkspace<T> {
36 #[inline]
37 pub(crate) fn new() -> Self {
38 Self {
39 predicted_x: DVector::zeros(0),
40 covariance: DMatrix::zeros(0, 0),
41 product: DMatrix::zeros(0, 0),
42 transpose: DMatrix::zeros(0, 0),
43 hp: DMatrix::zeros(0, 0),
44 pht: DMatrix::zeros(0, 0),
45 innovation_covariance: DMatrix::zeros(0, 0),
46 }
47 }
48
49 #[inline]
50 fn prepare_state(&mut self, state_dim: usize) {
51 resize_vector(&mut self.predicted_x, state_dim);
52 resize_matrix(&mut self.covariance, state_dim, state_dim);
53 resize_matrix(&mut self.product, state_dim, state_dim);
54 resize_matrix(&mut self.transpose, state_dim, state_dim);
55 }
56
57 #[inline]
58 fn prepare_measurement(&mut self, state_dim: usize, measurement_dim: usize) {
59 self.prepare_state(state_dim);
60 resize_matrix(&mut self.hp, measurement_dim, state_dim);
61 resize_matrix(&mut self.pht, state_dim, measurement_dim);
62 resize_matrix(
63 &mut self.innovation_covariance,
64 measurement_dim,
65 measurement_dim,
66 );
67 }
68}
69
70#[inline]
71fn resize_vector<T: RealField + Copy>(vector: &mut DVector<T>, len: usize) {
72 if vector.len() != len {
73 vector.resize_vertically_mut(len, T::zero());
74 }
75}
76
77#[inline]
78fn resize_matrix<T: RealField + Copy>(matrix: &mut DMatrix<T>, rows: usize, columns: usize) {
79 if matrix.shape() != (rows, columns) {
80 matrix.resize_mut(rows, columns, T::zero());
81 }
82}
83
84#[inline]
85pub(crate) fn check_square<T: RealField>(
86 name: &'static str,
87 matrix: &DMatrix<T>,
88 size: usize,
89) -> Result<(), FilterError> {
90 if matrix.nrows() != size || matrix.ncols() != size {
91 return Err(FilterError::Dimension {
92 name,
93 expected: format!("{size}x{size}"),
94 actual: format!("{}x{}", matrix.nrows(), matrix.ncols()),
95 });
96 }
97 Ok(())
98}
99
100pub(crate) fn replace_square<T: RealField>(
101 name: &'static str,
102 target: &mut DMatrix<T>,
103 value: DMatrix<T>,
104 size: usize,
105) -> Result<(), FilterError> {
106 check_square(name, &value, size)?;
107 *target = value;
108 Ok(())
109}
110
111pub(crate) fn right_solve<T>(a: &DMatrix<T>, b: &DMatrix<T>) -> Result<DMatrix<T>, FilterError>
112where
113 T: RealField + Copy,
114{
115 a.transpose()
116 .lu()
117 .solve(&b.transpose())
118 .map(|solution| solution.transpose())
119 .ok_or(FilterError::SingularInnovation)
120}
121
122pub(crate) fn linear_predict<T>(
123 x: &mut DVector<T>,
124 p: &mut DMatrix<T>,
125 f: &DMatrix<T>,
126 q: &DMatrix<T>,
127 workspace: &mut FilterWorkspace<T>,
128) where
129 T: RealField + Copy,
130{
131 let n = x.len();
132 workspace.prepare_state(n);
133 workspace.predicted_x.gemv(T::one(), f, x, T::zero());
134 core::mem::swap(x, &mut workspace.predicted_x);
135
136 f.transpose_to(&mut workspace.transpose);
137 workspace.product.gemm(T::one(), f, &*p, T::zero());
138 workspace.covariance.gemm(
139 T::one(),
140 &workspace.product,
141 &workspace.transpose,
142 T::zero(),
143 );
144 core::mem::swap(p, &mut workspace.covariance);
145 *p += q;
146}
147
148pub(crate) fn linear_update<T>(
149 x: &mut DVector<T>,
150 p: &mut DMatrix<T>,
151 h: &DMatrix<T>,
152 r: &DMatrix<T>,
153 innovation: DVector<T>,
154 workspace: &mut FilterWorkspace<T>,
155) -> Result<DVector<T>, FilterError>
156where
157 T: RealField + Copy,
158{
159 let n = x.len();
160 let m = h.nrows();
161 workspace.prepare_measurement(n, m);
162
163 workspace.hp.gemm(T::one(), h, &*p, T::zero());
164 workspace.hp.transpose_to(&mut workspace.pht);
165 workspace.innovation_covariance.copy_from(r);
166 workspace
167 .innovation_covariance
168 .gemm(T::one(), h, &workspace.pht, T::one());
169 let k = right_solve(&workspace.innovation_covariance, &workspace.pht)?;
170 x.gemv(T::one(), &k, &innovation, T::one());
171
172 workspace
174 .covariance
175 .gemm(T::one(), &k, &workspace.hp, T::zero());
176 *p -= &workspace.covariance;
177
178 let half = T::from_f64(0.5).unwrap_or_else(|| T::one() / (T::one() + T::one()));
180 for row in 0..n {
181 for column in 0..row {
182 let average = (p[(row, column)] + p[(column, row)]) * half;
183 p[(row, column)] = average;
184 p[(column, row)] = average;
185 }
186 }
187 Ok(innovation)
188}