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

#[cfg(test)]
mod tests {
    use nalgebra::{VectorN, MatrixN, MatrixMN, U0, U1, U2};

    use crate::filter::Filter;

    #[test]
    fn it_works() {
        assert_eq!(2 + 2, 4);
    }

    #[test]
    fn filter_test(){
        let mut filter = Filter::new(
            VectorN::<f32, U2>::new(-1.0, 9.0),//x
            MatrixN::<f32, U2>::new(//p
                0.1, 0.1,
                0.1, 0.1
            ),
            MatrixN::<f32, U2>::new(//f
                1.0, 0.1,
                0.0, 1.0
            ),
            MatrixN::<f32, U2>::new(//r
                0.1, 0.1,
                0.1, 1.0
            ),
            MatrixN::<f32, U2>::new(//h
                1.0, 0.0,
                0.0, 1.0
            ),
            MatrixN::<f32, U2>::new(//q
                1.0, 1.0,
                1.0, 1.0
            ),
        );
        let mut measurement = VectorN::<f32, U2>::new(0.0, 9.0);
        for i in 0..100{
            measurement[0] = i as f32;
            let(x, p) = filter.run(measurement);
            println!("{}, {}", x[0], x[1])
        }
    }
}