INSTINCT Code Coverage Report

Directory: src/
File: NodeData/State/InsGnssLCKFSolution.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 38 112 33.9%
Functions: 5 7 71.4%
Branches: 30 169 17.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 InsGnssLCKFSolution.hpp
10 /// @brief Loosely-coupled Kalman Filter INS/GNSS errors
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2022-06-08
13
14 #pragma once
15
16 #include "util/Eigen.hpp"
17 #include "PosVelAtt.hpp"
18 #include <cstdint>
19
20 #include "Navigation/Transformations/Units.hpp"
21
22 namespace NAV
23 {
24 /// Loosely-coupled Kalman Filter INS/GNSS errors
25 class InsGnssLCKFSolution : public PosVelAtt
26 {
27 public:
28 /// @brief Returns the type of the data class
29 /// @return The data type
30 582168 [[nodiscard]] static std::string type()
31 {
32
1/2
✓ Branch 1 taken 582168 times.
✗ Branch 2 not taken.
 1164336 return "InsGnssLCKFSolution";
33 }
34
35 /// @brief Returns the type of the data class
36 /// @return The data type
37 ✗ [[nodiscard]] std::string getType() const override { return type(); }
38
39 /// @brief Returns the parent types of the data class
40 /// @return The parent data types
41 112 [[nodiscard]] static std::vector<std::string> parentTypes()
42 {
43 112 auto parent = PosVelAtt::parentTypes();
44
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 112 parent.push_back(PosVelAtt::type());
45 112 return parent;
46 ✗ }
47
48 /// @brief Returns a vector of data descriptors
49 290688 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
50 {
51 290688 auto desc = PosVelAtt::GetStaticDataDescriptors();
52
1/2
✓ Branch 2 taken 290688 times.
✗ Branch 3 not taken.
 290688 desc.reserve(GetStaticDescriptorCount());
53
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Roll error [deg]");
54
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Pitch error [deg]");
55
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Yaw error [deg]");
56
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Position North error [deg]");
57
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Position East error [deg]");
58
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Position Down error [m]");
59
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Velocity North error [m/s]");
60
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Velocity East error [m/s]");
61
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Velocity Down error [m/s]");
62
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Alpha_eb [deg]");
63
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Beta_eb [deg]");
64
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Gamma_eb [deg]");
65
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Position ECEF X error [m]");
66
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Position ECEF Y error [m]");
67
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Position ECEF Z error [m]");
68
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Velocity ECEF X error [m/s]");
69
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Velocity ECEF Y error [m/s]");
70
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("KF State Velocity ECEF Z error [m/s]");
71
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("Accelerometer bias b_X [m/s^2]");
72
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("Accelerometer bias b_Y [m/s^2]");
73
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("Accelerometer bias b_Z [m/s^2]");
74
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("Gyroscope bias b_X [rad/s]");
75
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("Gyroscope bias b_Y [rad/s]");
76
1/2
✓ Branch 1 taken 290688 times.
✗ Branch 2 not taken.
 290688 desc.emplace_back("Gyroscope bias b_Z [rad/s]");
77 290688 return desc;
78 ✗ }
79
80 /// @brief Get the number of descriptors
81 436020 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVelAtt::GetStaticDescriptorCount() + 24; }
82
83 /// @brief Returns a vector of data descriptors
84 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
85
86 /// @brief Get the value at the index
87 /// @param idx Index corresponding to data descriptor order
88 /// @return Value if in the observation
89 145332 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
90 {
91
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 145332 times.
 145332 INS_ASSERT(idx < GetStaticDescriptorCount());
92
1/2
✓ Branch 1 taken 145332 times.
✗ Branch 2 not taken.
 145332 if (idx < PosVelAtt::GetStaticDescriptorCount()) { return PosVelAtt::getValueAt(idx); }
93 ✗ switch (idx)
94 {
95 ✗ case PosVelAtt::GetStaticDescriptorCount() + 0: // KF State Roll error [deg]
96 ✗ if (frame == Frame::NED) { return rad2deg(attitudeError(0)); }
97 ✗ break;
98 ✗ case PosVelAtt::GetStaticDescriptorCount() + 1: // KF State Pitch error [deg]
99 ✗ if (frame == Frame::NED) { return rad2deg(attitudeError(1)); }
100 ✗ break;
101 ✗ case PosVelAtt::GetStaticDescriptorCount() + 2: // KF State Yaw error [deg]
102 ✗ if (frame == Frame::NED) { return rad2deg(attitudeError(2)); }
103 ✗ break;
104 ✗ case PosVelAtt::GetStaticDescriptorCount() + 3: // KF State Position North error [deg]
105 ✗ if (frame == Frame::NED) { return rad2deg(positionError(0)); }
106 ✗ break;
107 ✗ case PosVelAtt::GetStaticDescriptorCount() + 4: // KF State Position East error [deg]
108 ✗ if (frame == Frame::NED) { return rad2deg(positionError(1)); }
109 ✗ break;
110 ✗ case PosVelAtt::GetStaticDescriptorCount() + 5: // KF State Position Down error [m]
111 ✗ if (frame == Frame::NED) { return positionError(2); }
112 ✗ break;
113 ✗ case PosVelAtt::GetStaticDescriptorCount() + 6: // KF State Velocity North error [m/s]
114 ✗ if (frame == Frame::NED) { return velocityError(0); }
115 ✗ break;
116 ✗ case PosVelAtt::GetStaticDescriptorCount() + 7: // KF State Velocity East error [m/s]
117 ✗ if (frame == Frame::NED) { return velocityError(1); }
118 ✗ break;
119 ✗ case PosVelAtt::GetStaticDescriptorCount() + 8: // KF State Velocity Down error [m/s]
120 ✗ if (frame == Frame::NED) { return velocityError(2); }
121 ✗ break;
122 ✗ case PosVelAtt::GetStaticDescriptorCount() + 9: // KF State Alpha_eb [deg]
123 ✗ if (frame == Frame::ECEF) { return rad2deg(attitudeError(0)); }
124 ✗ break;
125 ✗ case PosVelAtt::GetStaticDescriptorCount() + 10: // KF State Beta_eb [deg]
126 ✗ if (frame == Frame::ECEF) { return rad2deg(attitudeError(1)); }
127 ✗ break;
128 ✗ case PosVelAtt::GetStaticDescriptorCount() + 11: // KF State Gamma_eb [deg]
129 ✗ if (frame == Frame::ECEF) { return rad2deg(attitudeError(2)); }
130 ✗ break;
131 ✗ case PosVelAtt::GetStaticDescriptorCount() + 12: // KF State Position ECEF X error [m]
132 ✗ if (frame == Frame::ECEF) { return positionError(0); }
133 ✗ break;
134 ✗ case PosVelAtt::GetStaticDescriptorCount() + 13: // KF State Position ECEF Y error [m]
135 ✗ if (frame == Frame::ECEF) { return positionError(1); }
136 ✗ break;
137 ✗ case PosVelAtt::GetStaticDescriptorCount() + 14: // KF State Position ECEF Z error [m]
138 ✗ if (frame == Frame::ECEF) { return positionError(2); }
139 ✗ break;
140 ✗ case PosVelAtt::GetStaticDescriptorCount() + 15: // KF State Velocity ECEF X error [m/s]
141 ✗ if (frame == Frame::ECEF) { return velocityError(0); }
142 ✗ break;
143 ✗ case PosVelAtt::GetStaticDescriptorCount() + 16: // KF State Velocity ECEF Y error [m/s]
144 ✗ if (frame == Frame::ECEF) { return velocityError(1); }
145 ✗ break;
146 ✗ case PosVelAtt::GetStaticDescriptorCount() + 17: // KF State Velocity ECEF Z error [m/s]
147 ✗ if (frame == Frame::ECEF) { return velocityError(2); }
148 ✗ break;
149 ✗ case PosVelAtt::GetStaticDescriptorCount() + 18: // Accelerometer bias b_X [m/s^2]
150 ✗ return b_biasAccel(0);
151 ✗ case PosVelAtt::GetStaticDescriptorCount() + 19: // Accelerometer bias b_Y [m/s^2]
152 ✗ return b_biasAccel(1);
153 ✗ case PosVelAtt::GetStaticDescriptorCount() + 20: // Accelerometer bias b_Z [m/s^2]
154 ✗ return b_biasAccel(2);
155 ✗ case PosVelAtt::GetStaticDescriptorCount() + 21: // Gyroscope bias b_X [rad/s]
156 ✗ return b_biasGyro(0);
157 ✗ case PosVelAtt::GetStaticDescriptorCount() + 22: // Gyroscope bias b_Y [rad/s]
158 ✗ return b_biasGyro(1);
159 ✗ case PosVelAtt::GetStaticDescriptorCount() + 23: // Gyroscope bias b_Z [rad/s]
160 ✗ return b_biasGyro(2);
161 ✗ default:
162 ✗ return std::nullopt;
163 }
164 ✗ return std::nullopt;
165 }
166
167 /// @brief Available Frames
168 enum class Frame : uint8_t
169 {
170 ECEF, ///< Earth-Centered Earth-Fixed frame
171 NED, ///< Local North-East-Down frame
172 };
173 /// Frame in which the errors are set
174 Frame frame = Frame::NED;
175
176 /// δ𝛙_{i,e,n}b_{i,e,n} The attitude error in {i,e,n} frame coordinates in [rad]
177 Eigen::Vector3d attitudeError{ 0, 0, 0 };
178 /// δ𝐯_{i,e,n} The velocity error in {i,e,n} coordinates in [m/s]
179 Eigen::Vector3d velocityError{ 0, 0, 0 };
180 /// NED: δ𝐩 = [δ𝜙 δλ δ𝘩] The position error (latitude, longitude, altitude) in [rad, rad, m]
181 /// ECEF/i: δr The position error in [m]
182 Eigen::Vector3d positionError{ 0, 0, 0 };
183
184 /// 𝐛_a The accelerometer bias in body frame in [m/s^2]
185 Eigen::Vector3d b_biasAccel{ 0, 0, 0 };
186 /// 𝐛_g The gyroscope bias in body frame in [rad/s]
187 Eigen::Vector3d b_biasGyro{ 0, 0, 0 };
188 };
189
190 } // namespace NAV
191