Crate minikalman

Expand description

§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).