Line data Source code
1 : #ifndef FA800472_29A5_4B6F_90A0_4283A0D513D6
2 : #define FA800472_29A5_4B6F_90A0_4283A0D513D6
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "math/linalg/covariance_matrix_factored.h" // IWYU pragma: keep
6 : #include "math/linalg/covariance_matrix_full.h" // IWYU pragma: keep
7 : #include "math/linalg/matrix.h"
8 : #include "motion/imotion_model.h"
9 :
10 :
11 : namespace tracking
12 : {
13 : namespace motion
14 : {
15 :
16 : // TODO(matthias): add interface contract
17 : // TODO(matthias): add doxygen
18 :
19 : // forward declaration to avoid recursive inclusion
20 : template <typename CovarianceMatrixPolicy_>
21 : class MotionModelCA;
22 :
23 : struct StateDefCV
24 : {
25 : enum _
26 : {
27 : X = 0,
28 : VX,
29 : Y,
30 : VY,
31 : NUM_STATE_VARIABLES
32 : };
33 : };
34 :
35 : template <typename CovarianceMatrixPolicy_>
36 : class MotionModelCV TEST_REMOVE_FINAL
37 : : public StateDefCV
38 : , public ExtendedMotionModel<MotionModelCV<CovarianceMatrixPolicy_>, MotionModelTraits<CovarianceMatrixPolicy_, StateDefCV>>
39 : {
40 : public:
41 : enum NoiseDef
42 : {
43 : Q_VX = 0,
44 : Q_VY,
45 : NUM_PROC_NOISE_VARIABLES
46 : };
47 :
48 : using instance_type = MotionModelCV<CovarianceMatrixPolicy_>;
49 : using instance_trait = MotionModelTraits<CovarianceMatrixPolicy_, StateDefCV>;
50 : using BaseExtendedMotionModel = ExtendedMotionModel<instance_type, instance_trait>;
51 : using value_type = typename instance_trait::value_type;
52 : using StateVec = typename BaseExtendedMotionModel::StateVec;
53 : using StateCov = typename BaseExtendedMotionModel::StateCov;
54 :
55 : using StateMatrix = math::SquareMatrix<value_type, NUM_STATE_VARIABLES>;
56 : using ProcessNoiseDiagMatrix = math::DiagonalMatrix<value_type, NUM_PROC_NOISE_VARIABLES>;
57 : using ProcessNoiseMappingMatrix = math::Matrix<value_type, NUM_STATE_VARIABLES, NUM_PROC_NOISE_VARIABLES>;
58 :
59 : using EgoMotionType = typename BaseExtendedMotionModel::EgoMotionType;
60 : using EgoMotionMappingMatrix = math::Matrix<value_type, NUM_STATE_VARIABLES, EgoMotionType::DS_NUM_VARIABLES>;
61 :
62 : static constexpr sint32 NUM_AUG_PROC_NOISE_VARIABLES =
63 : NUM_PROC_NOISE_VARIABLES + static_cast<sint32>(EgoMotionType::DS_NUM_VARIABLES);
64 : using AugmentedProcessNoiseDiagMatrix = math::DiagonalMatrix<value_type, NUM_AUG_PROC_NOISE_VARIABLES>;
65 : using AugmentedProcessNoiseMappingMatrix = math::Matrix<value_type, NUM_STATE_VARIABLES, NUM_AUG_PROC_NOISE_VARIABLES>;
66 :
67 : // rule of 5 declarations
68 2 : MotionModelCV() = default;
69 : MotionModelCV(const MotionModelCV&) = default;
70 : MotionModelCV(MotionModelCV&&) noexcept = default;
71 : auto operator=(const MotionModelCV&) -> MotionModelCV& = default;
72 : auto operator=(MotionModelCV&&) noexcept -> MotionModelCV& = default;
73 8 : virtual ~MotionModelCV() TEST_REMOVE_FINAL = default;
74 :
75 : /// \brief Construct a new CV given the vector and the covariance matrix
76 : /// \param[in] vec
77 : /// \param[in] cov
78 : explicit MotionModelCV(const StateVec& vec, const StateCov& cov);
79 :
80 : /// \brief Read access to x velocity
81 : /// \return value_type
82 0 : auto getVx() const -> value_type final { return this->operator[](StateDefCV::VX); }
83 : /// \brief Read access to y velocity
84 : /// \return value_type
85 0 : auto getVy() const -> value_type final { return this->operator[](StateDefCV::VY); }
86 : /// \brief Read access to x acceleration
87 : /// \return value_type
88 0 : auto getAx() const -> value_type final { return static_cast<value_type>(0.0); }
89 : /// \brief Read access to y acceleration
90 : /// \return value_type
91 0 : auto getAy() const -> value_type final { return static_cast<value_type>(0.0); }
92 : // ... all the other virtual functions
93 :
94 : /// \brief Creates a CV model based on a CA model
95 : /// \param[in] other The CA model
96 : void convertFrom(const MotionModelCA<CovarianceMatrixPolicy_>& other);
97 :
98 : /// \brief Compute Ge and Go for compensation of the covariance
99 : /// \param[out] Ge The propagated errors of the ego motion to the state space
100 : /// \param[out] Go The transformation of the state caused by the ego motion
101 : /// \param[in] egoMotion The known egoMotion from last state to predicted state
102 : TEST_VIRTUAL void computeEgoMotionCompensationMatrices(EgoMotionMappingMatrix& Ge,
103 : StateMatrix& Go,
104 : const EgoMotionType& egoMotion);
105 :
106 : /// \brief Compensate the state with the given ego motion
107 : /// \param[in] egoMotion The known egoMotion from last state to predicted state
108 : TEST_VIRTUAL void compensateState(const EgoMotionType& egoMotion);
109 :
110 : /// \brief Apply the state transition from k to k+1 defined by the process model
111 : /// \param[in] dt The delta time from last state to predicted state
112 : void applyProcessModel(const value_type dt);
113 :
114 : /// \brief Compute matrix A using the 1st order linearisation of the state transition from k to k+1
115 : /// \param[out] A Linearisation of the state transition
116 : /// \param[in] dt The delta time from last state to predicted state
117 : void computeA(StateMatrix& A, const value_type dt) const;
118 :
119 : /// \brief Compute the diagonal process noise matrix Q.
120 : /// \param[out] Q The process noise
121 : /// \param[in] dt The delta time from last state to predicted state
122 : static void computeQ(ProcessNoiseDiagMatrix& Q, const value_type dt);
123 :
124 : /// \brief Compute the matrix G to map the diagonoal process noise to the full state
125 : /// \param[out] G The transformation of the process noise to the full state space
126 : /// \param[in] dt The delta time from last state to predicted state
127 : static void computeG(ProcessNoiseMappingMatrix& G, const value_type dt);
128 : };
129 :
130 : } // namespace motion
131 : } // namespace tracking
132 :
133 : #endif // FA800472_29A5_4B6F_90A0_4283A0D513D6
|
