tracking/
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::{DefaultAllocator, OMatrix, RealField, allocator::Allocator, dimension::U6};
7
8use crate::motion_model_3d_fixed_dt::MotionModel3D;
9use crate::motion_model_3d_fixed_dt::MotionModel3DFixedDt;
10
11/// constant velocity 3D motion model parameterized by `dt`
12///
13/// The important method is `calc_for_dt()`. Calling this
14/// returns a motion model for a specific `dt`.
15///
16/// The state vector is [x y z xvel yvel zvel].
17#[derive(Debug, Clone)]
18pub struct ConstantVelocity3DModel<R: RealField + Copy>
19where
20    DefaultAllocator: Allocator<U6, U6>,
21    DefaultAllocator: Allocator<U6>,
22{
23    motion_noise_scale: R,
24}
25
26impl<R: RealField + Copy> ConstantVelocity3DModel<R>
27where
28    DefaultAllocator: Allocator<U6, U6>,
29    DefaultAllocator: Allocator<U6>,
30{
31    pub fn new(motion_noise_scale: R) -> Self {
32        Self { motion_noise_scale }
33    }
34}
35
36impl<R: RealField + Copy> MotionModel3D<R> for ConstantVelocity3DModel<R>
37where
38    DefaultAllocator: Allocator<U6, U6>,
39    DefaultAllocator: Allocator<U6>,
40{
41    fn calc_for_dt(&self, dt: R) -> MotionModel3DFixedDt<R> {
42        let zero: R = Zero::zero();
43        let one: R = One::one();
44        let two: R = one + one;
45        let three: R = two + one;
46
47        // Create transition model. 3D position and 3D velocity.
48        // This is "A" in most Kalman filter descriptions.
49        #[rustfmt::skip]
50        let transition_model = OMatrix::<R,U6,U6>::from_row_slice(
51                          &[one, zero, zero,   dt, zero, zero,
52                         zero,  one, zero, zero,   dt, zero,
53                         zero, zero,  one, zero, zero,   dt,
54                         zero, zero, zero,  one, zero, zero,
55                         zero, zero, zero, zero,  one, zero,
56                         zero, zero, zero, zero, zero,  one]);
57        let transition_model_transpose = transition_model.transpose();
58
59        let t33 = (dt * dt * dt) / three;
60        let t22 = (dt * dt) / two;
61
62        // This is "Q" in most Kalman filter descriptions.
63        #[rustfmt::skip]
64        let transition_noise_covariance = OMatrix::<R,U6,U6>::from_row_slice(
65                        &[t33,  zero, zero, t22, zero,  zero,
66                        zero,  t33, zero, zero,  t22, zero,
67                        zero, zero,  t33, zero, zero,  t22,
68                        t22,  zero, zero,   dt, zero, zero,
69                        zero,  t22, zero, zero,   dt, zero,
70                        zero, zero,  t22, zero, zero,   dt]) * self.motion_noise_scale;
71        MotionModel3DFixedDt {
72            transition_model,
73            transition_model_transpose,
74            transition_noise_covariance,
75        }
76    }
77}
tracking/motion_model_3d.rs

tracking/
motion_model_3d.rs
