Crate minikalman

Kalman Filters for Embedded Targets

This is the Rust port of the kalman-clib library, a microcontroller targeted Kalman filter implementation. It can use micromath for square root and reciprocal calculations on no_std; libm is supported as well.

This implementation uses statically allocated buffers for all matrix operations. Due to lack of const generics for array allocations in Rust, this crate also provides helper macros to create the required arrays (see e.g. impl_buffer_A).

If allocation is available (via std or alloc crate features), the KalmanFilterBuilder can be used to quickly create a RegularKalman filter instance with all necessary buffers, alongside Control and Observation instances. Similar types exist for Extended Kalman Filters.

Crate Features

• std - Disabled by default. Disables the no_std configuration attribute (enabling std support).
• alloc - Enables allocation support for builder types.
• libm - Enables libm support.
• micromath - Enables micromath support.
• fixed - Enables fixed-point support via the fixed crate.
• unsafe - Enables some unsafe pointer operations. Disabled by default; when turned off, compiles the crate as #![forbid(unsafe)].
• nalgebra - Enables nalgebra support. For major parts, must be used in conjunction with unsafe.

Example

On std or alloc crates, the KalmanFilterBuilder is enabled. An overly simplified example for setting up and operating the Kalman Filter could look like this:

use minikalman::regular::builder::KalmanFilterBuilder;
use minikalman::prelude::MatrixMut;

const NUM_STATES: usize = 3;
const NUM_CONTROLS: usize = 2;
const NUM_OBSERVATIONS: usize = 1;

let builder = KalmanFilterBuilder::<NUM_STATES, f32>::default();
let mut filter = builder.build();
let mut control = builder.controls().build::<NUM_CONTROLS>();
let mut measurement = builder.observations().build::<NUM_OBSERVATIONS>();

// Set up the system dynamics, control matrices, observation matrices, ...

// Filter!
loop {
    // Update your control vector(s).
    control.control_vector_mut().apply(|u| {
        u[0] = 0.0;
        u[1] = 1.0;
    });

    // Update your measurement vectors.
    measurement.measurement_vector_mut().apply(|z| {
        z[0] = 42.0;
    });

    // Update prediction (without controls).
    filter.predict();

    // Apply any controls to the prediction.
    filter.control(&mut control);

    // Apply any measurements.
    filter.correct(&mut measurement);

    // Access the state
    let state = filter.state_vector();
    let covariance = filter.estimate_covariance();
}

Extended Kalman Filters

The general setup remains the same, however the predict and correct methods are replaced with their nonlinear counterparts:

use minikalman::extended::builder::KalmanFilterBuilder;
use minikalman::prelude::MatrixMut;

const NUM_STATES: usize = 3;
const NUM_CONTROLS: usize = 2;
const NUM_OBSERVATIONS: usize = 1;

let builder = KalmanFilterBuilder::<NUM_STATES, f32>::default();
let mut filter = builder.build();
let mut measurement = builder.observations().build::<NUM_OBSERVATIONS>();

// Set up the system dynamics, control matrices, observation matrices, ...

// Filter!
loop {
    // Obtain the control values.
    let control_value = 1.0;

    // Update prediction using nonlinear transfer function.
    filter.predict_nonlinear(|current, next| {
        next[0] = current[0] * current[0];
        next[1] = current[1].sin() * control_value;
    });

    // Update your measurement vectors.
    measurement.measurement_vector_mut().apply(|z| {
        z[0] = 42.0;
    });

    // Apply any measurements using a nonlinear measurement function.
    filter.correct_nonlinear(&mut measurement, |state, observation| {
        observation[0] = state[0].cos() + state[1].sin();
    });

    // Access the state
    let state = filter.state_vector();
    let covariance = filter.estimate_covariance();
}

no_std Example

Systems without the liberty of heap allocation may make use of the provided helper macros to wire up new types. This comes at the cost of potentially confusing IDEs due to recursive macro expansion, so buyer beware. In the example below, types are set up as arrays bound to static mut variables.

use minikalman::buffers::types::*;
use minikalman::prelude::*;
use minikalman::regular::{RegularKalmanBuilder, RegularObservationBuilder};

const NUM_STATES: usize = 3;
const NUM_OBSERVATIONS: usize = 1;

