tracking/
observation_model_2d.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4use num_traits::{One, Zero};
5
6use nalgebra::{
7    DefaultAllocator, OMatrix, OVector, RealField,
8    allocator::Allocator,
9    dimension::{DimMin, U2, U4},
10};
11
12use adskalman::ObservationModel;
13
14#[derive(Debug)]
15pub struct ObservationModel2D<R: RealField + Copy> {
16    observation_matrix: OMatrix<R, U2, U4>,
17    observation_matrix_transpose: OMatrix<R, U4, U2>,
18    observation_noise_covariance: OMatrix<R, U2, U2>,
19}
20
21impl<R: RealField + Copy> ObservationModel2D<R> {
22    pub fn new(observation_noise_covariance: OMatrix<R, U2, U2>) -> Self {
23        let zero: R = Zero::zero();
24        let one: R = One::one();
25
26        #[rustfmt::skip]
27        let observation_matrix = OMatrix::<R,U2,U4>::new(
28                          one, zero, zero, zero,
29                         zero,  one, zero, zero);
30        let observation_matrix_transpose = observation_matrix.transpose();
31        Self {
32            observation_matrix,
33            observation_matrix_transpose,
34            observation_noise_covariance,
35        }
36    }
37}
38
39impl<R: RealField + Copy> ObservationModel<R, U4, U2> for ObservationModel2D<R>
40where
41    DefaultAllocator: Allocator<U4, U4>,
42    DefaultAllocator: Allocator<U4>,
43    DefaultAllocator: Allocator<U2, U4>,
44    DefaultAllocator: Allocator<U4, U2>,
45    DefaultAllocator: Allocator<U2, U2>,
46    DefaultAllocator: Allocator<U2>,
47    U2: DimMin<U2, Output = U2>,
48{
49    fn H(&self) -> &OMatrix<R, U2, U4> {
50        &self.observation_matrix
51    }
52    fn HT(&self) -> &OMatrix<R, U4, U2> {
53        &self.observation_matrix_transpose
54    }
55    fn R(&self) -> &OMatrix<R, U2, U2> {
56        &self.observation_noise_covariance
57    }
58    fn predict_observation(&self, state: &OVector<R, U4>) -> OVector<R, U2> {
59        self.observation_matrix * state
60    }
61}
tracking/observation_model_2d.rs

tracking/
observation_model_2d.rs
