INSTINCT Code Coverage Report

Directory: src/
File: NodeData/State/InsGnssLCKFSolution.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 49 143 34.3%
Functions: 6 8 75.0%
Branches: 40 211 19.0%
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 "PosVelAtt.hpp"
17 #include <cstdint>
18 #include "util/Container/UncertainValue.hpp"
19 #include "Navigation/Transformations/Units.hpp"
20 #include <Eigen/src/Core/Matrix.h>
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 582144 [[nodiscard]] static std::string type()
31 {
32
1/2
✓ Branch 1 taken 582144 times.
✗ Branch 2 not taken.
 1164288 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 290676 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
50 {
51 290676 auto desc = PosVelAtt::GetStaticDataDescriptors();
52
1/2
✓ Branch 2 taken 290676 times.
✗ Branch 3 not taken.
 290676 desc.reserve(GetStaticDescriptorCount());
53
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Roll error [deg]");
54
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Pitch error [deg]");
55
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Yaw error [deg]");
56
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Position North error [deg]");
57
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Position East error [deg]");
58
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Position Down error [m]");
59
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Velocity North error [m/s]");
60
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Velocity East error [m/s]");
61
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Velocity Down error [m/s]");
62
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Alpha_eb [deg]");
63
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Beta_eb [deg]");
64
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Gamma_eb [deg]");
65
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Position ECEF X error [m]");
66
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Position ECEF Y error [m]");
67
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Position ECEF Z error [m]");
68
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Velocity ECEF X error [m/s]");
69
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Velocity ECEF Y error [m/s]");
70
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("KF State Velocity ECEF Z error [m/s]");
71
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Accelerometer bias b_X [m/s^2]");
72
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Accelerometer bias b_Y [m/s^2]");
73
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Accelerometer bias b_Z [m/s^2]");
74
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Accelerometer bias b_X StdDev [m/s^2]");
75
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Accelerometer bias b_Y StdDev [m/s^2]");
76
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Accelerometer bias b_Z StdDev [m/s^2]");
77
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Gyroscope bias b_X [rad/s]");
78
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Gyroscope bias b_Y [rad/s]");
79
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Gyroscope bias b_Z [rad/s]");
80
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Gyroscope bias b_X StdDev [rad/s]");
81
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Gyroscope bias b_Y StdDev [rad/s]");
82
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Gyroscope bias b_Z StdDev [rad/s]");
83
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Barometric height bias [m]");
84
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Barometric height bias StdDev [m]");
85
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Barometric height scale [m/m]");
86
1/2
✓ Branch 1 taken 290676 times.
✗ Branch 2 not taken.
 290676 desc.emplace_back("Barometric height scale StdDev [m/m]");
87
88 290676 return desc;
89 ✗ }
90
91 /// @brief Get the number of descriptors
92 581328 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVelAtt::GetStaticDescriptorCount() + 34; }
93
94 /// @brief Returns a vector of data descriptors
95 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
96
97 /// @brief Get the amount of descriptors
98 145326 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
99
100 /// @brief Get the value at the index
101 /// @param idx Index corresponding to data descriptor order
102 /// @return Value if in the observation
103 145326 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
104 {
105
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 145326 times.
 145326 INS_ASSERT(idx < GetStaticDescriptorCount());
106
1/2
✓ Branch 1 taken 145326 times.
✗ Branch 2 not taken.
 145326 if (idx < PosVelAtt::GetStaticDescriptorCount()) { return PosVelAtt::getValueAt(idx); }
107 ✗ switch (idx)
108 {
109 ✗ case PosVelAtt::GetStaticDescriptorCount() + 0: // KF State Roll error [deg]
110 ✗ if (frame == Frame::NED) { return rad2deg(attitudeError(0)); }
111 ✗ break;
112 ✗ case PosVelAtt::GetStaticDescriptorCount() + 1: // KF State Pitch error [deg]
113 ✗ if (frame == Frame::NED) { return rad2deg(attitudeError(1)); }
114 ✗ break;
115 ✗ case PosVelAtt::GetStaticDescriptorCount() + 2: // KF State Yaw error [deg]
116 ✗ if (frame == Frame::NED) { return rad2deg(attitudeError(2)); }
117 ✗ break;
118 ✗ case PosVelAtt::GetStaticDescriptorCount() + 3: // KF State Position North error [deg]
119 ✗ if (frame == Frame::NED) { return rad2deg(positionError(0)); }
120 ✗ break;
121 ✗ case PosVelAtt::GetStaticDescriptorCount() + 4: // KF State Position East error [deg]
122 ✗ if (frame == Frame::NED) { return rad2deg(positionError(1)); }
123 ✗ break;
124 ✗ case PosVelAtt::GetStaticDescriptorCount() + 5: // KF State Position Down error [m]
125 ✗ if (frame == Frame::NED) { return positionError(2); }
126 ✗ break;
127 ✗ case PosVelAtt::GetStaticDescriptorCount() + 6: // KF State Velocity North error [m/s]
128 ✗ if (frame == Frame::NED) { return velocityError(0); }
129 ✗ break;
130 ✗ case PosVelAtt::GetStaticDescriptorCount() + 7: // KF State Velocity East error [m/s]
131 ✗ if (frame == Frame::NED) { return velocityError(1); }
132 ✗ break;
133 ✗ case PosVelAtt::GetStaticDescriptorCount() + 8: // KF State Velocity Down error [m/s]
134 ✗ if (frame == Frame::NED) { return velocityError(2); }
135 ✗ break;
136 ✗ case PosVelAtt::GetStaticDescriptorCount() + 9: // KF State Alpha_eb [deg]
137 ✗ if (frame == Frame::ECEF) { return rad2deg(attitudeError(0)); }
138 ✗ break;
139 ✗ case PosVelAtt::GetStaticDescriptorCount() + 10: // KF State Beta_eb [deg]
140 ✗ if (frame == Frame::ECEF) { return rad2deg(attitudeError(1)); }
141 ✗ break;
142 ✗ case PosVelAtt::GetStaticDescriptorCount() + 11: // KF State Gamma_eb [deg]
143 ✗ if (frame == Frame::ECEF) { return rad2deg(attitudeError(2)); }
144 ✗ break;
145 ✗ case PosVelAtt::GetStaticDescriptorCount() + 12: // KF State Position ECEF X error [m]
146 ✗ if (frame == Frame::ECEF) { return positionError(0); }
147 ✗ break;
148 ✗ case PosVelAtt::GetStaticDescriptorCount() + 13: // KF State Position ECEF Y error [m]
149 ✗ if (frame == Frame::ECEF) { return positionError(1); }
150 ✗ break;
151 ✗ case PosVelAtt::GetStaticDescriptorCount() + 14: // KF State Position ECEF Z error [m]
152 ✗ if (frame == Frame::ECEF) { return positionError(2); }
153 ✗ break;
154 ✗ case PosVelAtt::GetStaticDescriptorCount() + 15: // KF State Velocity ECEF X error [m/s]
155 ✗ if (frame == Frame::ECEF) { return velocityError(0); }
156 ✗ break;
157 ✗ case PosVelAtt::GetStaticDescriptorCount() + 16: // KF State Velocity ECEF Y error [m/s]
158 ✗ if (frame == Frame::ECEF) { return velocityError(1); }
159 ✗ break;
160 ✗ case PosVelAtt::GetStaticDescriptorCount() + 17: // KF State Velocity ECEF Z error [m/s]
161 ✗ if (frame == Frame::ECEF) { return velocityError(2); }
162 ✗ break;
163 ✗ case PosVelAtt::GetStaticDescriptorCount() + 18: // Accelerometer bias b_X [m/s^2]
164 ✗ return b_biasAccel.value(0);
165 ✗ case PosVelAtt::GetStaticDescriptorCount() + 19: // Accelerometer bias b_Y [m/s^2]
166 ✗ return b_biasAccel.value(1);
167 ✗ case PosVelAtt::GetStaticDescriptorCount() + 20: // Accelerometer bias b_Z [m/s^2]
168 ✗ return b_biasAccel.value(2);
169 ✗ case PosVelAtt::GetStaticDescriptorCount() + 21: // Accelerometer bias b_X StdDev [m/s^2]
170 ✗ return b_biasAccel.stdDev(0);
171 ✗ case PosVelAtt::GetStaticDescriptorCount() + 22: // Accelerometer bias b_Y StdDev [m/s^2]
172 ✗ return b_biasAccel.stdDev(1);
173 ✗ case PosVelAtt::GetStaticDescriptorCount() + 23: // Accelerometer bias b_Z StdDev [m/s^2]
174 ✗ return b_biasAccel.stdDev(2);
175 ✗ case PosVelAtt::GetStaticDescriptorCount() + 24: // Gyroscope bias b_X [rad/s]
176 ✗ return b_biasGyro.value(0);
177 ✗ case PosVelAtt::GetStaticDescriptorCount() + 25: // Gyroscope bias b_Y [rad/s]
178 ✗ return b_biasGyro.value(1);
179 ✗ case PosVelAtt::GetStaticDescriptorCount() + 26: // Gyroscope bias b_Z [rad/s]
180 ✗ return b_biasGyro.value(2);
181 ✗ case PosVelAtt::GetStaticDescriptorCount() + 27: // Gyroscope bias b_X StdDev [rad/s]
182 ✗ return b_biasGyro.stdDev(0);
183 ✗ case PosVelAtt::GetStaticDescriptorCount() + 28: // Gyroscope bias b_Y StdDev [rad/s]
184 ✗ return b_biasGyro.stdDev(1);
185 ✗ case PosVelAtt::GetStaticDescriptorCount() + 29: // Gyroscope bias b_Z StdDev [rad/s]
186 ✗ return b_biasGyro.stdDev(2);
187 ✗ case PosVelAtt::GetStaticDescriptorCount() + 30: // Barometric height bias [m]
188 ✗ return heightBias.value;
189 ✗ case PosVelAtt::GetStaticDescriptorCount() + 31: // Barometric height bias StdDev[m]
190 ✗ return heightBias.stdDev;
191 ✗ case PosVelAtt::GetStaticDescriptorCount() + 32: // Barometric height scale [m/m]
192 ✗ return heightScale.value;
193 ✗ case PosVelAtt::GetStaticDescriptorCount() + 33: // Barometric height scale StdDev [m/m]
194 ✗ return heightScale.stdDev;
195 ✗ default:
196 ✗ return std::nullopt;
197 }
198 ✗ return std::nullopt;
199 }
200
201 /// @brief Available Frames
202 enum class Frame : uint8_t
203 {
204 ECEF, ///< Earth-Centered Earth-Fixed frame
205 NED, ///< Local North-East-Down frame
206 };
207 /// Frame in which the errors are set
208 Frame frame = Frame::NED;
209
210 /// δ𝛙_{i,e,n}b_{i,e,n} The attitude error in {i,e,n} frame coordinates in [rad]
211 Eigen::Vector3d attitudeError{ 0, 0, 0 };
212 /// δ𝐯_{i,e,n} The velocity error in {i,e,n} coordinates in [m/s]
213 Eigen::Vector3d velocityError{ 0, 0, 0 };
214 /// NED: δ𝐩 = [δ𝜙 δλ δ𝘩] The position error (latitude, longitude, altitude) in [rad, rad, m]
215 /// ECEF/i: δr The position error in [m]
216 Eigen::Vector3d positionError{ 0, 0, 0 };
217
218 /// 𝐛_a The accelerometer bias in body frame in [m/s^2]
219 // Eigen::Vector3d b_biasAccel{ 0, 0, 0 };
220 UncertainValue<Eigen::Vector3d> b_biasAccel = { .value = Eigen::Vector3d::Zero(), .stdDev = Eigen::Vector3d::Zero() };
221 /// 𝐛_g The gyroscope bias in body frame in [rad/s]
222 // Eigen::Vector3d b_biasGyro{ 0, 0, 0 };
223 UncertainValue<Eigen::Vector3d> b_biasGyro = { .value = Eigen::Vector3d::Zero(), .stdDev = Eigen::Vector3d::Zero() };
224
225 /// Barometric height bias in [m]
226 UncertainValue<double> heightBias = { .value = std::nan(""), .stdDev = std::nan("") };
227
228 /// Barometric height scale in [m/m]
229 UncertainValue<double> heightScale = { .value = std::nan(""), .stdDev = std::nan("") };
230 };
231
232 } // namespace NAV
233