// System buffers.
impl_buffer_x!(static mut gravity_x, NUM_STATES, f32, 0.0);
impl_buffer_A!(static mut gravity_A, NUM_STATES, f32, 0.0);
impl_buffer_P!(static mut gravity_P, NUM_STATES, f32, 0.0);
impl_buffer_Q_direct!(static mut gravity_Q, NUM_STATES, f32, 0.0);

// Observation buffers.
impl_buffer_z!(static mut gravity_z, NUM_OBSERVATIONS, f32, 0.0);
impl_buffer_H!(static mut gravity_H, NUM_OBSERVATIONS, NUM_STATES, f32, 0.0);
impl_buffer_R!(static mut gravity_R, NUM_OBSERVATIONS, f32, 0.0);
impl_buffer_y!(static mut gravity_y, NUM_OBSERVATIONS, f32, 0.0);
impl_buffer_S!(static mut gravity_S, NUM_OBSERVATIONS, f32, 0.0);
impl_buffer_K!(static mut gravity_K, NUM_STATES, NUM_OBSERVATIONS, f32, 0.0);

// Filter temporaries.
impl_buffer_temp_x!(static mut gravity_temp_x, NUM_STATES, f32, 0.0);
impl_buffer_temp_P!(static mut gravity_temp_P, NUM_STATES, f32, 0.0);

// Observation temporaries.
impl_buffer_temp_S_inv!(static mut gravity_temp_S_inv, NUM_OBSERVATIONS, f32, 0.0);

// Observation temporaries.
impl_buffer_temp_HP!(static mut gravity_temp_HP, NUM_OBSERVATIONS, NUM_STATES, f32, 0.0);
impl_buffer_temp_PHt!(static mut gravity_temp_PHt, NUM_STATES, NUM_OBSERVATIONS, f32, 0.0);
impl_buffer_temp_KHP!(static mut gravity_temp_KHP, NUM_STATES, f32, 0.0);

let mut filter = RegularKalmanBuilder::new::<NUM_STATES, f32>(
    StateTransitionMatrixMutBuffer::from(unsafe { gravity_A.as_mut_slice() }),
    StateVectorBuffer::from(unsafe { gravity_x.as_mut_slice() }),
    EstimateCovarianceMatrixBuffer::from(unsafe { gravity_P.as_mut_slice() }),
    DirectProcessNoiseCovarianceMatrixBuffer::from(unsafe { gravity_Q.as_mut_slice() }),
    PredictedStateEstimateVectorBuffer::from(unsafe { gravity_temp_x.as_mut_slice() }),
    TemporaryStateMatrixBuffer::from(unsafe { gravity_temp_P.as_mut_slice() }),
);

let mut measurement = RegularObservationBuilder::new::<NUM_STATES, NUM_OBSERVATIONS, f32>(
    ObservationMatrixMutBuffer::from(unsafe { gravity_H.as_mut_slice() }),
    MeasurementVectorBuffer::from(unsafe { gravity_z.as_mut_slice() }),
    MeasurementNoiseCovarianceMatrixBuffer::from(unsafe { gravity_R.as_mut_slice() }),
    InnovationVectorBuffer::from(unsafe { gravity_y.as_mut_slice() }),
    InnovationCovarianceMatrixBuffer::from(unsafe { gravity_S.as_mut_slice() }),
    KalmanGainMatrixBuffer::from(unsafe { gravity_K.as_mut_slice() }),
    TemporaryResidualCovarianceInvertedMatrixBuffer::from(unsafe {
        gravity_temp_S_inv.as_mut_slice()
    }),
    TemporaryHPMatrixBuffer::from(unsafe { gravity_temp_HP.as_mut_slice() }),
    TemporaryPHTMatrixBuffer::from(unsafe { gravity_temp_PHt.as_mut_slice() }),
    TemporaryKHPMatrixBuffer::from(unsafe { gravity_temp_KHP.as_mut_slice() }),
);

After that, the filter and measurement variables can be used similar to the example above.

Re-exports

• pub use crate::buffers::builder::BufferBuilder;
  alloc
• pub use num_traits;

Modules

• buffers
• extended
  Extended Kalman Filters (EKF)
• matrix
• prelude
  Exports all macros and common types.
• regular
  Regular Kalman Filters

Macros

• impl_buffer_A
  Creates a static buffer fitting the square state transition matrix A (num_states × num_states).
