0.4.1
Loading...
Searching...
No Matches
MotionModel.hpp
Go to the documentation of this file.
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>
19#include <Eigen/Core>
20#include <cstdint>
21#include <imgui.h>
22#include <fmt/ranges.h>
23
29#include "Units.hpp"
32
33namespace NAV
34{
35
36namespace Keys
37{
38
39/// Keys used in the model
40enum 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
53};
54
55/// @brief All position keys
56template<typename StateKeyType>
57constexpr std::array<StateKeyType, 3> Pos = { Keys::PosX, Keys::PosY, Keys::PosZ };
58/// @brief All velocity keys
59template<typename StateKeyType>
60constexpr std::array<StateKeyType, 3> Vel = { Keys::VelX, Keys::VelY, Keys::VelZ };
61/// @brief All attitude keys
62template<typename StateKeyType>
63constexpr std::array<StateKeyType, 4> Att = { Keys::AttQ1, Keys::AttQ2, Keys::AttQ3, Keys::AttQ0 };
64
65/// @brief Vector with all position and velocity keys
66template<typename StateKeyType>
67constexpr std::array<StateKeyType, 6> PosVel = { Keys::PosX, Keys::PosY, Keys::PosZ,
69/// @brief Vector with all position velocity and attitude keys
70template<typename StateKeyType>
74
75} // namespace Keys
76
77/// Motion System Model
78template<typename StateKeyType>
80{
81 public:
82 /// @brief Initializes the motion model
84 {
85 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³]
89 }
90 }
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>
98 {
99 initialize();
100
101 F.template block<3>(Pos, Vel) = Eigen::Matrix3d::Identity();
102 W.template block<3>(Vel, Vel) = Eigen::DiagonalMatrix<double, 3>(_covarianceAccel[0], _covarianceAccel[0], _covarianceAccel[1]);
103 }
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>
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)##{}",
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.",
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],
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
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
276std::ostream& operator<<(std::ostream& os, const NAV::Keys::MotionModelKey& obj);
277
278#ifndef DOXYGEN_IGNORE
279
280/// @brief Formatter
281template<>
282struct 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);
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
Transformation collection.
nlohmann::json json
json namespace
Units used by the system model parameters.
Text Help Marker (?) with Tooltip.
Defines Widgets which allow the input of values and the selection of the unit.
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
Generalized Kalman Filter class.
std::ostream & operator<<(std::ostream &os, const NAV::Keys::MotionModelKey &obj)
Stream insertion operator overload.
System Model.
Van Loan Method loan1978.
Static sized KeyedMatrix.
Motion System Model.
friend void to_json(json &j, const MotionModel &data)
Converts the provided data into a json object.
const std::array< StateKeyType, 6 > & PosVel
All position and velocity keys.
void updatePhiAndQ(KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Phi, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Q, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &G, const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &F, const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &W, double dt, const double &latitude, const double &longitude, SystemModelCalcAlgorithm algorithm)
Updates the provided Phi, Q and G matrix.
std::array< double, 2 > _covarianceAccel
Covariance of the acceleration 𝜎_a due to user motion in horizontal and vertical component [m²/s³].
std::array< double, 2 > _gui_covarianceAccel
GUI selection for the Standard deviation of the acceleration 𝜎_a due to user motion in horizontal and...
void initialize()
Initializes the motion model.
const std::array< StateKeyType, 3 > & Vel
All velocity keys.
void initialize(KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &F, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &W)
Initializes the motion model.
bool ShowGui(float itemWidth, float unitWidth, const char *id)
Shows a GUI.
Units::CovarianceAccelUnits _gui_covarianceAccelUnit
Gui selection for the Unit of the input covarianceAccel parameter for the StDev due to acceleration d...
const std::array< StateKeyType, 3 > & Pos
All position keys.
KeyedMatrix6d< StateKeyType > calcProcessNoiseMatrixTaylor(double dt, double latitude, double longitude) const
Calculates the process noise matrix Q.
friend void from_json(const json &j, MotionModel &data)
Converts the provided json object into the data object.
std::pair< KeyedMatrix6d< StateKeyType >, KeyedMatrix6d< StateKeyType > > calcPhiAndQ(double dt, const double &latitude, const double &longitude, SystemModelCalcAlgorithm algorithm)
Calculates the state transition matrix (𝚽) and the process noise covariance matrix (𝐐)
constexpr std::array< StateKeyType, 4 > Att
All attitude keys.
constexpr std::array< StateKeyType, 3 > Pos
All position keys.
MotionModelKey
Keys used in the model.
@ PosY
Position ECEF_Y [m].
@ VelZ
Velocity ECEF_Z [m/s].
@ VelY
Velocity ECEF_Y [m/s].
@ VelX
Velocity ECEF_X [m/s].
@ AttQ0
w: Real (scalar) part of the Quaternion
@ PosX
Position ECEF_X [m].
@ AttQ3
z: Coefficient of k
@ AttQ1
x: Coefficient of i
@ AttQ2
y: Coefficient of j
@ PosZ
Position ECEF_Z [m].
@ MotionModelKey_COUNT
Count.
constexpr std::array< StateKeyType, 10 > PosVelAtt
Vector with all position velocity and attitude keys.
constexpr std::array< StateKeyType, 6 > PosVel
Vector with all position and velocity keys.
constexpr std::array< StateKeyType, 3 > Vel
All velocity keys.
CovarianceAccelUnits
Possible Units for the Standard deviation of the acceleration due to user motion.
Definition Units.hpp:29
@ m_sqrts3
[ m / √(s^3) ]
Definition Units.hpp:31
InputWithUnitChange InputDouble2WithUnit(const char *label, float itemWidth, float unitWidth, double v[2], U &combo_current_item, const char *combo_items_separated_by_zeros, const char *format="%.6f", ImGuiInputTextFlags flags=0, int combo_popup_max_height_in_items=-1)
Shows an InputText GUI element to modify the provided value and also set its unit.
void HelpMarker(const char *desc, const char *symbol="(?)")
Text Help Marker, e.g. '(?)', with Tooltip.
Eigen::Quaternion< Scalar > e_Quat_n(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from navigation to Earth-fixed frame.
Eigen::Quaternion< Scalar > n_Quat_e(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from Earth-fixed to navigation frame.
std::pair< typename DerivedF::PlainObject, typename DerivedF::PlainObject > calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase< DerivedF > &F, const Eigen::MatrixBase< DerivedG > &G, const Eigen::MatrixBase< DerivedW > &W, double dt)
Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matri...
Definition VanLoan.hpp:65
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Derived::PlainObject transitionMatrix_Phi_Taylor(const Eigen::MatrixBase< Derived > &F, double tau_s, size_t order)
Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ
double convertUnit(const double &value, Units::PositionUncertaintyUnits unit)
Converts the value depending on the unit provided.
Definition Units.cpp:135
std::string MakeComboItems()
Units separated by '\0' and terminated by double '\0'.
static const internal::all_t all
Used to request all rows or columns in KeyedMatrices.
SystemModelCalcAlgorithm
Algorithms to calculate the system model with.
KeyedMatrix< double, RowKeyType, ColKeyType, 6, 6 > KeyedMatrix6d
Static 6x6 squared size KeyedMatrix with double types.
Matrix which can be accessed by keys.