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

use fehler::{throw, throws};
use nalgebra::{
    allocator::Allocator, DefaultAllocator, Dim, DimName, MatrixMN, MatrixN, RealField, VectorN,
};
use thiserror::Error;

#[derive(Error, Debug)]
pub enum KfError {
    #[error("Matrix inversion failed")]
    InverseError,
}

pub struct Filter<T, DimZ, DimX>
where
    T: RealField,
    DimZ: Dim + DimName,
    DimX: Dim + DimName,
    DefaultAllocator: Allocator<T, DimX>
        + Allocator<T, DimX, DimX>
        + Allocator<T, DimZ, DimX>
        + Allocator<T, DimZ, DimZ>,
{
    ///state estimate vector
    x: VectorN<T, DimX>,
    ///state covariance matrix
    p: MatrixN<T, DimX>,
    ///system model
    f: MatrixN<T, DimX>,
    ///measurement noise covariance matrix
    r: MatrixN<T, DimZ>,
    ///measurement function
    h: MatrixMN<T, DimZ, DimX>,
    ///process noise covariance matrix
    q: MatrixN<T, DimX>,
}

impl<T, DimZ, DimX> Filter<T, DimZ, DimX>
where
    T: RealField,
    DimZ: Dim + DimName,
    DimX: Dim + DimName,
    DefaultAllocator: Allocator<T, DimX>
        + Allocator<T, DimZ>
        + Allocator<T, DimX, DimX>
        + Allocator<T, DimX, DimZ>
        + Allocator<T, DimZ, DimX>
        + Allocator<T, DimZ, DimZ>,
{
    pub fn new(
        x: VectorN<T, DimX>,
        p: MatrixN<T, DimX>,
        f: MatrixN<T, DimX>,
        r: MatrixN<T, DimZ>,
        h: MatrixMN<T, DimZ, DimX>,
        q: MatrixN<T, DimX>,
    ) -> Filter<T, DimZ, DimX> {
        Filter { x, p, f, r, h, q }
    }

    #[throws(KfError)]
    pub fn run(&mut self, z: VectorN<T, DimZ>) -> (VectorN<T, DimX>, MatrixN<T, DimX>, VectorN<T, DimZ>, MatrixN<T, DimZ>, MatrixN<T, DimZ>) {
        //predict
        let x = &self.f * &self.x;
        let p = &self.f * &self.p * &self.f.transpose() + &self.q;
        //update
        let s = &self.h * &p * &self.h.transpose() + &self.r;
        let s_inverse = match s.clone().try_inverse() {
            Some(m) => m,
            None => throw!(KfError::InverseError),
        };
        let k = &p * &self.h.transpose() * &s_inverse;
        let y = z - &self.h * &x;
        self.x = x + &k * &y;
        self.p = &p - k * &self.h * &p;
        (self.x.clone(), self.p.clone(), y, s, s_inverse)
    }

    pub fn set_p(&mut self, p: MatrixN<T, DimX>){
        self.p = p;
    }

    pub fn set_x(&mut self, x: VectorN<T, DimX>){
        self.x = x;
    }
}