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

use num_traits::{One, Zero};

use nalgebra::allocator::Allocator;
use nalgebra::core::dimension::DimMin;
use nalgebra::core::dimension::{U2, U4};
use nalgebra::{DefaultAllocator, OMatrix, OVector, RealField};

use adskalman::ObservationModel;

#[derive(Debug)]
pub struct ObservationModel2D<R: RealField+Copy> {
    observation_matrix: OMatrix<R, U2, U4>,
    observation_matrix_transpose: OMatrix<R, U4, U2>,
    observation_noise_covariance: OMatrix<R, U2, U2>,
}

impl<R: RealField+Copy> ObservationModel2D<R> {
    pub fn new(observation_noise_covariance: OMatrix<R, U2, U2>) -> Self {
        let zero: R = Zero::zero();
        let one: R = One::one();

        #[rustfmt::skip]
        let observation_matrix = OMatrix::<R,U2,U4>::new(
                          one, zero, zero, zero,
                         zero,  one, zero, zero);
        let observation_matrix_transpose = observation_matrix.transpose();
        Self {
            observation_matrix,
            observation_matrix_transpose,
            observation_noise_covariance,
        }
    }
}

impl<R: RealField+Copy> ObservationModel<R, U4, U2> for ObservationModel2D<R>
where
    DefaultAllocator: Allocator<R, U4, U4>,
    DefaultAllocator: Allocator<R, U4>,
    DefaultAllocator: Allocator<R, U2, U4>,
    DefaultAllocator: Allocator<R, U4, U2>,
    DefaultAllocator: Allocator<R, U2, U2>,
    DefaultAllocator: Allocator<R, U2>,
    DefaultAllocator: Allocator<(usize, usize), U2>,
    U2: DimMin<U2, Output = U2>,
{
    fn H(&self) -> &OMatrix<R, U2, U4> {
        &self.observation_matrix
    }
    fn HT(&self) -> &OMatrix<R, U4, U2> {
        &self.observation_matrix_transpose
    }
    fn R(&self) -> &OMatrix<R, U2, U2> {
        &self.observation_noise_covariance
    }
    fn predict_observation(&self, state: &OVector<R, U4>) -> OVector<R, U2> {
        &self.observation_matrix * state
    }
}