tracking/
flat_motion_model_3d.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::allocator::Allocator;
7use nalgebra::dimension::U6;
8use nalgebra::{DefaultAllocator, OMatrix, RealField};
9
10use crate::motion_model_3d_fixed_dt::MotionModel3D;
11use crate::motion_model_3d_fixed_dt::MotionModel3DFixedDt;
12
13/// constant velocity 3D motion model with Z fixed to 0
14///
15/// The important method is `calc_for_dt()`. Calling this
16/// returns a motion model for a specific `dt`.
17///
18/// The state vector is [x y z xvel yvel zvel].
19#[derive(Debug, Clone)]
20pub struct FlatZZero3DModel<R: RealField + Copy>
21where
22    DefaultAllocator: Allocator<U6, U6>,
23    DefaultAllocator: Allocator<U6>,
24{
25    motion_noise_scale: R,
26}
27
28impl<R: RealField + Copy> FlatZZero3DModel<R>
29where
30    DefaultAllocator: Allocator<U6, U6>,
31    DefaultAllocator: Allocator<U6>,
32{
33    pub fn new(motion_noise_scale: R) -> Self {
34        Self { motion_noise_scale }
35    }
36}
37
38impl<R: RealField + Copy> MotionModel3D<R> for FlatZZero3DModel<R>
39where
40    DefaultAllocator: Allocator<U6, U6>,
41    DefaultAllocator: Allocator<U6>,
42{
43    fn calc_for_dt(&self, dt: R) -> MotionModel3DFixedDt<R> {
44        let zero: R = Zero::zero();
45        let one: R = One::one();
46        let two: R = one + one;
47        let three: R = two + one;
48
49        // Create transition model. 3D position and 3D velocity.
50        // This is "A" in most Kalman filter descriptions.
51        #[rustfmt::skip]
52        let transition_model = {
53            OMatrix::<R,U6,U6>::from_row_slice(
54                        &[one, zero, zero,   dt, zero, zero,
55                         zero,  one, zero, zero,   dt, zero,
56                         zero, zero, zero, zero, zero, zero,
57                         zero, zero, zero,  one, zero, zero,
58                         zero, zero, zero, zero,  one, zero,
59                         zero, zero, zero, zero, zero, zero])
60            };
61        let transition_model_transpose = transition_model.transpose();
62
63        let t33 = (dt * dt * dt) / three;
64        let t22 = (dt * dt) / two;
65
66        // This is "Q" in most Kalman filter descriptions.
67        #[rustfmt::skip]
68        let transition_noise_covariance = {
69            OMatrix::<R,U6,U6>::from_row_slice(
70                       &[t33,  zero, zero, t22, zero,  zero,
71                        zero,  t33, zero, zero,  t22, zero,
72                        zero, zero, zero, zero, zero, zero,
73                        t22,  zero, zero,   dt, zero, zero,
74                        zero,  t22, zero, zero,   dt, zero,
75                        zero, zero, zero, zero, zero, zero]) * self.motion_noise_scale
76            };
77        MotionModel3DFixedDt {
78            transition_model,
79            transition_model_transpose,
80            transition_noise_covariance,
81        }
82    }
83}
84
85#[cfg(test)]
86mod test {
87    use super::*;
88    use adskalman::TransitionModelLinearNoControl;
89
90    #[test]
91    fn test_fix_z() {
92        let model = FlatZZero3DModel::new(1.0);
93        let m2 = model.calc_for_dt(1.0);
94        let matrix = m2.F();
95
96        let pos1 = na::OVector::<_, U6>::from_row_slice(&[0.1, 0.2, 0.3, 0.4, 0.5, 0.6]);
97        let pos2 = matrix * pos1;
98
99        // Check the z position is zero after update.
100        assert_eq!(pos2[2], 0.0);
101
102        // Check the z vel is zero after update.
103        assert_eq!(pos2[5], 0.0);
104    }
105}
tracking/flat_motion_model_3d.rs

tracking/
flat_motion_model_3d.rs
