Crate minikalman
source ·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 theno_std
configuration attribute (enablingstd
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 withunsafe
.
§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§
- Extended Kalman Filters (EKF)
- Exports all macros and common types.
- Regular Kalman Filters
Macros§
- Creates a static buffer fitting the square state transition matrix A (
num_states
×num_states
). - Creates a static buffer fitting the control matrix B (
num_states
×num_controls
). - Creates a static buffer fitting the observation matrix H (
num_measurements
×num_states
). - Creates a static buffer fitting the Kalman gain matrix (
num_states
×num_measurements
). - Creates a static buffer fitting the square estimate covariance matrix P (
num_states
×num_states
). - Creates a static buffer fitting the square control process noise covariance matrix Q (
num_controls
×num_controls
). - Creates a static buffer fitting the square direct process noise covariance matrix Q (
num_states
×num_states
). - Creates a static buffer fitting the square measurement noise covariance matrix (
num_measurements
×num_measurements
). - Creates a static buffer fitting the square innovation (residual) covariance matrix S (
num_measurements
×num_measurements
). - Creates a static buffer fitting the temporary B×Q matrix (
num_states
×num_controls
). - Creates a static buffer fitting the temporary H×P matrix (
num_measurements
×num_states
). - Creates a buffer fitting the temporary K×(H×P) buffer (
num_states
×num_states
). - Creates a static buffer fitting the square temporary P matrix (
num_states
×num_states
). - Creates a buffer fitting the temporary P×Hᵀ buffer (
num_states
×num_measurements
). - Creates a static buffer fitting the square temporary S-inverted (
num_measurements
×num_measurements
). - Creates a static buffer fitting the temporary x predictions (
num_states
×1
). - Sizes a static buffer fitting the control vector u (
num_controls
×1
). - Creates a static buffer fitting the state vector x (
num_states
×1
). - Creates a static buffer fitting the innovation (or measurement residual) vector y (
num_measurements
×1
). - Creates a static buffer fitting the measurement vector z (
num_measurements
×1
). - Sizes a buffer fitting the state transition matrix (
num_states
×num_states
). - Sizes a buffer fitting the control transition matrix (
num_states
×num_controls
). - Sizes a buffer fitting the measurement transformation matrix (
num_measurements
×num_states
). - Sizes a buffer fitting the Kalman gain matrix (
num_states
×num_measurements
). - Sizes a buffer fitting the state covariance matrix (
num_states
×num_states
). - Sizes a buffer fitting the control process noise matrix (
num_controls
×num_controls
). - Sizes a buffer fitting the direct process noise matrix (
num_states
×num_states
). - Sizes a buffer fitting the measurement uncertainty matrix (
num_measurements
×num_measurements
). - Sizes a buffer fitting the innovation covariance matrix (
num_measurements
×num_measurements
). - Sizes a buffer fitting the temporary B×Q matrix (
num_states
×num_controls
). - Sizes a buffer fitting the temporary H×P matrix (
num_measurements
×num_states
). - Sizes a buffer fitting the temporary K×(H×P) buffer (
num_states
×num_states
). - Sizes a buffer fitting the temporary P matrix (
num_states
×num_states
). - Sizes a buffer fitting the temporary P×H^-1 buffer (
num_states
×num_measurements
). - Sizes a buffer fitting the temporary S-inverted (
num_measurements
×num_measurements
). - Sizes a buffer fitting the temporary x predictions (
num_states
×1
). - Sizes a buffer fitting the control vector (
num_controls
×1
). - Sizes a buffer fitting the state vector (
num_states
×1
). - Sizes a buffer fitting the innovation vector (
num_measurements
×1
). - Sizes a buffer fitting the measurement vector z (
num_measurements
×1
).