Struct rudie::KalmanFilter

source · 
pub struct KalmanFilter<N, DP, MP, CP>where
    N: Real,
    DP: DimName,
    MP: DimName,
    CP: DimName,
    <DP as DimName>::Value: Mul<U1>,
    <<DP as DimName>::Value as Mul<U1>>::Output: ArrayLength<N>,
    <DP as DimName>::Value: Mul,
    <<DP as DimName>::Value as Mul>::Output: ArrayLength<N>,
    <MP as DimName>::Value: Mul<<DP as DimName>::Value>,
    <<MP as DimName>::Value as Mul<<DP as DimName>::Value>>::Output: ArrayLength<N>,
    <MP as DimName>::Value: Mul,
    <<MP as DimName>::Value as Mul>::Output: ArrayLength<N>,
    <DP as DimName>::Value: Mul<<CP as DimName>::Value>,
    <<DP as DimName>::Value as Mul<<CP as DimName>::Value>>::Output: ArrayLength<N>,
    <DP as DimName>::Value: Mul<<MP as DimName>::Value>,
    <<DP as DimName>::Value as Mul<<MP as DimName>::Value>>::Output: ArrayLength<N>,
    <MP as DimName>::Value: Mul<U1>,
    <<MP as DimName>::Value as Mul<U1>>::Output: ArrayLength<N>,{
    pub state_pre: Vector<N, DP, MatrixArray<N, DP, U1>>,
    pub state_post: Vector<N, DP, MatrixArray<N, DP, U1>>,
    pub transition_matrix: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>,
    pub process_noise_cov: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>,
    pub measurement_matrix: Matrix<N, MP, DP, MatrixArray<N, MP, DP>>,
    pub measurement_noise_cov: Matrix<N, MP, MP, MatrixArray<N, MP, MP>>,
    pub control_matrix: Matrix<N, DP, CP, MatrixArray<N, DP, CP>>,
    pub error_cov_pre: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>,
    pub error_cov_post: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>,
    pub gain: Matrix<N, DP, MP, MatrixArray<N, DP, MP>>,
    pub residual: Vector<N, MP, MatrixArray<N, MP, U1>>,
    pub innov_cov: Matrix<N, MP, MP, MatrixArray<N, MP, MP>>,
}
Expand description

A Rust implementation of the OpenCV Kalman Filter

Because Rust currently doesn’t have const generics, we make heavy use of the typenum to specify type-level numerics that are used when interacting with the library.

rudie can be used in #![no_std] mode for embedded applications with no OS since we make use of only #![no_std] compatible libraries

Examples

The following example shows a Kalman filter estimating the orientation and rotational rate of a rotating point. An example is given further down showing that the kalman filter is correct by examining its internal state.

extern crate rudie;
extern crate rand;
extern crate libm;
extern crate assert as asrt;

use rand::{Rng, StdRng, SeedableRng};
use rand::distributions::{Distribution, Normal};
use libm::F64Ext;
use asrt::close;

use rudie::KalmanFilter;
use rudie::na::{U0, U1, U2, Vector, Matrix, MatrixArray};

// seed the rng so we get reproducible results
let seed: [u8; 32] = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
                     1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
                     1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
                     1, 2];
let mut state_rng: StdRng = SeedableRng::from_seed(seed);
let state_generator = Normal::new(0., 0.1);

// We use the typenum crate here to specify type-level numbers such as U0 = 0, U1 = 1, ...
// until Rust has support for const generics
let mut kf: KalmanFilter<f64, U2, U1, U0> = KalmanFilter::init();

// (phi, delta_phi), i.e. orientation and angular rate
let mut state: Vector<f64, U2, MatrixArray<f64, U2, U1>>;

let mut process_noise: Vector<f64, U2, MatrixArray<f64, U2, U1>>;

let mut measurement: Vector<f64, U1, MatrixArray<f64, U1, U1>>;

state = rudie::na::Matrix2x1::new(
    state_generator.sample(&mut state_rng),
    state_generator.sample(&mut state_rng)
);
kf.transition_matrix = rudie::na::Matrix2::new(
    1., 1.,
    0., 1.
);

kf.measurement_matrix = rudie::na::Matrix1x2::identity();
kf.process_noise_cov = rudie::na::Matrix2::from_diagonal_element(1e-5);
kf.measurement_noise_cov = rudie::na::Matrix1::from_diagonal_element(1e-1);
kf.error_cov_post = rudie::na::Matrix2::from_diagonal_element(1.);

let measurement_noise_generator = Normal::new(0., kf.measurement_noise_cov[(0)]);
let mut measurement_noise_rng: StdRng = SeedableRng::from_seed(seed);
let process_noise_generator = Normal::new(0., kf.process_noise_cov[(0)].sqrt());
let mut process_noise_rng: StdRng = SeedableRng::from_seed(seed);