• impl_buffer_B
  Creates a static buffer fitting the control matrix B (num_states × num_controls).
• impl_buffer_H
  Creates a static buffer fitting the observation matrix H (num_measurements × num_states).
• impl_buffer_K
  Creates a static buffer fitting the Kalman gain matrix (num_states × num_measurements).
• impl_buffer_P
  Creates a static buffer fitting the square estimate covariance matrix P (num_states × num_states).
• impl_buffer_Q_control
  Creates a static buffer fitting the square control process noise covariance matrix Q (num_controls × num_controls).
• impl_buffer_Q_direct
  Creates a static buffer fitting the square direct process noise covariance matrix Q (num_states × num_states).
• impl_buffer_R
  Creates a static buffer fitting the square measurement noise covariance matrix (num_measurements × num_measurements).
• impl_buffer_S
  Creates a static buffer fitting the square innovation (residual) covariance matrix S (num_measurements × num_measurements).
• impl_buffer_temp_BQ
  Creates a static buffer fitting the temporary B×Q matrix (num_states × num_controls).
• impl_buffer_temp_HP
  Creates a static buffer fitting the temporary H×P matrix (num_measurements × num_states).
• impl_buffer_temp_KHP
  Creates a buffer fitting the temporary K×(H×P) buffer (num_states × num_states).
• impl_buffer_temp_P
  Creates a static buffer fitting the square temporary P matrix (num_states × num_states).
• impl_buffer_temp_PHt
  Creates a buffer fitting the temporary P×Hᵀ buffer (num_states × num_measurements).
• impl_buffer_temp_S_inv
  Creates a static buffer fitting the square temporary S-inverted (num_measurements × num_measurements).
• impl_buffer_temp_x
  Creates a static buffer fitting the temporary x predictions (num_states × 1).
• impl_buffer_u
  Sizes a static buffer fitting the control vector u (num_controls × 1).
• impl_buffer_x
  Creates a static buffer fitting the state vector x (num_states × 1).
• impl_buffer_y
  Creates a static buffer fitting the innovation (or measurement residual) vector y (num_measurements × 1).
• impl_buffer_z
  Creates a static buffer fitting the measurement vector z (num_measurements × 1).
• impl_mutable_vec
• size_buffer_A
  Sizes a buffer fitting the state transition matrix (num_states × num_states).
• size_buffer_B
  Sizes a buffer fitting the control transition matrix (num_states × num_controls).
• size_buffer_H
  Sizes a buffer fitting the measurement transformation matrix (num_measurements × num_states).
• size_buffer_K
  Sizes a buffer fitting the Kalman gain matrix (num_states × num_measurements).
• size_buffer_P
  Sizes a buffer fitting the state covariance matrix (num_states × num_states).
• size_buffer_Q_control
  Sizes a buffer fitting the control process noise matrix (num_controls × num_controls).
• size_buffer_Q_direct
  Sizes a buffer fitting the direct process noise matrix (num_states × num_states).
• size_buffer_R
  Sizes a buffer fitting the measurement uncertainty matrix (num_measurements × num_measurements).
• size_buffer_S
  Sizes a buffer fitting the innovation covariance matrix (num_measurements × num_measurements).
• size_buffer_temp_BQ
  Sizes a buffer fitting the temporary B×Q matrix (num_states × num_controls).
• size_buffer_temp_HP
  Sizes a buffer fitting the temporary H×P matrix (num_measurements × num_states).
• size_buffer_temp_KHP
  Sizes a buffer fitting the temporary K×(H×P) buffer (num_states × num_states).
• size_buffer_temp_P
  Sizes a buffer fitting the temporary P matrix (num_states × num_states).
• size_buffer_temp_PHt
  Sizes a buffer fitting the temporary P×H^-1 buffer (num_states × num_measurements).
• size_buffer_temp_S_inv
  Sizes a buffer fitting the temporary S-inverted (num_measurements × num_measurements).
• size_buffer_temp_x
  Sizes a buffer fitting the temporary x predictions (num_states × 1).
• size_buffer_u
  Sizes a buffer fitting the control vector (num_controls × 1).
• size_buffer_x
  Sizes a buffer fitting the state vector (num_states × 1).
• size_buffer_y
  Sizes a buffer fitting the innovation vector (num_measurements × 1).
• size_buffer_z
  Sizes a buffer fitting the measurement vector z (num_measurements × 1).