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