// set up the initial state
kf.state_post = rudie::na::Matrix2x1::new(
    -0.13210335109501573,
    -0.06167960574363984
);

for cycle in 0..3 {
    // Kalman predict
    kf.predict_no_control();

    // generate measurement
    measurement = rudie::na::Matrix1::new(
        measurement_noise_generator.sample(&mut measurement_noise_rng)
    );
    measurement += kf.measurement_matrix*state;

    // Kalman correct
    kf.correct(measurement);

    // generate next state
    process_noise = rudie::na::Matrix2x1::new(
        process_noise_generator.sample(&mut process_noise_rng),
        process_noise_generator.sample(&mut process_noise_rng)
    );
    state = kf.transition_matrix * state + process_noise;
}

The following example shows a Kalman filter estimating the orientation and rotational rate of a rotating point. Assertions are included to ensure the example code does not regress.

extern crate rudie;
extern crate rand;
extern crate libm;
extern crate assert as asrt;

use rand::{Rng, StdRng, SeedableRng};
use rand::distributions::{Distribution, Normal};
use libm::F64Ext;
use asrt::close;

use rudie::KalmanFilter;
use rudie::na::{U0, U1, U2, Vector, Matrix, MatrixArray};

/**************************************************
** filter configurations to assert against - begin
***************************************************/

// known filter configurations to assert state against for kalman predict
let state_pre_assert_predict = rudie::na::Matrix2x3::new(
    -0.19378295683865557, 0.1361050053334287,   0.04097848073026808,
    -0.06167960574363984, 0.06884248182130964, -0.00403224753640588
);
let error_cov_pre_assert_predict = rudie::na::Matrix2x6::new(
    2.00001, 1.,      0.7143075510116619, 0.57144061223518,   0.3509028377933237, 0.1929981748186918,
    1.,      1.00001, 0.57144061223518,   0.5238317913724221, 0.1929981748186918, 0.1228331394128128
);

// known filter configurations to assert state against for kalman correct
let residual_assert_correct = rudie::na::Matrix1x3::new(
    0.27409768910726956, -0.10384708598466963, -0.1451463671400915
);
let innov_cov_assert_correct = rudie::na::Matrix1x3::new(
    2.10001, 0.8143075510116619, 0.45090283779332374
);
let gain_assert_correct = rudie::na::Matrix2x3::new(
    0.9523811791372422, 0.8771962756875286, 0.7782227308894513,
    0.47618820862757794, 0.70175035405879,  0.4280260815460971
);
let state_post_assert_correct = rudie::na::Matrix2x3::new(
    0.06726252351211906, 0.04501072826667396,  -0.07197772148417683,
    0.06884248182130964, -0.00403224753640588, -0.06615867831403044
);
let error_cov_post_assert_correct = rudie::na::Matrix2x6::new(
    0.09523811791372422, 0.04761882086275779, 0.08771962756875286, 0.07017503540587901, 0.07782227308894514, 0.04280260815460972,
    0.04761882086275779, 0.5238217913724221,  0.07017503540587901, 0.12282313941281281, 0.04280260815460972, 0.04022488689961951
);

/**************************************************
** filter configurations to assert against - end
***************************************************/

// seed the rng so we get reproducible results
let seed: [u8; 32] = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
                     1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
                     1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
                     1, 2];
let mut state_rng: StdRng = SeedableRng::from_seed(seed);
let state_generator = Normal::new(0., 0.1);

// We use the typenum crate here to specify type-level numbers such as U0 = 0, U1 = 1, ...
// until Rust has support for const generics
let mut kf: KalmanFilter<f64, U2, U1, U0> = KalmanFilter::init();

// (phi, delta_phi), i.e. orientation and angular rate
let mut state: Vector<f64, U2, MatrixArray<f64, U2, U1>>;

let mut process_noise: Vector<f64, U2, MatrixArray<f64, U2, U1>>;

let mut measurement: Vector<f64, U1, MatrixArray<f64, U1, U1>>;

state = rudie::na::Matrix2x1::new(
    state_generator.sample(&mut state_rng),
    state_generator.sample(&mut state_rng)
);
kf.transition_matrix = rudie::na::Matrix2::new(
    1., 1.,
    0., 1.
);

kf.measurement_matrix = rudie::na::Matrix1x2::identity();
kf.process_noise_cov = rudie::na::Matrix2::from_diagonal_element(1e-5);
kf.measurement_noise_cov = rudie::na::Matrix1::from_diagonal_element(1e-1);
kf.error_cov_post = rudie::na::Matrix2::from_diagonal_element(1.);

let measurement_noise_generator = Normal::new(0., kf.measurement_noise_cov[(0)]);
let mut measurement_noise_rng: StdRng = SeedableRng::from_seed(seed);
let process_noise_generator = Normal::new(0., kf.process_noise_cov[(0)].sqrt());
let mut process_noise_rng: StdRng = SeedableRng::from_seed(seed);

