yakf 0.1.12

Yet Another Kalman Filter Implementation, as well as, Lie Theory (Lie group, algebra, vector) on SO(2), SO(3), SE(2) and SE(3). [no_std] is supported by default.
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

#[cfg(test)]
mod tests {

    use crate::lie::so3::{sosekf::SOEKF, Alg3, Grp3, One2OneMapSO3, Vec3, SO3};

    use crate::linalg::{Const, OMatrix, OVector, U2, U3};
    /// import Re-exports of hifitime (for time) and nalgebra (for matrix)
    use crate::{
        linalg,
        time::{Duration, Epoch, Unit},
    };
    #[test]
    fn vec_to_alg() {
        let v = Vec3::new(1.0, 2.0, 3.0);
        let alg = SO3::from_vec(v).to_alg();
        let v2 = SO3::from_alg(alg).to_vec();
        assert!((v - v2).norm() < 1e-10);
    }

    #[test]
    fn vec_to_grp() {
        let vectors = [
            Vec3::new(0.0, 0.5, 0.25),
            Vec3::new(-1.0, 20.0, 33.0),
            Vec3::new(1e-10, -1e-8, 1e-9),
            10.0 * Vec3::new(1.0, 0.0, 0.0),
            100.0 * Vec3::new(0.0, 0.0, 1.0),
        ];

        for v in vectors.into_iter() {
            let grp = SO3::from_vec(v).to_grp();
            let v2 = SO3::from_grp(grp).to_vec();

            let grp2 = SO3::from_vec(v2).to_grp();
            assert!((grp - grp2).norm() < 1e-6);
        }
    }

    #[test]
    fn so3ekf() {
        use core::f64::consts::PI;
        use rand::Rng;

        let state = SO3::from_vec(Vec3::new(0.3, 0.6, -0.9));
        let pmatrix = OMatrix::<f64, U3, U3>::from_diagonal_element(1e-2);
        let qmatrix = OMatrix::<f64, U3, U3>::from_diagonal_element(1e-2);
        let nmatrix = OMatrix::<f64, Const<12>, Const<12>>::from_diagonal_element(1e-4);

        let mut soekf = SOEKF::build(state, pmatrix, qmatrix, nmatrix);

        let nums = 2000;
        let bk = [
            OVector::<f64, U3>::new(10.0, 20.0, 10.0),
            OVector::<f64, U3>::new(-50.0, 30.0, 10.0),
            OVector::<f64, U3>::new(50.0, 20.0, -10.0),
            OVector::<f64, U3>::new(-20.0, -10.0, -5.0),
        ];

        let true_pose_and_u = |dur: Duration| {
            let omega = 0.3;

            let theta = omega * dur.in_seconds();

            let e1 = OVector::<f64, U3>::new(theta.cos(), theta.sin(), 0.0);
            let e2 =
                OVector::<f64, U3>::new((theta + PI / 2.0).cos(), (theta + PI / 2.0).sin(), 0.0);
            let e3 = OVector::<f64, U3>::new(0.0, 0.0, 1.0);
            let r = OMatrix::<f64, U3, U3>::from_columns(&[e1, e2, e3]);
            let x = SO3::from_grp(r);

            let u = OVector::<f64, U3>::new(0.0, 0.0, omega);

            (x, u)
        };
        pub fn measure(x_predict: &SO3, bk: &[Vec3; 4]) -> OVector<f64, Const<12>> {
            let mut ob = OVector::<f64, Const<12>>::zeros();
            let x_inv = x_predict.inverse();
            for i in 0..4 {
                let block = x_inv.act_v(bk[i]);
                ob.index_mut((i * 3..i * 3 + 3, 0..1)).copy_from(&block);
            }
            ob
        }

        let mut rng = rand::thread_rng();
        let mut add_noisies = |mut y: OVector<f64, Const<12>>| {
            for i in 0..12 {
                y[i] += rng.gen_range(-1e-2..1e-2);
            }
            y
        };

        let mut dur = Duration::from_f64(0.0, Unit::Second);
        let dt = Duration::from_f64(0.1, Unit::Second);
        let mut error: f64 = 1.0;
        for i in 0..nums {
            dur = dur + dt;

            let (true_state, true_u_b) = true_pose_and_u(dur);
            let meas_ob = {
                let mut true_ob = measure(&true_state, &bk);
                let meas_ob = add_noisies(true_ob);
                meas_ob
            };

            // every time the measurement is ready, ekf is trigger to update.
            soekf.feed_and_update(meas_ob, dt, true_u_b, &bk).unwrap();

            let estimate_state = soekf.state;
            let error_state = estimate_state.minus_r(true_state);
            error = error_state.norm();
        }

        assert!(error < 1e-2);
    }
}