Line data Source code
1 : #ifndef D2E08F36_A9FC_4D2E_9737_D64D5B314B9E
2 : #define D2E08F36_A9FC_4D2E_9737_D64D5B314B9E
3 :
4 : #include "motion/motion_model_ca.h"
5 :
6 : #include "motion/state_cov_converter.hpp" // IWYU pragma: keep
7 : #include "motion/state_vec_converter.hpp" // IWYU pragma: keep
8 :
9 : namespace tracking
10 : {
11 : namespace motion
12 : {
13 :
14 : template <typename CovarianceMatrixPolicy_>
15 10 : MotionModelCA<CovarianceMatrixPolicy_>::MotionModelCA(const StateVec& vec, const StateCov& cov)
16 10 : : BaseExtendedMotionModel{vec, cov}
17 : {
18 10 : }
19 :
20 : template <typename CovarianceMatrixPolicy_>
21 20 : void MotionModelCA<CovarianceMatrixPolicy_>::computeEgoMotionCompensationMatrices(EgoMotionMappingMatrix& Ge,
22 : StateMatrix& Go,
23 : const EgoMotionType& egoMotion)
24 : {
25 20 : const value_type& x = this->operator[](StateDefCA::X);
26 20 : const value_type& y = this->operator[](StateDefCA::Y);
27 20 : const value_type& vx = this->operator[](StateDefCA::VX);
28 20 : const value_type& vy = this->operator[](StateDefCA::VY);
29 20 : const value_type& ax = this->operator[](StateDefCA::AX);
30 20 : const value_type& ay = this->operator[](StateDefCA::AY);
31 :
32 20 : const value_type sinDeltaPsiEgo = egoMotion.getDisplacementCog().sinDeltaPsi;
33 20 : const value_type cosDeltaPsiEgo = egoMotion.getDisplacementCog().cosDeltaPsi;
34 20 : const value_type deltaXEgo = egoMotion.getDisplacementCog().vec.at_unsafe(EgoMotionType::DS_X);
35 20 : const value_type deltaYEgo = egoMotion.getDisplacementCog().vec.at_unsafe(EgoMotionType::DS_Y);
36 20 : const value_type distCog2Ego = egoMotion.getGeometry().distCog2Ego;
37 :
38 20 : Go.setZeros();
39 20 : Go.at_unsafe(X, X) = cosDeltaPsiEgo;
40 20 : Go.at_unsafe(X, Y) = sinDeltaPsiEgo;
41 20 : Go.at_unsafe(Y, X) = -sinDeltaPsiEgo;
42 20 : Go.at_unsafe(Y, Y) = cosDeltaPsiEgo;
43 20 : Go.at_unsafe(VX, VX) = cosDeltaPsiEgo;
44 20 : Go.at_unsafe(VX, VY) = sinDeltaPsiEgo;
45 20 : Go.at_unsafe(VY, VX) = -sinDeltaPsiEgo;
46 20 : Go.at_unsafe(VY, VY) = cosDeltaPsiEgo;
47 20 : Go.at_unsafe(AX, AX) = cosDeltaPsiEgo;
48 20 : Go.at_unsafe(AX, AY) = sinDeltaPsiEgo;
49 20 : Go.at_unsafe(AY, AX) = -sinDeltaPsiEgo;
50 20 : Go.at_unsafe(AY, AY) = cosDeltaPsiEgo;
51 :
52 20 : const value_type x0 = -deltaYEgo + y;
53 20 : const value_type x1 = deltaXEgo - distCog2Ego - x;
54 20 : Ge.setZeros();
55 20 : Ge.at_unsafe(X, EgoMotionType::DS_X) = -cosDeltaPsiEgo;
56 20 : Ge.at_unsafe(X, EgoMotionType::DS_Y) = -sinDeltaPsiEgo;
57 20 : Ge.at_unsafe(X, EgoMotionType::DS_PSI) = (x0 * cosDeltaPsiEgo) + (x1 * sinDeltaPsiEgo);
58 20 : Ge.at_unsafe(Y, EgoMotionType::DS_X) = sinDeltaPsiEgo;
59 20 : Ge.at_unsafe(Y, EgoMotionType::DS_Y) = cosDeltaPsiEgo;
60 20 : Ge.at_unsafe(Y, EgoMotionType::DS_PSI) = -(x0 * sinDeltaPsiEgo) + (x1 * cosDeltaPsiEgo);
61 20 : Ge.at_unsafe(VX, EgoMotionType::DS_PSI) = -(vx * sinDeltaPsiEgo) + (vy * cosDeltaPsiEgo);
62 20 : Ge.at_unsafe(VY, EgoMotionType::DS_PSI) = -(vx * cosDeltaPsiEgo) - (vy * sinDeltaPsiEgo);
63 20 : Ge.at_unsafe(AX, EgoMotionType::DS_PSI) = -(ax * sinDeltaPsiEgo) + (ay * cosDeltaPsiEgo);
64 20 : Ge.at_unsafe(AY, EgoMotionType::DS_PSI) = -(ax * cosDeltaPsiEgo) - (ay * sinDeltaPsiEgo);
65 20 : }
66 :
67 : template <typename CovarianceMatrixPolicy_>
68 20 : void MotionModelCA<CovarianceMatrixPolicy_>::compensateState(const EgoMotionType& egoMotion)
69 : {
70 20 : value_type& x = this->operator[](StateDefCA::X);
71 20 : value_type& y = this->operator[](StateDefCA::Y);
72 20 : value_type& vx = this->operator[](StateDefCA::VX);
73 20 : value_type& vy = this->operator[](StateDefCA::VY);
74 20 : value_type& ax = this->operator[](StateDefCA::AX);
75 20 : value_type& ay = this->operator[](StateDefCA::AY);
76 :
77 : // translate and rotate position
78 20 : egoMotion.compensatePosition(x, y, x, y);
79 : // rotate velocity and acceleration
80 20 : egoMotion.compensateDirection(vx, vy, vx, vy);
81 20 : egoMotion.compensateDirection(ax, ay, ax, ay);
82 20 : }
83 :
84 : template <typename CovarianceMatrixPolicy_>
85 40 : void MotionModelCA<CovarianceMatrixPolicy_>::applyProcessModel(const value_type dt)
86 : {
87 40 : const StateVec& stateVec = this->getVec();
88 40 : const auto halfDtSq = static_cast<value_type>(0.5) * dt * dt;
89 :
90 40 : this->operator[](StateDefCA::X) += dt * stateVec.at_unsafe(StateDefCA::VX) + halfDtSq * stateVec.at_unsafe(StateDefCA::AX);
91 40 : this->operator[](StateDefCA::Y) += dt * stateVec.at_unsafe(StateDefCA::VY) + halfDtSq * stateVec.at_unsafe(StateDefCA::AY);
92 40 : this->operator[](StateDefCA::VX) += dt * stateVec.at_unsafe(StateDefCA::AX);
93 40 : this->operator[](StateDefCA::VY) += dt * stateVec.at_unsafe(StateDefCA::AY);
94 40 : }
95 :
96 : template <typename CovarianceMatrixPolicy_>
97 40 : void MotionModelCA<CovarianceMatrixPolicy_>::computeA(StateMatrix& A, const value_type dt) const
98 : {
99 40 : const auto halfDtSq = static_cast<value_type>(0.5) * dt * dt;
100 :
101 40 : A.setIdentity();
102 40 : A.at_unsafe(StateDefCA::X, StateDefCA::AX) = halfDtSq;
103 40 : A.at_unsafe(StateDefCA::X, StateDefCA::VX) = dt;
104 40 : A.at_unsafe(StateDefCA::VX, StateDefCA::AX) = dt;
105 :
106 40 : A.at_unsafe(StateDefCA::Y, StateDefCA::AY) = halfDtSq;
107 40 : A.at_unsafe(StateDefCA::Y, StateDefCA::VY) = dt;
108 40 : A.at_unsafe(StateDefCA::VY, StateDefCA::AY) = dt;
109 40 : }
110 :
111 : template <typename CovarianceMatrixPolicy_>
112 40 : void MotionModelCA<CovarianceMatrixPolicy_>::computeQ(ProcessNoiseDiagMatrix& Q, const value_type dt)
113 : {
114 : // DWPA process, elements in Q define an acceleration increment, i.e. unit is equal to m/s**2
115 40 : Q.at_unsafe(Q_AX) = static_cast<value_type>(100.0 * dt * dt);
116 40 : Q.at_unsafe(Q_AY) = static_cast<value_type>(100.0 * dt * dt);
117 40 : }
118 :
119 : template <typename CovarianceMatrixPolicy_>
120 40 : void MotionModelCA<CovarianceMatrixPolicy_>::computeG(ProcessNoiseMappingMatrix& G, const value_type dt)
121 : {
122 40 : const value_type halfDeltaTimePow2 = static_cast<value_type>(0.5) * dt * dt;
123 :
124 40 : G.setZeros();
125 40 : G.at_unsafe(X, Q_AX) = halfDeltaTimePow2;
126 40 : G.at_unsafe(VX, Q_AX) = dt;
127 40 : G.at_unsafe(AX, Q_AX) = static_cast<value_type>(1.0);
128 40 : G.at_unsafe(Y, Q_AY) = halfDeltaTimePow2;
129 40 : G.at_unsafe(VY, Q_AY) = dt;
130 40 : G.at_unsafe(AY, Q_AY) = static_cast<value_type>(1.0);
131 40 : }
132 :
133 : template <typename CovarianceMatrixPolicy_>
134 2 : void MotionModelCA<CovarianceMatrixPolicy_>::convertFrom(const MotionModelCV<CovarianceMatrixPolicy_>& other)
135 : {
136 : using other_type = MotionModelCV<CovarianceMatrixPolicy_>;
137 2 : StateVecConverter<instance_type, other_type>::convertFrom(this->getVec(), other.getVec());
138 2 : StateCovConverter<instance_type, other_type>::convertFrom(this->getCov(), other.getCov());
139 2 : }
140 :
141 : } // namespace motion
142 : } // namespace tracking
143 :
144 : #endif // D2E08F36_A9FC_4D2E_9737_D64D5B314B9E
|
