Line | Branch | Exec | Source |
---|---|---|---|
1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
3 | // the University of Stuttgart, Germany. | ||
4 | // | ||
5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
8 | |||
9 | /// @file MotionModel.hpp | ||
10 | /// @brief Motion System Model | ||
11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
12 | /// @date 2024-08-20 | ||
13 | |||
14 | #pragma once | ||
15 | |||
16 | #include <array> | ||
17 | #include <cmath> | ||
18 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
19 | #include <Eigen/Core> | ||
20 | #include <cstdint> | ||
21 | #include <imgui.h> | ||
22 | #include <fmt/ranges.h> | ||
23 | |||
24 | #include "Navigation/GNSS/SystemModel/Units.hpp" | ||
25 | #include "Navigation/Math/KalmanFilter.hpp" | ||
26 | #include "Navigation/Math/VanLoan.hpp" | ||
27 | #include "Navigation/GNSS/SystemModel/SystemModel.hpp" | ||
28 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
29 | #include "Units.hpp" | ||
30 | #include "util/Container/KeyedMatrix.hpp" | ||
31 | #include "internal/gui/widgets/InputWithUnit.hpp" | ||
32 | |||
33 | namespace NAV | ||
34 | { | ||
35 | |||
36 | namespace Keys | ||
37 | { | ||
38 | |||
39 | /// Keys used in the model | ||
40 | enum MotionModelKey : uint8_t | ||
41 | { | ||
42 | PosX, ///< Position ECEF_X [m] | ||
43 | PosY, ///< Position ECEF_Y [m] | ||
44 | PosZ, ///< Position ECEF_Z [m] | ||
45 | VelX, ///< Velocity ECEF_X [m/s] | ||
46 | VelY, ///< Velocity ECEF_Y [m/s] | ||
47 | VelZ, ///< Velocity ECEF_Z [m/s] | ||
48 | AttQ1, ///< x: Coefficient of i | ||
49 | AttQ2, ///< y: Coefficient of j | ||
50 | AttQ3, ///< z: Coefficient of k | ||
51 | AttQ0, ///< w: Real (scalar) part of the Quaternion | ||
52 | MotionModelKey_COUNT, ///< Count | ||
53 | }; | ||
54 | |||
55 | /// @brief All position keys | ||
56 | template<typename StateKeyType> | ||
57 | constexpr std::array<StateKeyType, 3> Pos = { Keys::PosX, Keys::PosY, Keys::PosZ }; | ||
58 | /// @brief All velocity keys | ||
59 | template<typename StateKeyType> | ||
60 | constexpr std::array<StateKeyType, 3> Vel = { Keys::VelX, Keys::VelY, Keys::VelZ }; | ||
61 | /// @brief All attitude keys | ||
62 | template<typename StateKeyType> | ||
63 | constexpr std::array<StateKeyType, 4> Att = { Keys::AttQ1, Keys::AttQ2, Keys::AttQ3, Keys::AttQ0 }; | ||
64 | |||
65 | /// @brief Vector with all position and velocity keys | ||
66 | template<typename StateKeyType> | ||
67 | constexpr std::array<StateKeyType, 6> PosVel = { Keys::PosX, Keys::PosY, Keys::PosZ, | ||
68 | Keys::VelX, Keys::VelY, Keys::VelZ }; | ||
69 | /// @brief Vector with all position velocity and attitude keys | ||
70 | template<typename StateKeyType> | ||
71 | constexpr std::array<StateKeyType, 10> PosVelAtt = { Keys::PosX, Keys::PosY, Keys::PosZ, | ||
72 | Keys::VelX, Keys::VelY, Keys::VelZ, | ||
73 | Keys::AttQ1, Keys::AttQ2, Keys::AttQ3, Keys::AttQ0 }; | ||
74 | |||
75 | } // namespace Keys | ||
76 | |||
77 | /// Motion System Model | ||
78 | template<typename StateKeyType> | ||
79 | class MotionModel | ||
80 | { | ||
81 | public: | ||
82 | /// @brief Initializes the motion model | ||
83 | 21 | void initialize() | |
84 | { | ||
85 |
2/2✓ Branch 0 taken 42 times.
✓ Branch 1 taken 21 times.
|
126 | for (size_t i = 0; i < _gui_covarianceAccel.size(); i++) |
86 | { | ||
87 | // Covariance of the acceleration 𝜎_a due to user motion in horizontal and vertical component [m²/s³] | ||
88 | 42 | _covarianceAccel.at(i) = convertUnit(_gui_covarianceAccel.at(i), _gui_covarianceAccelUnit); | |
89 | } | ||
90 | 21 | } | |
91 | |||
92 | /// @brief Initializes the motion model | ||
93 | /// @param[in, out] F System model matrix | ||
94 | /// @param[in, out] W Noise scale matrix | ||
95 | template<typename Scalar, int Size> | ||
96 | 21 | void initialize(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& F, | |
97 | KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& W) | ||
98 | { | ||
99 | 21 | initialize(); | |
100 | |||
101 |
3/6✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
|
21 | F.template block<3>(Pos, Vel) = Eigen::Matrix3d::Identity(); |
102 |
3/6✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 21 times.
✗ Branch 13 not taken.
|
21 | W.template block<3>(Vel, Vel) = Eigen::DiagonalMatrix<double, 3>(_covarianceAccel[0], _covarianceAccel[0], _covarianceAccel[1]); |
103 | 21 | } | |
104 | |||
105 | /// @brief Updates the provided Phi, Q and G matrix | ||
106 | /// @param[in, out] Phi State transition matrix | ||
107 | /// @param[in, out] Q System/Process noise covariance matrix | ||
108 | /// @param[in, out] G Noise input matrix | ||
109 | /// @param[in] F System model matrix | ||
110 | /// @param[in] W Noise scale matrix | ||
111 | /// @param[in] dt Time step size in [s] | ||
112 | /// @param[in] latitude Latitude [rad] | ||
113 | /// @param[in] longitude Longitude [rad] | ||
114 | /// @param[in] algorithm Algorithm to use for the calculation | ||
115 | template<typename Scalar, int Size> | ||
116 | ✗ | void updatePhiAndQ(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Phi, | |
117 | KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Q, | ||
118 | KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& G, | ||
119 | const KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& F, | ||
120 | const KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& W, | ||
121 | double dt, | ||
122 | const double& latitude, | ||
123 | const double& longitude, | ||
124 | SystemModelCalcAlgorithm algorithm) | ||
125 | { | ||
126 | ✗ | if (algorithm == SystemModelCalcAlgorithm::VanLoan) | |
127 | { | ||
128 | ✗ | G.template block<3>(Vel, Vel) = trafo::e_Quat_n(latitude, longitude).toRotationMatrix(); | |
129 | ✗ | auto [PhiMot, QMot] = NAV::calcPhiAndQWithVanLoanMethod( | |
130 | ✗ | F.template block<6>(PosVel, PosVel), | |
131 | ✗ | G.template block<6>(PosVel, PosVel), | |
132 | ✗ | W.template block<6>(PosVel, PosVel), | |
133 | dt); | ||
134 | ✗ | Phi.template block<6>(PosVel, PosVel) = PhiMot; | |
135 | ✗ | Q.template block<6>(PosVel, PosVel) = QMot; | |
136 | } | ||
137 | else // QCalculationAlgorithm::Taylor1 | ||
138 | { | ||
139 | ✗ | Phi.template block<6>(PosVel, PosVel) = transitionMatrix_Phi_Taylor(F.template block<6>(PosVel, PosVel), dt, 1); | |
140 | ✗ | Q.template block<6>(PosVel, PosVel) = calcProcessNoiseMatrixTaylor(dt, latitude, longitude)(all, all); | |
141 | } | ||
142 | ✗ | } | |
143 | |||
144 | /// @brief Calculates the state transition matrix (𝚽) and the process noise covariance matrix (𝐐) | ||
145 | /// @param[in] dt Time step size in [s] | ||
146 | /// @param[in] latitude Latitude [rad] | ||
147 | /// @param[in] longitude Longitude [rad] | ||
148 | /// @param[in] algorithm Algorithm to use for the calculation | ||
149 | /// @return Phi and Q matrix | ||
150 | [[nodiscard]] std::pair<KeyedMatrix6d<StateKeyType>, KeyedMatrix6d<StateKeyType>> | ||
151 | calcPhiAndQ(double dt, const double& latitude, const double& longitude, SystemModelCalcAlgorithm algorithm) | ||
152 | { | ||
153 | KeyedMatrix6d<StateKeyType> F(Eigen::Matrix6d::Zero(), PosVel, PosVel); | ||
154 | KeyedMatrix6d<StateKeyType> G(Eigen::Matrix6d::Zero(), PosVel, PosVel); | ||
155 | KeyedMatrix6d<StateKeyType> W(Eigen::Matrix6d::Zero(), PosVel, PosVel); | ||
156 | initialize(F, W); | ||
157 | |||
158 | KeyedMatrix6d<StateKeyType> Phi(Eigen::Matrix6d::Zero(), PosVel, PosVel); | ||
159 | KeyedMatrix6d<StateKeyType> Q(Eigen::Matrix6d::Zero(), PosVel, PosVel); | ||
160 | updatePhiAndQ(Phi, Q, G, F, W, dt, latitude, longitude, algorithm); | ||
161 | |||
162 | return { Phi, Q }; | ||
163 | } | ||
164 | |||
165 | /// @brief Shows a GUI | ||
166 | /// @param[in] itemWidth Width of the space for the config items | ||
167 | /// @param[in] unitWidth Width of the units | ||
168 | /// @param[in] id Unique id for ImGui | ||
169 | /// @return True if something was changed | ||
170 | ✗ | bool ShowGui(float itemWidth, float unitWidth, const char* id) | |
171 | { | ||
172 | ✗ | bool changed = false; | |
173 | |||
174 | ✗ | if (gui::widgets::InputDouble2WithUnit(fmt::format("{} of the acceleration due to user motion (Hor/Ver)##{}", | |
175 | ✗ | _gui_covarianceAccelUnit == Units::CovarianceAccelUnits::m_sqrts3 | |
176 | ✗ | ? "StdDev" | |
177 | : "Variance", | ||
178 | id) | ||
179 | .c_str(), | ||
180 | ✗ | itemWidth, unitWidth, _gui_covarianceAccel.data(), _gui_covarianceAccelUnit, | |
181 | MakeComboItems<Units::CovarianceAccelUnits>().c_str(), //"m/√(s^3)\0m^2/s^3\0\0", | ||
182 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
183 | { | ||
184 | ✗ | LOG_DEBUG("{}: _gui_covarianceAccel changed to {}", id, fmt::join(_gui_covarianceAccel.begin(), _gui_covarianceAccel.end(), ", ")); | |
185 | ✗ | LOG_DEBUG("{}: _gui_covarianceAccelUnit changed to {}", id, to_string(_gui_covarianceAccelUnit)); | |
186 | ✗ | changed = true; | |
187 | } | ||
188 | ✗ | ImGui::SameLine(); | |
189 | ✗ | gui::widgets::HelpMarker(fmt::format("Suitable values for the horizontal acceleration are around\n" | |
190 | "- 1 {} for a pedestrian or ship,\n" | ||
191 | "- {} {} for a car, and\n" | ||
192 | "- {} {} for a military aircraft.\n" | ||
193 | "The vertical acceleration PSD is usually smaller.", | ||
194 | to_string(_gui_covarianceAccelUnit), | ||
195 | ✗ | _gui_covarianceAccelUnit == Units::CovarianceAccelUnits::m_sqrts3 ? 3 : 10, | |
196 | to_string(_gui_covarianceAccelUnit), | ||
197 | ✗ | _gui_covarianceAccelUnit == Units::CovarianceAccelUnits::m_sqrts3 ? 10 : 100, | |
198 | to_string(_gui_covarianceAccelUnit)) | ||
199 | .c_str()); | ||
200 | |||
201 | ✗ | return changed; | |
202 | } | ||
203 | |||
204 | private: | ||
205 | /// @brief All position keys | ||
206 | const std::array<StateKeyType, 3>& Pos = Keys::Pos<StateKeyType>; | ||
207 | /// @brief All velocity keys | ||
208 | const std::array<StateKeyType, 3>& Vel = Keys::Vel<StateKeyType>; | ||
209 | /// @brief All position and velocity keys | ||
210 | const std::array<StateKeyType, 6>& PosVel = Keys::PosVel<StateKeyType>; | ||
211 | |||
212 | /// @brief Calculates the process noise matrix Q | ||
213 | /// @param[in] dt Time step [s] | ||
214 | /// @param[in] latitude Latitude [rad] | ||
215 | /// @param[in] longitude Longitude [rad] | ||
216 | /// @note See \cite Groves2013 Groves, ch. 9.4.2.2, eq. 9.152, p. 417 | ||
217 | [[nodiscard]] KeyedMatrix6d<StateKeyType> | ||
218 | ✗ | calcProcessNoiseMatrixTaylor(double dt, double latitude, double longitude) const | |
219 | { | ||
220 | // Scaling matrix in n-frame | ||
221 | ✗ | Eigen::Matrix3d a_S_n = Eigen::DiagonalMatrix<double, 3>(_covarianceAccel[0], | |
222 | _covarianceAccel[0], | ||
223 | _covarianceAccel[1]); | ||
224 | // Scaling matrix in e-frame | ||
225 | ✗ | Eigen::Matrix3d a_S_e = trafo::e_Quat_n(latitude, longitude) * a_S_n * trafo::n_Quat_e(latitude, longitude); | |
226 | |||
227 | ✗ | double dt2 = std::pow(dt, 2); | |
228 | ✗ | double dt3 = dt2 * dt; | |
229 | |||
230 | ✗ | KeyedMatrix6d<StateKeyType> Q(Eigen::Matrix6d::Zero(), PosVel, PosVel); | |
231 | |||
232 | // Groves ch. 9.4.2.2, eq. 9.152, p. 417 | ||
233 | ✗ | Q.template block<3>(Pos, Pos) = dt3 / 3.0 * a_S_e; | |
234 | ✗ | Q.template block<3>(Pos, Vel) = dt2 / 2.0 * a_S_e; | |
235 | ✗ | Q.template block<3>(Vel, Pos) = Q.template block<3>(Pos, Vel).transpose(); | |
236 | ✗ | Q.template block<3>(Vel, Vel) = dt * a_S_e; | |
237 | |||
238 | ✗ | return Q; | |
239 | ✗ | } | |
240 | |||
241 | /// Gui selection for the Unit of the input covarianceAccel parameter for the StDev due to acceleration due to user motion | ||
242 | Units::CovarianceAccelUnits _gui_covarianceAccelUnit = Units::CovarianceAccelUnits::m_sqrts3; | ||
243 | /// @brief GUI selection for the Standard deviation of the acceleration 𝜎_a due to user motion in horizontal and vertical component | ||
244 | /// @note See Groves (2013) eq. (9.156) | ||
245 | std::array<double, 2> _gui_covarianceAccel = { 3.0, 1.5 } /* [ m / √(s^3) ] */; | ||
246 | |||
247 | /// @brief Covariance of the acceleration 𝜎_a due to user motion in horizontal and vertical component [m²/s³] | ||
248 | std::array<double, 2> _covarianceAccel = { 3.0, 1.5 }; | ||
249 | |||
250 | /// @brief Converts the provided data into a json object | ||
251 | /// @param[out] j Json object which gets filled with the info | ||
252 | /// @param[in] data Data to convert into json | ||
253 | ✗ | friend void to_json(json& j, const MotionModel& data) | |
254 | { | ||
255 | ✗ | j = { | |
256 | ✗ | { "covarianceAccelUnit", data._gui_covarianceAccelUnit }, | |
257 | ✗ | { "covarianceAccel", data._gui_covarianceAccel }, | |
258 | }; | ||
259 | ✗ | } | |
260 | /// @brief Converts the provided json object into the data object | ||
261 | /// @param[in] j Json object with the needed values | ||
262 | /// @param[out] data Object to fill from the json | ||
263 | ✗ | friend void from_json(const json& j, MotionModel& data) | |
264 | { | ||
265 | ✗ | if (j.contains("covarianceAccelUnit")) { j.at("covarianceAccelUnit").get_to(data._gui_covarianceAccelUnit); } | |
266 | ✗ | if (j.contains("covarianceAccel")) { j.at("covarianceAccel").get_to(data._gui_covarianceAccel); } | |
267 | ✗ | } | |
268 | }; | ||
269 | |||
270 | } // namespace NAV | ||
271 | |||
272 | /// @brief Stream insertion operator overload | ||
273 | /// @param[in, out] os Output stream object to stream the time into | ||
274 | /// @param[in] obj Object to print | ||
275 | /// @return Returns the output stream object in order to chain stream insertions | ||
276 | std::ostream& operator<<(std::ostream& os, const NAV::Keys::MotionModelKey& obj); | ||
277 | |||
278 | #ifndef DOXYGEN_IGNORE | ||
279 | |||
280 | /// @brief Formatter | ||
281 | template<> | ||
282 | struct fmt::formatter<NAV::Keys::MotionModelKey> : fmt::formatter<const char*> | ||
283 | { | ||
284 | /// @brief Defines how to format structs | ||
285 | /// @param[in] state Struct to format | ||
286 | /// @param[in, out] ctx Format context | ||
287 | /// @return Output iterator | ||
288 | template<typename FormatContext> | ||
289 | ✗ | auto format(const NAV::Keys::MotionModelKey& state, FormatContext& ctx) const | |
290 | { | ||
291 | using namespace NAV::Keys; // NOLINT(google-build-using-namespace) | ||
292 | |||
293 | ✗ | switch (state) | |
294 | { | ||
295 | ✗ | case PosX: | |
296 | ✗ | return fmt::formatter<const char*>::format("PosX", ctx); | |
297 | ✗ | case PosY: | |
298 | ✗ | return fmt::formatter<const char*>::format("PosY", ctx); | |
299 | ✗ | case PosZ: | |
300 | ✗ | return fmt::formatter<const char*>::format("PosZ", ctx); | |
301 | ✗ | case VelX: | |
302 | ✗ | return fmt::formatter<const char*>::format("VelX", ctx); | |
303 | ✗ | case VelY: | |
304 | ✗ | return fmt::formatter<const char*>::format("VelY", ctx); | |
305 | ✗ | case VelZ: | |
306 | ✗ | return fmt::formatter<const char*>::format("VelZ", ctx); | |
307 | ✗ | case AttQ1: | |
308 | ✗ | return fmt::formatter<const char*>::format("AttQ1", ctx); | |
309 | ✗ | case AttQ2: | |
310 | ✗ | return fmt::formatter<const char*>::format("AttQ2", ctx); | |
311 | ✗ | case AttQ3: | |
312 | ✗ | return fmt::formatter<const char*>::format("AttQ3", ctx); | |
313 | ✗ | case AttQ0: | |
314 | ✗ | return fmt::formatter<const char*>::format("AttQ0", ctx); | |
315 | ✗ | case MotionModelKey_COUNT: | |
316 | ✗ | return fmt::formatter<const char*>::format("MotionModelKey_COUNT", ctx); | |
317 | } | ||
318 | |||
319 | ✗ | return fmt::formatter<const char*>::format("ERROR", ctx); | |
320 | } | ||
321 | }; | ||
322 | |||
323 | #endif | ||
324 |