minikalman 0.7.0

A microcontroller targeted Kalman filter implementation
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134

use crate::extended::builder as extended;
use crate::prelude::*;
use crate::regular::builder as regular;

pub struct TestFilter {
    pub filter: regular::KalmanFilterType<3, f32>,
    pub control: regular::KalmanFilterControlType<3, 1, f32>,
    pub measurement: regular::KalmanFilterObservationType<3, 2, f32>,
}

pub struct ExtendedTestFilter {
    pub filter: extended::KalmanFilterType<3, f32>,
    pub measurement: extended::KalmanFilterObservationType<3, 2, f32>,
}

#[allow(unused)]
pub fn create_test_filter(delta_t: f32) -> TestFilter {
    let builder = regular::KalmanFilterBuilder::<3, f32>::default();
    let mut filter = builder.build();

    let controls = builder.controls();
    let mut control = controls.build::<1>();

    let measurements = builder.observations();
    let mut measurement = measurements.build::<2>();

    // Simple model of linear motion.
    filter.state_transition_mut().apply(|mat| {
        mat.set_at(0, 0, 1.0);
        mat.set_at(0, 1, delta_t);
        mat.set_at(0, 2, 0.5 * delta_t * delta_t);

        mat.set_at(1, 0, 0.0);
        mat.set_at(1, 1, 1.0);
        mat.set_at(1, 2, delta_t);

        mat.set_at(2, 0, 0.0);
        mat.set_at(2, 1, 0.0);
        mat.set_at(2, 2, 1.0);
    });

    // No state estimate so far.
    filter.state_vector_mut().set_zero();

    // Estimate covariance is identity.
    filter.estimate_covariance_mut().make_scalar(0.1);

    // Control input directly affects the acceleration and, over the time step,
    // the velocity and position.
    control.control_matrix_mut().apply(|mat| {
        mat.set_at(0, 0, 0.5 * delta_t * delta_t);
        mat.set_at(1, 0, delta_t);
        mat.set_at(2, 0, 1.0);
    });

    // No control inputs now.
    control.control_vector_mut().set_zero();

    // Process noise is almost-identity.
    control
        .process_noise_covariance_mut()
        .make_comatrix(1.0, 0.1);

    // The measurement is both directly observing position and an average of the states.
    measurement.observation_matrix_mut().set_all(1.0 / 3.0);
    measurement.observation_matrix_mut().apply(|mat| {
        mat.set_at(0, 0, 1.0); // measure position directly
        mat.set_at(0, 1, 0.0);
        mat.set_at(0, 2, 0.0);
    });

    // Measurement noise covariance is identity.
    measurement
        .measurement_noise_covariance_mut()
        .make_identity();

    TestFilter {
        filter,
        control,
        measurement,
    }
}

#[allow(unused)]
pub fn create_test_filter_ekf(delta_t: f32) -> ExtendedTestFilter {
    let builder = extended::KalmanFilterBuilder::<3, f32>::default();
    let mut filter = builder.build();

    let measurements = builder.observations();
    let mut measurement = measurements.build::<2>();

    // Simple model of linear motion.
    filter.state_transition_jacobian_mut().apply(|mat| {
        // TODO: Add meaningful Jacobian.
        mat.set_at(0, 0, 1.0);
        mat.set_at(0, 1, delta_t);
        mat.set_at(0, 2, 0.5 * delta_t * delta_t);

        mat.set_at(1, 0, 0.0);
        mat.set_at(1, 1, 1.0);
        mat.set_at(1, 2, delta_t);

        mat.set_at(2, 0, 0.0);
        mat.set_at(2, 1, 0.0);
        mat.set_at(2, 2, 1.0);
    });

    // No state estimate so far.
    filter.state_vector_mut().set_zero();

    // Estimate covariance is identity.
    filter.estimate_covariance_mut().make_scalar(0.1);

    // The measurement is both directly observing position and an average of the states.
    measurement
        .observation_jacobian_matrix_mut()
        .set_all(1.0 / 3.0);
    measurement.observation_jacobian_matrix_mut().apply(|mat| {
        // TODO: Add meaningful Jacobian.
        mat.set_at(0, 0, 1.0); // measure position directly
        mat.set_at(0, 1, 0.0);
        mat.set_at(0, 2, 0.0);
    });

    // Measurement noise covariance is identity.
    measurement
        .measurement_noise_covariance_mut()
        .make_identity();

    ExtendedTestFilter {
        filter,
        measurement,
    }
}