INSTINCT Code Coverage Report


Directory: src/
File: Navigation/GNSS/SystemModel/MotionModel.hpp
Date: 2025-06-06 13:17:47
Exec Total Coverage
Lines: 9 81 11.1%
Functions: 2 8 25.0%
Branches: 8 172 4.7%

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