Skip to main content

typed_kalman/
lib.rs

1#![cfg_attr(not(feature = "std"), no_std)]
2#![deny(missing_docs)]
3
4//! Type-safe Kalman filters for desktop and embedded targets.
5//!
6//! Dimensions are const generics on [`nalgebra`] static matrices, so a measurement of the wrong
7//! size doesn't compile. Nothing allocates, the whole crate works without `std`, and every
8//! filter runs in `f32` or `f64`.
9//!
10//! # Quick start
11//!
12//! Track a target moving at constant velocity, `[position, velocity]`, from noisy position
13//! measurements:
14//!
15//! ```
16//! use typed_kalman::LinearKf;
17//! use nalgebra::{Matrix1, Matrix1x2, Matrix2, Vector1, Vector2};
18//!
19//! let dt = 0.1;
20//! let f = Matrix2::new(1.0, dt, 0.0, 1.0); // state transition
21//! let q = Matrix2::new(1e-4, 0.0, 0.0, 1e-2); // process noise
22//! let h = Matrix1x2::new(1.0, 0.0); // we measure position only
23//! let r = Matrix1::new(0.25); // measurement noise
24//!
25//! let mut kf = LinearKf::new(Vector2::new(0.0, 0.0), Matrix2::identity());
26//! for k in 1..=50 {
27//!     let z = Vector1::new(2.0 * k as f64 * dt); // truly moving at 2 units/s
28//!     kf.predict(&f, &q);
29//!     let nis = kf.update(&h, &z, &r)?; // normalized innovation squared, for diagnostics
30//!     assert!(nis.is_finite());
31//! }
32//! assert!((kf.state()[1] - 2.0).abs() < 0.1); // the velocity has been estimated
33//! # Ok::<(), typed_kalman::KalmanError>(())
34//! ```
35//!
36//! # Choosing a filter
37//!
38//! | Filter | Models | Use it when |
39//! |---|---|---|
40//! | [`LinearKf`] | matrices `F`, `H` | the system is linear; fastest |
41//! | [`SqrtKf`] | matrices, noise as square roots | the covariance is too ill-conditioned for `LinearKf` (very precise sensors, `f32`); 2–3× slower |
42//! | [`Ekf`] | [`ProcessJacobian`], [`MeasurementJacobian`] | the system is mildly nonlinear and you have Jacobians, or let the `autodiff` feature compute them |
43//! | [`Ukf`] | [`ProcessModel`], [`MeasurementModel`] | the system is nonlinear and Jacobians are awkward |
44//! | [`SqrtUkf`] | the same, noise as square roots | as the UKF, with a covariance factor that can't lose positive-definiteness |
45//!
46//! [`smoother::smooth`] runs a Rauch-Tung-Striebel smoother over a recorded history (for the
47//! UKF, [`smoother::smooth_with_cross`] with [`Ukf::predict_with_cross`]), and
48//! [`diagnostics`] checks filter consistency with NEES and chi-squared bounds.
49//!
50//! # Errors
51//!
52//! Filters never panic on bad data. A NaN or infinite input, a singular innovation covariance,
53//! or a covariance that stops being positive-definite returns a [`KalmanError`] and leaves the
54//! filter exactly as it was, so the caller can skip that measurement and carry on.
55//!
56//! # Features
57//!
58//! - `std` (default): links the standard library. Turn it off with `default-features = false`
59//!   for bare-metal targets; nothing else changes.
60//! - `autodiff`: Jacobians computed exactly with dual numbers, in the `autodiff` module.
61//!
62//! # Embedded use
63//!
64//! Without `std`, the filters work unchanged, in single precision:
65//!
66//! ```
67//! use typed_kalman::LinearKf;
68//! use nalgebra::{Matrix1, Matrix1x2, Matrix2, Vector1, Vector2};
69//!
70//! fn step(kf: &mut LinearKf<2, f32>, position: f32) -> Option<f32> {
71//!     let f = Matrix2::new(1.0, 0.01, 0.0, 1.0);
72//!     let q = Matrix2::new(1e-6, 0.0, 0.0, 1e-4);
73//!     kf.predict(&f, &q);
74//!     // On a bad reading the filter is unchanged, so just skip it.
75//!     kf.update(&Matrix1x2::new(1.0, 0.0), &Vector1::new(position), &Matrix1::new(0.01)).ok()?;
76//!     Some(kf.state()[1]) // estimated velocity
77//! }
78//! # fn main() {
79//! # let mut kf = LinearKf::new(Vector2::new(0.0f32, 0.0), Matrix2::identity());
80//! # assert!(step(&mut kf, 0.5).is_some());
81//! # assert!(step(&mut kf, f32::NAN).is_none());
82//! # }
83//! ```
84
85#[cfg(feature = "autodiff")]
86pub mod autodiff;
87mod cholupdate;
88pub mod diagnostics;
89pub mod ekf;
90pub mod error;
91pub mod linear;
92pub mod model;
93pub mod scalar;
94pub mod smoother;
95pub mod sqrt_kf;
96pub mod sqrt_ukf;
97pub mod ukf;
98mod update;
99
100/// Compiles and runs the README's examples as doctests, so they can't go stale.
101#[cfg(doctest)]
102#[doc = include_str!("../README.md")]
103struct ReadmeDoctests;
104
105pub use ekf::Ekf;
106pub use error::KalmanError;
107pub use linear::LinearKf;
108pub use model::{
109    wrap_angle, ControlledJacobian, ControlledProcess, MeasurementJacobian, MeasurementModel,
110    ProcessJacobian, ProcessModel, WithInput,
111};
112pub use scalar::Float;
113pub use smoother::{CrossStep, Estimate, RtsStep};
114pub use sqrt_kf::SqrtKf;
115pub use sqrt_ukf::SqrtUkf;
116pub use ukf::{Ukf, UkfParams};