adskalman/
lib.rs

1//! Kalman filter and Rauch-Tung-Striebel smoothing implementation
2//!
3//! Characteristics:
4//! - Uses the [nalgebra](https://nalgebra.org) crate for math.
5//! - Supports `no_std` to facilitate running on embedded microcontrollers.
6//! - Includes [various methods of computing the covariance matrix on the update
7//!   step](enum.CovarianceUpdateMethod.html).
8//! - [Examples](https://github.com/strawlab/adskalman-rs/tree/main/examples)
9//!   included.
10//! - Strong typing used to ensure correct matrix dimensions at compile time.
11//!
12//! Throughout the library, the generic type `SS` means "state size" and `OS` is
13//! "observation size". These refer to the number of dimensions of the state
14//! vector and observation vector, respectively.
15
16// Ideas for improvement:
17//  - See http://mocha-java.uccs.edu/ECE5550/, especially
18//    "5.1: Maintaining symmetry of covariance matrices".
19//  - See http://www.anuncommonlab.com/articles/how-kalman-filters-work/part2.html
20//  - See https://stats.stackexchange.com/questions/67262/non-overlapping-state-and-measurement-covariances-in-kalman-filter/292690
21//  - https://en.wikipedia.org/wiki/Kalman_filter#Square_root_form
22
23#![cfg_attr(not(feature = "std"), no_std)]
24#![allow(non_snake_case)]
25#[cfg(feature = "std")]
26use log::trace;
27
28use nalgebra as na;
29use nalgebra::{
30    allocator::Allocator, base::storage::Owned, dimension::DimMin, DefaultAllocator, Dim, DimName,
31    Matrix, RealField, Vector, U1,
32};
33
34use num_traits::identities::One;
35
36// Without std, create a dummy trace!() macro.
37#[cfg(not(feature = "std"))]
38macro_rules! trace {
39    ($e:expr) => {{}};
40    ($e:expr, $($es:expr),+) => {{}};
41}
42
43/// perform a runtime check that matrix is symmetric
44///
45/// only compiled in debug mode
46macro_rules! debug_assert_symmetric {
47    ($mat:expr) => {
48        #[cfg(debug_assertions)]
49        {
50            if approx::relative_ne!($mat, &$mat.transpose(), max_relative = na::convert(1e-5)) {
51                return Err(Error::CovarianceNotPositiveSemiDefinite);
52            }
53        }
54    };
55}
56
57/// convert an nalgebra array to a String
58#[cfg(feature = "std")]
59macro_rules! pretty_print {
60    ($arr:expr) => {{
61        let indent = 4;
62        let prefix = String::from_utf8(vec![b' '; indent]).unwrap();
63        let mut result_els = vec!["".to_string()];
64        for i in 0..$arr.nrows() {
65            let mut row_els = vec![];
66            for j in 0..$arr.ncols() {
67                row_els.push(format!("{:12.3}", $arr[(i, j)]));
68            }
69            let row_str = row_els.into_iter().collect::<Vec<_>>().join(" ");
70            let row_str = format!("{}{}", prefix, row_str);
71            result_els.push(row_str);
72        }
73        result_els.into_iter().collect::<Vec<_>>().join("\n")
74    }};
75}
76
77mod error;
78pub use error::Error;
79
80mod state_and_covariance;
81pub use state_and_covariance::StateAndCovariance;
82
83/// A linear model of process dynamics with no control inputs
84pub trait TransitionModelLinearNoControl<R, SS>
85where
86    R: RealField,
87    SS: Dim,
88    DefaultAllocator: Allocator<SS, SS> + Allocator<SS>,
89{
90    /// Get the state transition model, `F`.
91    fn F(&self) -> &Matrix<R, SS, SS, Owned<R, SS, SS>>;
92
93    /// Get the transpose of the state transition model, `FT`.
94    fn FT(&self) -> &Matrix<R, SS, SS, Owned<R, SS, SS>>;
95
96    /// Get the process covariance, `Q`.
97    fn Q(&self) -> &Matrix<R, SS, SS, Owned<R, SS, SS>>;
98
99    /// Predict new state from previous estimate.
100    fn predict(&self, previous_estimate: &StateAndCovariance<R, SS>) -> StateAndCovariance<R, SS> {
101        // The prior.
102        let P = previous_estimate.state();
103        let F = self.F();
104        let mut state = P.clone(); // allocate output
105        F.mul_to(P, &mut state);
106        let covariance: Matrix<R, SS, SS, _> =
107            ((F * previous_estimate.covariance()) * self.FT()) + self.Q();
108        StateAndCovariance::new(state, covariance)
109    }
110}
111
112/// An observation model, potentially non-linear.
113///
114/// To use a non-linear observation model, the non-linear model must be
115/// linearized (e.g. using the prior state estimate) and use this linearization
116/// as the basis for a `ObservationModel` implementation. This would be done
117/// every timestep. For an example, see
118/// [`nonlinear_observation.rs`](https://github.com/strawlab/adskalman-rs/blob/main/examples/src/bin/nonlinear_observation.rs).
119pub trait ObservationModel<R, SS, OS>
120where
121    R: RealField,
122    SS: Dim,
123    OS: Dim + DimMin<OS, Output = OS>,
124    DefaultAllocator: Allocator<SS, SS>
125        + Allocator<SS>
126        + Allocator<OS, SS>
127        + Allocator<SS, OS>
128        + Allocator<OS, OS>
129        + Allocator<OS>,
130    Matrix<R, SS, SS, Owned<R, SS, SS>>: One,
131{
132    /// For a given state, predict the observation.
133    ///
134    /// The default implementation implements a linear observation model, namely
135    /// `y = Hx` where `y` is the predicted observation, `H` is the observation
136    /// matrix, and `x` is the state. For a non-linear observation model, any
137    /// implementation of this trait should provide an implementation of this
138    /// method.
139    ///
140    /// If an observation is not possible, this returns NaN values. (This
141    /// happens, for example, when a non-linear observation model implements
142    /// this trait and must be evaluated for a state for which no observation is
143    /// possible.) Observations with NaN values are treated as missing
144    /// observations.
145    fn predict_observation(
146        &self,
147        state: &Vector<R, SS, Owned<R, SS>>,
148    ) -> Vector<R, OS, Owned<R, OS>> {
149        self.H() * state
150    }
151
152    /// Get the observation matrix, `H`.
153    fn H(&self) -> &Matrix<R, OS, SS, Owned<R, OS, SS>>;
154
155    /// Get the transpose of the observation matrix, `HT`.
156    fn HT(&self) -> &Matrix<R, SS, OS, Owned<R, SS, OS>>;
157
158    /// Get the observation noise covariance, `R`.
159    // TODO: ensure this is positive definite?
160    fn R(&self) -> &Matrix<R, OS, OS, Owned<R, OS, OS>>;
161
162    /// Given prior state and observation, estimate the posterior state.
163    ///
164    /// This is the *update* step in the Kalman filter literature.
165    fn update(
166        &self,
167        prior: &StateAndCovariance<R, SS>,
168        observation: &Vector<R, OS, Owned<R, OS>>,
169        covariance_method: CovarianceUpdateMethod,
170    ) -> Result<StateAndCovariance<R, SS>, Error> {
171        let h = self.H();
172        trace!("h {}", pretty_print!(h));
173
174        let p = prior.covariance();
175        trace!("p {}", pretty_print!(p));
176        debug_assert_symmetric!(p);
177
178        let ht = self.HT();
179        trace!("ht {}", pretty_print!(ht));
180
181        let r = self.R();
182        trace!("r {}", pretty_print!(r));
183
184        // Calculate innovation covariance
185        //
186        // Math note: if (h*p*ht) and r are positive definite, s is also
187        // positive definite. If p is positive definite, then (h*p*ht) is at
188        // least positive semi-definite. If h is full rank, it is positive
189        // definite.
190        let s = (h * p * ht) + r;
191        trace!("s {}", pretty_print!(s));
192
193        // Calculate kalman gain by inverting.
194        let s_chol = match na::linalg::Cholesky::new(s) {
195            Some(v) => v,
196            None => {
197                // Maybe state covariance is not symmetric or
198                // for from positive definite? Also, observation
199                // noise should be positive definite.
200                return Err(Error::CovarianceNotPositiveSemiDefinite);
201            }
202        };
203        let s_inv: Matrix<R, OS, OS, _> = s_chol.inverse();
204        trace!("s_inv {}", pretty_print!(s_inv));
205
206        let k_gain: Matrix<R, SS, OS, _> = p * ht * s_inv;
207        // let k_gain: OMatrix<R,SS,OS> = solve!( (p*ht), s );
208        trace!("k_gain {}", pretty_print!(k_gain));
209
210        let predicted: Vector<R, OS, _> = self.predict_observation(prior.state());
211        trace!("predicted {}", pretty_print!(predicted));
212        trace!("observation {}", pretty_print!(observation));
213        let innovation: Vector<R, OS, _> = observation - predicted;
214        trace!("innovation {}", pretty_print!(innovation));
215        let state: Vector<R, SS, _> = prior.state() + &k_gain * innovation;
216        trace!("state {}", pretty_print!(state));
217
218        trace!("self.observation_matrix() {}", pretty_print!(self.H()));
219        let kh: Matrix<R, SS, SS, _> = &k_gain * self.H();
220        trace!("kh {}", pretty_print!(kh));
221        let one_minus_kh = Matrix::<R, SS, SS, Owned<R, SS, SS>>::one() - kh;
222        trace!("one_minus_kh {}", pretty_print!(one_minus_kh));
223
224        let covariance: Matrix<R, SS, SS, _> = match covariance_method {
225            CovarianceUpdateMethod::JosephForm => {
226                // Joseph form of covariance update keeps covariance matrix symmetric.
227
228                let left = &one_minus_kh * prior.covariance() * one_minus_kh.transpose();
229                let right = &k_gain * r * &k_gain.transpose();
230                left + right
231            }
232            CovarianceUpdateMethod::OptimalKalman => &one_minus_kh * prior.covariance(),
233            CovarianceUpdateMethod::OptimalKalmanForcedSymmetric => {
234                let covariance1 = &one_minus_kh * prior.covariance();
235                trace!("covariance1 {}", pretty_print!(covariance1));
236                // Hack to force covariance to be symmetric.
237                // See https://math.stackexchange.com/q/2335831
238                covariance1.symmetric_part()
239            }
240        };
241        trace!("covariance {}", pretty_print!(covariance));
242
243        debug_assert_symmetric!(covariance);
244
245        Ok(StateAndCovariance::new(state, covariance))
246    }
247}
248
249/// Specifies the approach used for updating the covariance matrix
250#[derive(Debug, PartialEq, Clone, Copy)]
251pub enum CovarianceUpdateMethod {
252    /// Assumes optimal Kalman gain.
253    ///
254    /// Due to numerical errors, covariance matrix may not remain symmetric.
255    OptimalKalman,
256    /// Assumes optimal Kalman gain and then forces symmetric covariance matrix.
257    ///
258    /// With original covariance matrix P, returns covariance as (P + P.T)/2
259    /// to enforce that the covariance matrix remains symmetric.
260    OptimalKalmanForcedSymmetric,
261    /// Joseph form of covariance update keeps covariance matrix symmetric.
262    JosephForm,
263}
264
265/// A Kalman filter with no control inputs, a linear process model and linear
266/// observation model
267///
268/// Note that the structure is cheap to create, storing only references to the
269/// state transition model and the observation model. (The system state is
270/// passed as an argument to methods like [Self::step].) Given the lifetime
271/// bound of this struct, a useful strategy to avoid requiring lifetime
272/// annotations is to construct it just before [Self::step] and then dropping it
273/// immediately afterward.
274pub struct KalmanFilterNoControl<'a, R, SS, OS>
275where
276    R: RealField,
277    SS: Dim,
278    OS: Dim,
279{
280    transition_model: &'a dyn TransitionModelLinearNoControl<R, SS>,
281    observation_matrix: &'a dyn ObservationModel<R, SS, OS>,
282}
283
284impl<'a, R, SS, OS> KalmanFilterNoControl<'a, R, SS, OS>
285where
286    R: RealField,
287    SS: DimName,
288    OS: Dim + DimMin<OS, Output = OS>,
289    DefaultAllocator: Allocator<SS, SS>
290        + Allocator<SS>
291        + Allocator<OS, SS>
292        + Allocator<SS, OS>
293        + Allocator<OS, OS>
294        + Allocator<OS>,
295{
296    /// Initialize a new `KalmanFilterNoControl` struct.
297    ///
298    /// The first parameter, `transition_model`, specifies the state transition
299    /// model, including the function `F` and the process covariance `Q`. The
300    /// second parameter, `observation_matrix`, specifies the observation model,
301    /// including the measurement function `H` and the measurement covariance
302    /// `R`.
303    pub fn new(
304        transition_model: &'a dyn TransitionModelLinearNoControl<R, SS>,
305        observation_matrix: &'a dyn ObservationModel<R, SS, OS>,
306    ) -> Self {
307        Self {
308            transition_model,
309            observation_matrix,
310        }
311    }
312
313    /// Perform Kalman prediction and update steps with default values
314    ///
315    /// If any component of the observation is NaN (not a number), the
316    /// observation will not be used but rather the prior will be returned as
317    /// the posterior without performing the update step.
318    ///
319    /// This calls the prediction step of the transition model and then, if
320    /// there is a (non-`nan`) observation, calls the update step of the
321    /// observation model using the `CovarianceUpdateMethod::JosephForm`
322    /// covariance update method.
323    ///
324    /// This is a convenience method that calls
325    /// [step_with_options](struct.KalmanFilterNoControl.html#method.step_with_options).
326    pub fn step(
327        &self,
328        previous_estimate: &StateAndCovariance<R, SS>,
329        observation: &Vector<R, OS, Owned<R, OS>>,
330    ) -> Result<StateAndCovariance<R, SS>, Error> {
331        self.step_with_options(
332            previous_estimate,
333            observation,
334            CovarianceUpdateMethod::JosephForm,
335        )
336    }
337
338    /// Perform Kalman prediction and update steps with default values
339    ///
340    /// If any component of the observation is NaN (not a number), the
341    /// observation will not be used but rather the prior will be returned as
342    /// the posterior without performing the update step.
343    ///
344    /// This calls the prediction step of the transition model and then, if
345    /// there is a (non-`nan`) observation, calls the update step of the
346    /// observation model using the specified covariance update method.
347    pub fn step_with_options(
348        &self,
349        previous_estimate: &StateAndCovariance<R, SS>,
350        observation: &Vector<R, OS, Owned<R, OS>>,
351        covariance_update_method: CovarianceUpdateMethod,
352    ) -> Result<StateAndCovariance<R, SS>, Error> {
353        let prior = self.transition_model.predict(previous_estimate);
354        if observation.iter().any(|x| is_nan(x.clone())) {
355            Ok(prior)
356        } else {
357            self.observation_matrix
358                .update(&prior, observation, covariance_update_method)
359        }
360    }
361
362    /// Kalman filter (operates on in-place data without allocating)
363    ///
364    /// Operates on entire time series (by repeatedly calling
365    /// [`step`](struct.KalmanFilterNoControl.html#method.step) for each
366    /// observation) and returns a vector of state estimates. To be
367    /// mathematically correct, the interval between observations must be the
368    /// `dt` specified in the motion model.
369    ///
370    /// If any observation has a NaN component, it is treated as missing.
371    pub fn filter_inplace(
372        &self,
373        initial_estimate: &StateAndCovariance<R, SS>,
374        observations: &[Vector<R, OS, Owned<R, OS>>],
375        state_estimates: &mut [StateAndCovariance<R, SS>],
376    ) -> Result<(), Error> {
377        let mut previous_estimate = initial_estimate.clone();
378        assert!(state_estimates.len() >= observations.len());
379
380        for (this_observation, state_estimate) in
381            observations.iter().zip(state_estimates.iter_mut())
382        {
383            let this_observation: &Matrix<R, OS, U1, Owned<R, OS>> = this_observation;
384            let this_estimate: StateAndCovariance<R, SS> =
385                self.step(&previous_estimate, this_observation)?;
386            *state_estimate = this_estimate.clone();
387            previous_estimate = this_estimate;
388        }
389        Ok(())
390    }
391
392    /// Kalman filter
393    ///
394    /// This is a convenience function that calls [`filter_inplace`](struct.KalmanFilterNoControl.html#method.filter_inplace).
395    #[cfg(feature = "std")]
396    pub fn filter(
397        &self,
398        initial_estimate: &StateAndCovariance<R, SS>,
399        observations: &[Vector<R, OS, Owned<R, OS>>],
400    ) -> Result<Vec<StateAndCovariance<R, SS>>, Error> {
401        use nalgebra::OMatrix;
402
403        let mut state_estimates = Vec::with_capacity(observations.len());
404        let empty: StateAndCovariance<R, SS> = StateAndCovariance::new(na::zero(), OMatrix::one());
405        for _ in 0..observations.len() {
406            state_estimates.push(empty.clone());
407        }
408        self.filter_inplace(initial_estimate, observations, &mut state_estimates)?;
409        Ok(state_estimates)
410    }
411
412    /// Rauch-Tung-Striebel (RTS) smoother
413    ///
414    /// Operates on entire time series (by calling
415    /// [`filter`](struct.KalmanFilterNoControl.html#method.filter) then
416    /// [`smooth_from_filtered`](struct.KalmanFilterNoControl.html#method.smooth_from_filtered))
417    /// and returns a vector of state estimates. To be mathematically correct,
418    /// the interval between observations must be the `dt` specified in the
419    /// motion model.
420    ///
421    /// Operates on entire time series in one shot and returns a vector of state
422    /// estimates. To be mathematically correct, the interval between
423    /// observations must be the `dt` specified in the motion model.
424    ///
425    /// If any observation has a NaN component, it is treated as missing.
426    #[cfg(feature = "std")]
427    pub fn smooth(
428        &self,
429        initial_estimate: &StateAndCovariance<R, SS>,
430        observations: &[Vector<R, OS, Owned<R, OS>>],
431    ) -> Result<Vec<StateAndCovariance<R, SS>>, Error> {
432        let forward_results = self.filter(initial_estimate, observations)?;
433        self.smooth_from_filtered(forward_results)
434    }
435
436    /// Rauch-Tung-Striebel (RTS) smoother using already Kalman filtered estimates
437    ///
438    /// Operates on entire time series in one shot and returns a vector of state
439    /// estimates. To be mathematically correct, the interval between
440    /// observations must be the `dt` specified in the motion model.
441    #[cfg(feature = "std")]
442    pub fn smooth_from_filtered(
443        &self,
444        mut forward_results: Vec<StateAndCovariance<R, SS>>,
445    ) -> Result<Vec<StateAndCovariance<R, SS>>, Error> {
446        forward_results.reverse();
447
448        let mut smoothed_backwards = Vec::with_capacity(forward_results.len());
449
450        let mut smooth_future = forward_results[0].clone();
451        smoothed_backwards.push(smooth_future.clone());
452        for filt in forward_results.iter().skip(1) {
453            smooth_future = self.smooth_step(&smooth_future, filt)?;
454            smoothed_backwards.push(smooth_future.clone());
455        }
456
457        smoothed_backwards.reverse();
458        Ok(smoothed_backwards)
459    }
460
461    #[cfg(feature = "std")]
462    fn smooth_step(
463        &self,
464        smooth_future: &StateAndCovariance<R, SS>,
465        filt: &StateAndCovariance<R, SS>,
466    ) -> Result<StateAndCovariance<R, SS>, Error> {
467        let prior = self.transition_model.predict(filt);
468
469        let v_chol = match na::linalg::Cholesky::new(prior.covariance().clone()) {
470            Some(v) => v,
471            None => {
472                return Err(Error::CovarianceNotPositiveSemiDefinite);
473            }
474        };
475        let inv_prior_covariance: Matrix<R, SS, SS, _> = v_chol.inverse();
476        trace!(
477            "inv_prior_covariance {}",
478            pretty_print!(inv_prior_covariance)
479        );
480
481        // J = dot(Vfilt, dot(A.T, inv(Vpred)))  # smoother gain matrix
482        let j = filt.covariance() * (self.transition_model.FT() * inv_prior_covariance);
483
484        // xsmooth = xfilt + dot(J, xsmooth_future - xpred)
485        let residuals = smooth_future.state() - prior.state();
486        let state = filt.state() + &j * residuals;
487
488        // Vsmooth = Vfilt + dot(J, dot(Vsmooth_future - Vpred, J.T))
489        let covar_residuals = smooth_future.covariance() - prior.covariance();
490        let covariance = filt.covariance() + &j * (covar_residuals * j.transpose());
491
492        Ok(StateAndCovariance::new(state, covariance))
493    }
494}
495
496#[inline]
497fn is_nan<R: RealField>(x: R) -> bool {
498    x.partial_cmp(&R::zero()).is_none()
499}
500
501#[test]
502fn test_is_nan() {
503    assert!(!is_nan::<f64>(-1.0));
504    assert!(!is_nan::<f64>(0.0));
505    assert!(!is_nan::<f64>(1.0));
506    assert!(!is_nan::<f64>(1.0 / 0.0));
507    assert!(!is_nan::<f64>(-1.0 / 0.0));
508    assert!(is_nan::<f64>(std::f64::NAN));
509
510    assert!(!is_nan::<f32>(-1.0));
511    assert!(!is_nan::<f32>(0.0));
512    assert!(!is_nan::<f32>(1.0));
513    assert!(!is_nan::<f32>(1.0 / 0.0));
514    assert!(!is_nan::<f32>(-1.0 / 0.0));
515    assert!(is_nan::<f32>(std::f32::NAN));
516}
adskalman/lib.rs

adskalman/
lib.rs
