Skip to main content

smelly/
lib.rs

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    // P = P - KHP avoids the cubic matrix products of the Joseph form.
173    workspace
174        .covariance
175        .gemm(T::one(), &k, &workspace.hp, T::zero());
176    *p -= &workspace.covariance;
177
178    // Roundoff can make the algebraically symmetric covariance slightly asymmetric.
179    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}