INSTINCT Code Coverage Report

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