// set up the initial state
kf.state_post = rudie::na::Matrix2x1::new(
    -0.13210335109501573,
    -0.06167960574363984
);

for cycle in 0..3 {
    // Kalman predict
    kf.predict_no_control();

    /********************************************************************
    ** assert known filter configuration after predict_no_control - begin
    *********************************************************************/
    close(kf.state_pre[(0,0)], state_pre_assert_predict[(0,cycle)], core::f64::EPSILON);
    close(kf.state_pre[(1,0)], state_pre_assert_predict[(1,cycle)], core::f64::EPSILON);

    close(kf.error_cov_pre[(0,0)], error_cov_pre_assert_predict[(0,cycle*2)], core::f64::EPSILON);
    close(kf.error_cov_pre[(1,0)], error_cov_pre_assert_predict[(1,cycle*2)], core::f64::EPSILON);
    close(kf.error_cov_pre[(0,1)], error_cov_pre_assert_predict[(0,cycle*2+1)], core::f64::EPSILON);
    close(kf.error_cov_pre[(1,1)], error_cov_pre_assert_predict[(1,cycle*2+1)], core::f64::EPSILON);
    /********************************************************************
    ** assert known filter configuration after predict_no_control - end
    *********************************************************************/

    // generate measurement
    measurement = rudie::na::Matrix1::new(
        measurement_noise_generator.sample(&mut measurement_noise_rng)
    );
    measurement += kf.measurement_matrix*state;

    // Kalman correct
    kf.correct(measurement);

    /*************************************************************
    ** assert known filter configuration after correct - begin
    **************************************************************/
    close(kf.residual[(0,0)], residual_assert_correct[(0, cycle)], core::f64::EPSILON);

    close(kf.innov_cov[(0, 0)], innov_cov_assert_correct[(0, cycle)], core::f64::EPSILON);

    close(kf.gain[(0, 0)], gain_assert_correct[(0, cycle)], core::f64::EPSILON);
    close(kf.gain[(1, 0)], gain_assert_correct[(1, cycle)], core::f64::EPSILON);

    close(kf.state_post[(0,0)], state_post_assert_correct[(0, cycle)], core::f64::EPSILON);
    close(kf.state_post[(1,0)], state_post_assert_correct[(1, cycle)], core::f64::EPSILON);

    close(kf.error_cov_post[(0,0)], error_cov_post_assert_correct[(0, cycle*2)], core::f64::EPSILON);
    close(kf.error_cov_post[(1,0)], error_cov_post_assert_correct[(1, cycle*2)], core::f64::EPSILON);
    close(kf.error_cov_post[(0,1)], error_cov_post_assert_correct[(0, cycle*2+1)], core::f64::EPSILON);
    close(kf.error_cov_post[(1,1)], error_cov_post_assert_correct[(1, cycle*2+1)], core::f64::EPSILON);
    /*************************************************************
    ** assert known filter configuration after correct - end
    **************************************************************/

    // generate next state
    process_noise = rudie::na::Matrix2x1::new(
        process_noise_generator.sample(&mut process_noise_rng),
        process_noise_generator.sample(&mut process_noise_rng)
    );
    state = kf.transition_matrix * state + process_noise;
}

Fields

state_pre: Vector<N, DP, MatrixArray<N, DP, U1>>state_post: Vector<N, DP, MatrixArray<N, DP, U1>>transition_matrix: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>process_noise_cov: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>measurement_matrix: Matrix<N, MP, DP, MatrixArray<N, MP, DP>>measurement_noise_cov: Matrix<N, MP, MP, MatrixArray<N, MP, MP>>control_matrix: Matrix<N, DP, CP, MatrixArray<N, DP, CP>>error_cov_pre: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>error_cov_post: Matrix<N, DP, DP, MatrixArray<N, DP, DP>>gain: Matrix<N, DP, MP, MatrixArray<N, DP, MP>>residual: Vector<N, MP, MatrixArray<N, MP, U1>>innov_cov: Matrix<N, MP, MP, MatrixArray<N, MP, MP>>

Implementations

Trait Implementations

Formats the value using the given formatter. Read more

Auto Trait Implementations

Blanket Implementations

Gets the TypeId of self. Read more
Immutably borrows from an owned value. Read more
Mutably borrows from an owned value. Read more

Returns the argument unchanged.

Calls U::from(self).

That is, this conversion is whatever the implementation of From<T> for U chooses to do.

Should always be Self
The inverse inclusion map: attempts to construct self from the equivalent element of its superset. Read more
Checks if self is actually part of its subset T (and can be converted to it).
Use with care! Same as self.to_subset but without any property checks. Always succeeds.
The inclusion map: converts self to the equivalent element of its superset.
The type returned in the event of a conversion error.
Performs the conversion.
The type returned in the event of a conversion error.
Performs the conversion.