| 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 | 26 | void initialize() | |
| 84 | { | ||
| 85 |
2/2✓ Branch 0 taken 53 times.
✓ Branch 1 taken 27 times.
|
160 | 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 | 53 | _covarianceAccel.at(i) = convertUnit(_gui_covarianceAccel.at(i), _gui_covarianceAccelUnit); | |
| 89 | } | ||
| 90 | 27 | } | |
| 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 | 26 | void initialize(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& F, | |
| 97 | KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& W) | ||
| 98 | { | ||
| 99 | 26 | initialize(); | |
| 100 | |||
| 101 |
3/6✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
|
27 | F.template block<3>(Pos, Vel) = Eigen::Matrix3d::Identity(); |
| 102 |
3/6✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 27 times.
✗ Branch 13 not taken.
|
27 | W.template block<3>(Vel, Vel) = Eigen::DiagonalMatrix<double, 3>(_covarianceAccel[0], _covarianceAccel[0], _covarianceAccel[1]); |
| 103 | 27 | } | |
| 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 | 1 | friend void from_json(const json& j, MotionModel& data) | |
| 264 | { | ||
| 265 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("covarianceAccelUnit")) { j.at("covarianceAccelUnit").get_to(data._gui_covarianceAccelUnit); } |
| 266 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("covarianceAccel")) { j.at("covarianceAccel").get_to(data._gui_covarianceAccel); } |
| 267 | 1 | } | |
| 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 | 57696 | auto format(const NAV::Keys::MotionModelKey& state, FormatContext& ctx) const | |
| 290 | { | ||
| 291 | using namespace NAV::Keys; // NOLINT(google-build-using-namespace) | ||
| 292 | |||
| 293 |
6/12✓ Branch 0 taken 9616 times.
✓ Branch 1 taken 9616 times.
✓ Branch 2 taken 9616 times.
✓ Branch 3 taken 9616 times.
✓ Branch 4 taken 9616 times.
✓ Branch 5 taken 9616 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
57696 | switch (state) |
| 294 | { | ||
| 295 | 9616 | case PosX: | |
| 296 |
1/2✓ Branch 1 taken 9616 times.
✗ Branch 2 not taken.
|
9616 | return fmt::formatter<const char*>::format("PosX", ctx); |
| 297 | 9616 | case PosY: | |
| 298 |
1/2✓ Branch 1 taken 9616 times.
✗ Branch 2 not taken.
|
9616 | return fmt::formatter<const char*>::format("PosY", ctx); |
| 299 | 9616 | case PosZ: | |
| 300 |
1/2✓ Branch 1 taken 9616 times.
✗ Branch 2 not taken.
|
9616 | return fmt::formatter<const char*>::format("PosZ", ctx); |
| 301 | 9616 | case VelX: | |
| 302 |
1/2✓ Branch 1 taken 9616 times.
✗ Branch 2 not taken.
|
9616 | return fmt::formatter<const char*>::format("VelX", ctx); |
| 303 | 9616 | case VelY: | |
| 304 |
1/2✓ Branch 1 taken 9616 times.
✗ Branch 2 not taken.
|
9616 | return fmt::formatter<const char*>::format("VelY", ctx); |
| 305 | 9616 | case VelZ: | |
| 306 |
1/2✓ Branch 1 taken 9616 times.
✗ Branch 2 not taken.
|
9616 | 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 |