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


use nalgebra::{VectorN, DimName, MatrixMN, MatrixN, RealField, Dim, allocator::Allocator, DefaultAllocator};


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}
    }

    pub fn run(&mut self, z: VectorN<T, DimZ>) -> (VectorN<T, DimX>, MatrixN<T, DimX>){
        //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 k = &p * &self.h.transpose() * s.try_inverse().unwrap();
        let y = z - &self.h * &x;
        self.x = x + &k * y;
        self.p = &p - k * &self.h * &p;
        (self.x.clone(), self.p.clone())
    }
}
pub struct Filter2<T, DimZ, DimX, DimU>
where
    T: RealField,
    DimZ: Dim + DimName,
    DimX: Dim + DimName,
    DimU: Dim + DimName,
    DefaultAllocator: Allocator<T, DimX>
        + Allocator<T, DimX, DimX>
        + Allocator<T, DimZ, DimX>
        + Allocator<T, DimZ, DimZ>
        + Allocator<T, DimX, DimU>
{
    ///state estimate vector
    x: VectorN<T, DimX>,
    ///state covariance matrix
    p: MatrixN<T, DimX>,
    ///system model
    f: MatrixN<T, DimX>,
    ///control function
    b: MatrixMN<T, DimX, DimU>,
    ///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, DimU> Filter2<T, DimZ, DimX, DimU>
where
    T: RealField,
    DimZ: Dim + DimName,
    DimX: Dim + DimName,
    DimU: Dim + DimName,
    DefaultAllocator: Allocator<T, DimX>
        + Allocator<T, DimZ>
        + Allocator<T, DimU>
        + Allocator<T, DimX, DimX>
        + Allocator<T, DimX, DimZ>
        + Allocator<T, DimZ, DimX>
        + Allocator<T, DimZ, DimZ>
        + Allocator<T, DimX, DimU>
{
    pub fn new(x: VectorN<T, DimX>, p: MatrixN<T, DimX>, f: MatrixN<T, DimX>, b: MatrixMN<T, DimX, DimU>, r: MatrixN<T, DimZ>, h: MatrixMN<T, DimZ, DimX>, q: MatrixN<T, DimX>) ->  Filter2<T, DimZ, DimX, DimU>{
        Filter2{x, p, f, b, r, h, q}
    }

    pub fn run(&mut self, z: VectorN<T, DimZ>, u: VectorN<T, DimU>) -> (VectorN<T, DimX>, MatrixN<T, DimX>){
        //predict
        let x = &self.f * &self.x + &self.b * u;
        let p = &self.f * &self.p * &self.f.transpose() + &self.q;
        //update
        let s = &self.h * &p * &self.h.transpose() + &self.r;
        let k = &p * &self.h.transpose() * s.try_inverse().unwrap();
        let y = z - &self.h * &x;
        self.x = x + &k * y;
        self.p = &p - k * &self.h * &p;
        (self.x.clone(), self.p.clone())
    }
}