INSTINCT Code Coverage Report

Directory: src/
File: NodeData/IMU/ImuObsSimulated.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 23 102 22.5%
Functions: 4 10 40.0%
Branches: 19 96 19.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 ImuObsSimulated.hpp
10 /// @brief Data storage class for simulated IMU observations
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-11-20
13
14 #pragma once
15
16 #include "ImuObsWDelta.hpp"
17
18 namespace NAV
19 {
20 /// VectorNav Observation storage Class
21 class ImuObsSimulated final : public ImuObsWDelta
22 {
23 public:
24 /// @brief Constructor
25 /// @param[in] imuPos Reference to the position and rotation info of the Imu
26 35214 explicit ImuObsSimulated(const ImuPos& imuPos)
27
4/8
✓ Branch 2 taken 35207 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35207 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 35206 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 35208 times.
✗ Branch 12 not taken.
 35214 : ImuObsWDelta(imuPos) {}
28
29 /// @brief Returns the type of the data class
30 /// @return The data type
31 60677 [[nodiscard]] static std::string type()
32 {
33
1/2
✓ Branch 1 taken 60667 times.
✗ Branch 2 not taken.
 121344 return "ImuObsSimulated";
34 }
35
36 /// @brief Returns the type of the data class
37 /// @return The data type
38 ✗ [[nodiscard]] std::string getType() const override { return type(); }
39
40 /// @brief Returns the parent types of the data class
41 /// @return The parent data types
42 112 [[nodiscard]] static std::vector<std::string> parentTypes()
43 {
44 112 auto parent = ImuObsWDelta::parentTypes();
45
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 112 parent.push_back(ImuObsWDelta::type());
46 112 return parent;
47 ✗ }
48
49 /// @brief Returns a vector of data descriptors
50 4 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
51 {
52 4 auto desc = ImuObsWDelta::GetStaticDataDescriptors();
53
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AccelDynamicsN [m/s^2]");
54
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AccelDynamicsE [m/s^2]");
55
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AccelDynamicsD [m/s^2]");
56
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AngularRateN (ω_nb_n) [rad/s]");
57
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AngularRateE (ω_nb_n) [rad/s]");
58
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AngularRateD (ω_nb_n) [rad/s]");
59
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AccelDynamicsX ECEF [m/s^2]");
60
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AccelDynamicsY ECEF [m/s^2]");
61
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AccelDynamicsZ ECEF [m/s^2]");
62
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AngularRateX ECEF (ω_nb_e) [rad/s]");
63
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AngularRateY ECEF (ω_nb_e) [rad/s]");
64
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 desc.emplace_back("AngularRateZ ECEF (ω_nb_e) [rad/s]");
65 4 return desc;
66 ✗ }
67
68 /// @brief Get the amount of descriptors
69 ✗ [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return ImuObsWDelta::GetStaticDescriptorCount() + 12; }
70
71 /// @brief Returns a vector of data descriptors
72 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
73
74 /// @brief Get the amount of descriptors
75 ✗ [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
76
77 /// @brief Get the value at the index
78 /// @param idx Index corresponding to data descriptor order
79 /// @return Value if in the observation
80 ✗ [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
81 {
82 ✗ INS_ASSERT(idx < GetStaticDescriptorCount());
83 ✗ if (idx < ImuObsWDelta::GetStaticDescriptorCount()) { return ImuObsWDelta::getValueAt(idx); }
84 ✗ switch (idx)
85 {
86 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 0: // AccelDynamicsN [m/s^2]
87 ✗ return n_accelDynamics.x();
88 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 1: // AccelDynamicsE [m/s^2]
89 ✗ return n_accelDynamics.y();
90 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 2: // AccelDynamicsD [m/s^2]
91 ✗ return n_accelDynamics.z();
92 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 3: // AngularRateN (ω_nb_n) [rad/s]
93 ✗ return n_angularRateDynamics.x();
94 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 4: // AngularRateE (ω_nb_n) [rad/s]
95 ✗ return n_angularRateDynamics.y();
96 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 5: // AngularRateD (ω_nb_n) [rad/s]
97 ✗ return n_angularRateDynamics.z();
98 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 6: // AccelDynamicsX ECEF [m/s^2]
99 ✗ return e_accelDynamics.x();
100 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 7: // AccelDynamicsY ECEF [m/s^2]
101 ✗ return e_accelDynamics.y();
102 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 8: // AccelDynamicsZ ECEF [m/s^2]
103 ✗ return e_accelDynamics.z();
104 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 9: // AngularRateX ECEF (ω_nb_e) [rad/s]
105 ✗ return e_angularRateDynamics.x();
106 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 10: // AngularRateY ECEF (ω_nb_e) [rad/s]
107 ✗ return e_angularRateDynamics.y();
108 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 11: // AngularRateZ ECEF (ω_nb_e) [rad/s]
109 ✗ return e_angularRateDynamics.z();
110 ✗ default:
111 ✗ return std::nullopt;
112 }
113 }
114
115 /// @brief Set the value at the index
116 /// @param idx Index corresponding to data descriptor order
117 /// @param value Value to set
118 /// @return True if the value was updated
119 ✗ [[nodiscard]] bool setValueAt(size_t idx, double value) override
120 {
121 ✗ INS_ASSERT(idx < GetStaticDescriptorCount());
122 ✗ if (idx < ImuObsWDelta::GetStaticDescriptorCount()) { return ImuObsWDelta::setValueAt(idx, value); }
123 ✗ switch (idx)
124 {
125 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 0: // AccelDynamicsN [m/s^2]
126 ✗ n_accelDynamics.x() = value;
127 ✗ break;
128 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 1: // AccelDynamicsE [m/s^2]
129 ✗ n_accelDynamics.y() = value;
130 ✗ break;
131 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 2: // AccelDynamicsD [m/s^2]
132 ✗ n_accelDynamics.z() = value;
133 ✗ break;
134 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 3: // AngularRateN (ω_nb_n) [rad/s]
135 ✗ n_angularRateDynamics.x() = value;
136 ✗ break;
137 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 4: // AngularRateE (ω_nb_n) [rad/s]
138 ✗ n_angularRateDynamics.y() = value;
139 ✗ break;
140 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 5: // AngularRateD (ω_nb_n) [rad/s]
141 ✗ n_angularRateDynamics.z() = value;
142 ✗ break;
143 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 6: // AccelDynamicsX ECEF [m/s^2]
144 ✗ e_accelDynamics.x() = value;
145 ✗ break;
146 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 7: // AccelDynamicsY ECEF [m/s^2]
147 ✗ e_accelDynamics.y() = value;
148 ✗ break;
149 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 8: // AccelDynamicsZ ECEF [m/s^2]
150 ✗ e_accelDynamics.z() = value;
151 ✗ break;
152 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 9: // AngularRateX ECEF (ω_nb_e) [rad/s]
153 ✗ e_angularRateDynamics.x() = value;
154 ✗ break;
155 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 10: // AngularRateY ECEF (ω_nb_e) [rad/s]
156 ✗ e_angularRateDynamics.y() = value;
157 ✗ break;
158 ✗ case ImuObsWDelta::GetStaticDescriptorCount() + 11: // AngularRateZ ECEF (ω_nb_e) [rad/s]
159 ✗ e_angularRateDynamics.z() = value;
160 ✗ break;
161 ✗ default:
162 ✗ return false;
163 }
164
165 ✗ return true;
166 }
167
168 /// The acceleration derived from the trajectory in [m/s^2], given in the NED frame.
169 Eigen::Vector3d n_accelDynamics;
170 /// The angular rate ω_nb_n derived from the trajectory in [rad/s], given in the NED frame.
171 Eigen::Vector3d n_angularRateDynamics;
172
173 /// The acceleration derived from the trajectory in [m/s^2], given in the ECEF frame.
174 Eigen::Vector3d e_accelDynamics;
175 /// The angular rate ω_nb_e derived from the trajectory in [rad/s], given in the ECEF frame.
176 Eigen::Vector3d e_angularRateDynamics;
177 };
178
179 } // namespace NAV
180