Line data Source code
1 : #ifndef A5CB3020_67E9_40A3_8F41_9346328EADA3
2 : #define A5CB3020_67E9_40A3_8F41_9346328EADA3
3 :
4 : #include "motion/generic_predict.h"
5 :
6 : #include "filter/information_filter.hpp" // IWYU pragma: keep
7 : #include "filter/kalman_filter.hpp" // IWYU pragma: keep
8 : #include "math/linalg/square_matrix.h" // IWYU pragma: keep
9 : #include "motion/generic_predict_common.hpp" // IWYU pragma: keep
10 :
11 : namespace tracking
12 : {
13 : namespace motion
14 : {
15 : namespace generic
16 : {
17 :
18 : template <typename MotionModel_, typename CovarianceMatrixPolicy_>
19 40 : inline void Predict<MotionModel_, CovarianceMatrixPolicy_>::run(const value_type dt,
20 : const KalmanFilterType& filter,
21 : const EgoMotionType& egoMotion)
22 : {
23 : // Access to MotionModel internals
24 40 : auto& underlying = static_cast<MotionModel_&>(*this);
25 40 : auto& P = underlying.getCovForInternalUse();
26 :
27 40 : static typename BasePredictCommon::Storage data{};
28 40 : BasePredictCommon::run(data, dt, egoMotion);
29 :
30 : // Covariance prediction
31 : if constexpr (CovarianceMatrixPolicy_::is_factored)
32 : {
33 20 : if (egoMotion.getDisplacementCog().vec.isZeros())
34 : {
35 10 : filter.predictCovariance(P, data.AGo, data.G, data.Q);
36 : }
37 : else
38 : {
39 10 : filter.predictCovariance(P, data.AGo, data.Gstar, data.Qstar);
40 : }
41 : }
42 : else
43 : {
44 : // apply ego motion compensation on P
45 20 : P.apaT(data.Go);
46 40 : P += data.Ge * egoMotion.getDisplacementCog().cov * data.Ge.transpose();
47 20 : P.symmetrize();
48 :
49 : // apply time update
50 20 : filter.predictCovariance(P, data.A, data.G, data.Q);
51 : }
52 40 : }
53 :
54 : template <typename MotionModel_, typename CovarianceMatrixPolicy_>
55 40 : inline void Predict<MotionModel_, CovarianceMatrixPolicy_>::run(const value_type dt,
56 : const InformationFilterType& filter,
57 : const EgoMotionType& egoMotion)
58 : {
59 : // Access to MotionModel internals
60 40 : auto& underlying = static_cast<MotionModel_&>(*this);
61 40 : auto& Y = underlying.getCovForInternalUse();
62 :
63 : // prepare covariance prediction based on egomotion compensated state
64 40 : static typename BasePredictCommon::Storage data{};
65 :
66 : // Step 1: Transform from information space to state space for state prediction
67 40 : underlying.convertStateVecIntoStateSpace();
68 :
69 : // Step 2: Run state prediction in state space (existing code)
70 40 : BasePredictCommon::run(data, dt, egoMotion);
71 :
72 : // Step 3: Covariance prediction (Y_k → Y_k+1)
73 : // NOTE: This MUST happen BEFORE convertStateVecIntoInformationSpace() because
74 : // the transformation needs the NEW information matrix Y_k+1
75 : if constexpr (CovarianceMatrixPolicy_::is_factored)
76 : {
77 20 : if (egoMotion.getDisplacementCog().vec.isZeros())
78 : {
79 10 : filter.predictCovariance(Y, data.AGo, data.G, data.Q);
80 : }
81 : else
82 : {
83 10 : filter.predictCovariance(Y, data.AGo, data.Gstar, data.Qstar);
84 : }
85 : }
86 : else
87 : {
88 20 : if (egoMotion.getDisplacementCog().vec.isZeros()) // TODO(matthias): is this really correct, Yes: because we are checking if
89 : // the displacement vector is all zeros without tolerances!
90 : {
91 : // filter prediction step with zero ego motion
92 10 : filter.predictCovariance(Y, data.A, data.G, data.Q);
93 : }
94 : else
95 : {
96 : using EgoMotionCov = typename EgoMotionType::DisplacementCov;
97 : using StateCov = typename MotionModel_::StateCov;
98 : using StateMatrix = typename MotionModel_::StateMatrix;
99 : // Y = inv(Go*P*Go.T + Ge*Pe*Ge.T) with Po=inv(Y) can be solved directly in information space by
100 : // applying two steps
101 : // 1. Handle the State Transition
102 : // Ys = inv(Go*P*Go.T) = inv(Go).T * Y * inv(Go)
103 : // 2. Handle the Additive Noise Q=Ge*Pe*Ge.T
104 : // Y = inv(Ps + Q) applying the Woodbury Matrix Identity gives
105 : // Y = Ys - Ys*Ge * inv( inv(Pe) + Ge.T*Ys*Ge ) * Ge.T*Ys
106 :
107 : // step 1: Handle the State Transition (using linear solver)
108 10 : StateMatrix Z_T = data.Go.transpose().qrSolve(Y); // solve Go.T * Z_transpose = Y
109 10 : auto Ytr = StateCov{std::move(data.Go.transpose().qrSolve(Z_T.transpose()))}; // solve Go.T * Y_tr = Z
110 10 : Ytr.symmetrize();
111 :
112 : // step 2: Handle the Additive Noise to be applied on the transformed Ytr (different to the one-step factored solution)!!!
113 : // apply Woodbury Matrix Identity
114 : // (Y^-1 + Ge Pe Ge')^-1 = Y - Y Ge (Pe^-1 + Ge' Y Ge)^-1 Ge' Y
115 : // note: if any of the inverses fails, we skip the ego motion compensation step
116 10 : const auto invPe = egoMotion.getDisplacementCog().cov.inverse();
117 10 : if (invPe.has_value())
118 : {
119 : // Compute Y Ge
120 10 : auto YGe =
121 : math::Matrix<value_type, MotionModel_::NUM_STATE_VARIABLES, EgoMotionType::DS_NUM_VARIABLES, true>{Ytr * data.Ge};
122 :
123 : // Compute Ge' Y
124 10 : auto GeTY = math::Matrix<value_type, EgoMotionType::DS_NUM_VARIABLES, MotionModel_::NUM_STATE_VARIABLES, false>{
125 : data.Ge.transpose() * Ytr};
126 :
127 : // Compute Ge' Y Ge
128 10 : auto GeTYGe = math::SquareMatrix<value_type, EgoMotionType::DS_NUM_VARIABLES, false>{GeTY * data.Ge};
129 :
130 : // Compute M = (Pe^-1 + Ge' Y Ge)^-1
131 10 : auto M = EgoMotionCov{typename EgoMotionCov::BaseSquareMatrix{invPe.value() + std::move(GeTYGe)}};
132 10 : M.symmetrize();
133 10 : const auto invM = M.inverse();
134 10 : if (invM.has_value())
135 : {
136 10 : const auto mat = std::move(YGe) * std::move(invM.value()) * std::move(GeTY);
137 : // Y^-1 + Ge Pe Ge')^-1 = Y - Y Ge (Pe^-1 + Ge' Y Ge)^-1 Ge' Y
138 10 : Ytr -= mat;
139 10 : Ytr.symmetrize();
140 :
141 : // final filter prediction step with compensated Ytr
142 10 : filter.predictCovariance(Ytr, data.A, data.G, data.Q);
143 :
144 : // prevent destroying the Information matrix, e.g. removing information from a zero Y matrix (no information)
145 10 : if (Ytr.isPositiveSemiDefinite())
146 : {
147 10 : Y = std::move(Ytr);
148 : }
149 10 : }
150 10 : }
151 10 : }
152 : }
153 :
154 : // Step 4: Transform state to information vector using the NEW information matrix Y_k+1
155 40 : underlying.convertStateVecIntoInformationSpace();
156 40 : }
157 :
158 : } // namespace generic
159 : } // namespace motion
160 : } // namespace tracking
161 :
162 : #endif // A5CB3020_67E9_40A3_8F41_9346328EADA3
|
