Skip to main content

Crate typed_kalman

Crate typed_kalman 

Source
Expand description

Type-safe Kalman filters for desktop and embedded targets.

Dimensions are const generics on nalgebra static matrices, so a measurement of the wrong size doesn’t compile. Nothing allocates, the whole crate works without std, and every filter runs in f32 or f64.

§Quick start

Track a target moving at constant velocity, [position, velocity], from noisy position measurements:

use typed_kalman::LinearKf;
use nalgebra::{Matrix1, Matrix1x2, Matrix2, Vector1, Vector2};

let dt = 0.1;
let f = Matrix2::new(1.0, dt, 0.0, 1.0); // state transition
let q = Matrix2::new(1e-4, 0.0, 0.0, 1e-2); // process noise
let h = Matrix1x2::new(1.0, 0.0); // we measure position only
let r = Matrix1::new(0.25); // measurement noise

let mut kf = LinearKf::new(Vector2::new(0.0, 0.0), Matrix2::identity());
for k in 1..=50 {
    let z = Vector1::new(2.0 * k as f64 * dt); // truly moving at 2 units/s
    kf.predict(&f, &q);
    let nis = kf.update(&h, &z, &r)?; // normalized innovation squared, for diagnostics
    assert!(nis.is_finite());
}
assert!((kf.state()[1] - 2.0).abs() < 0.1); // the velocity has been estimated

§Choosing a filter

FilterModelsUse it when
LinearKfmatrices F, Hthe system is linear; fastest
SqrtKfmatrices, noise as square rootsthe covariance is too ill-conditioned for LinearKf (very precise sensors, f32); 2–3× slower
EkfProcessJacobian, MeasurementJacobianthe system is mildly nonlinear and you have Jacobians, or let the autodiff feature compute them
UkfProcessModel, MeasurementModelthe system is nonlinear and Jacobians are awkward
SqrtUkfthe same, noise as square rootsas the UKF, with a covariance factor that can’t lose positive-definiteness

smoother::smooth runs a Rauch-Tung-Striebel smoother over a recorded history (for the UKF, smoother::smooth_with_cross with Ukf::predict_with_cross), and diagnostics checks filter consistency with NEES and chi-squared bounds.

§Errors

Filters never panic on bad data. A NaN or infinite input, a singular innovation covariance, or a covariance that stops being positive-definite returns a KalmanError and leaves the filter exactly as it was, so the caller can skip that measurement and carry on.

§Features

  • std (default): links the standard library. Turn it off with default-features = false for bare-metal targets; nothing else changes.
  • autodiff: Jacobians computed exactly with dual numbers, in the autodiff module.

§Embedded use

Without std, the filters work unchanged, in single precision:

use typed_kalman::LinearKf;
use nalgebra::{Matrix1, Matrix1x2, Matrix2, Vector1, Vector2};

fn step(kf: &mut LinearKf<2, f32>, position: f32) -> Option<f32> {
    let f = Matrix2::new(1.0, 0.01, 0.0, 1.0);
    let q = Matrix2::new(1e-6, 0.0, 0.0, 1e-4);
    kf.predict(&f, &q);
    // On a bad reading the filter is unchanged, so just skip it.
    kf.update(&Matrix1x2::new(1.0, 0.0), &Vector1::new(position), &Matrix1::new(0.01)).ok()?;
    Some(kf.state()[1]) // estimated velocity
}

Re-exports§

pub use ekf::Ekf;
pub use error::KalmanError;
pub use linear::LinearKf;
pub use model::wrap_angle;
pub use model::ControlledJacobian;
pub use model::ControlledProcess;
pub use model::MeasurementJacobian;
pub use model::MeasurementModel;
pub use model::ProcessJacobian;
pub use model::ProcessModel;
pub use model::WithInput;
pub use scalar::Float;
pub use smoother::CrossStep;
pub use smoother::Estimate;
pub use smoother::RtsStep;
pub use sqrt_kf::SqrtKf;
pub use sqrt_ukf::SqrtUkf;
pub use ukf::Ukf;
pub use ukf::UkfParams;

Modules§

autodiff
Automatic Jacobians using forward-mode dual numbers.
diagnostics
NIS and NEES consistency diagnostics.
ekf
Extended Kalman filter with user-supplied or automatic Jacobians.
error
Error type returned by filter operations.
linear
Linear Kalman filter with the Joseph-form covariance update.
model
Traits that describe how the state evolves and how it is measured.
scalar
The scalar type that filters are generic over.
smoother
Rauch-Tung-Striebel smoother.
sqrt_kf
Square-root linear Kalman filter.
sqrt_ukf
Square-root unscented Kalman filter.
ukf
Unscented Kalman filter.