INSTINCT Code Coverage Report

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