tracking/
motion_model_3d_fixed_dt.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4use nalgebra as na;
5use nalgebra::{DefaultAllocator, OMatrix, RealField, allocator::Allocator, dimension::U6};
6
7use adskalman::TransitionModelLinearNoControl;
8
9/// constant velocity 3D motion model for fixed dt
10///
11/// The state vector is [x y z xvel yvel zvel]
12#[derive(Debug, Clone)]
13pub struct MotionModel3DFixedDt<R: RealField + Copy>
14where
15    DefaultAllocator: Allocator<U6, U6>,
16    DefaultAllocator: Allocator<U6>,
17{
18    pub transition_model: OMatrix<R, U6, U6>,
19    pub transition_model_transpose: OMatrix<R, U6, U6>,
20    pub transition_noise_covariance: OMatrix<R, U6, U6>,
21}
22
23impl<R: RealField + Copy> TransitionModelLinearNoControl<R, U6> for MotionModel3DFixedDt<R>
24where
25    DefaultAllocator: Allocator<U6, U6>,
26    DefaultAllocator: Allocator<U6>,
27{
28    fn F(&self) -> &OMatrix<R, U6, U6> {
29        &self.transition_model
30    }
31    fn FT(&self) -> &OMatrix<R, U6, U6> {
32        &self.transition_model_transpose
33    }
34    fn Q(&self) -> &OMatrix<R, U6, U6> {
35        &self.transition_noise_covariance
36    }
37}
38
39pub trait MotionModel3D<R>: Clone
40where
41    R: na::RealField + Copy,
42{
43    /// For a given `dt`, create a new instance of the motion model.
44    fn calc_for_dt(&self, dt: R) -> MotionModel3DFixedDt<R>;
45}
tracking/motion_model_3d_fixed_dt.rs

tracking/
motion_model_3d_fixed_dt.rs
