0.2.0
Loading...
Searching...
No Matches
InsGnssLCKFSolution.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
13
14#pragma once
15
16#include "util/Eigen.hpp"
17#include "PosVelAtt.hpp"
18
19#include "Navigation/Transformations/Units.hpp"
20
21namespace NAV
22{
24class InsGnssLCKFSolution : public PosVelAtt
25{
26 public:
29 [[nodiscard]] static std::string type()
30 {
31 return "InsGnssLCKFSolution";
32 }
33
36 [[nodiscard]] static std::vector<std::string> parentTypes()
37 {
38 auto parent = PosVelAtt::parentTypes();
39 parent.push_back(PosVelAtt::type());
40 return parent;
41 }
42
44 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
45 {
46 auto desc = PosVelAtt::GetStaticDataDescriptors();
47 desc.reserve(GetStaticDescriptorCount());
48 desc.emplace_back("KF State Roll error [deg]");
49 desc.emplace_back("KF State Pitch error [deg]");
50 desc.emplace_back("KF State Yaw error [deg]");
51 desc.emplace_back("KF State Position North error [deg]");
52 desc.emplace_back("KF State Position East error [deg]");
53 desc.emplace_back("KF State Position Down error [m]");
54 desc.emplace_back("KF State Velocity North error [m/s]");
55 desc.emplace_back("KF State Velocity East error [m/s]");
56 desc.emplace_back("KF State Velocity Down error [m/s]");
57 desc.emplace_back("KF State Alpha_eb [deg]");
58 desc.emplace_back("KF State Beta_eb [deg]");
59 desc.emplace_back("KF State Gamma_eb [deg]");
60 desc.emplace_back("KF State Position ECEF X error [m]");
61 desc.emplace_back("KF State Position ECEF Y error [m]");
62 desc.emplace_back("KF State Position ECEF Z error [m]");
63 desc.emplace_back("KF State Velocity ECEF X error [m/s]");
64 desc.emplace_back("KF State Velocity ECEF Y error [m/s]");
65 desc.emplace_back("KF State Velocity ECEF Z error [m/s]");
66 desc.emplace_back("Accelerometer bias b_X [m/s^2]");
67 desc.emplace_back("Accelerometer bias b_Y [m/s^2]");
68 desc.emplace_back("Accelerometer bias b_Z [m/s^2]");
69 desc.emplace_back("Gyroscope bias b_X [rad/s]");
70 desc.emplace_back("Gyroscope bias b_Y [rad/s]");
71 desc.emplace_back("Gyroscope bias b_Z [rad/s]");
72 return desc;
73 }
74
76 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 70; }
77
79 [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
80
84 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
85 {
86 INS_ASSERT(idx < GetStaticDescriptorCount());
87
88 switch (idx)
89 {
90 case 0: // Latitude [deg]
91 case 1: // Longitude [deg]
92 case 2: // Altitude [m]
93 case 3: // North/South [m]
94 case 4: // East/West [m]
95 case 5: // X-ECEF [m]
96 case 6: // Y-ECEF [m]
97 case 7: // Z-ECEF [m]
98 case 8: // X-ECEF StDev [m]
99 case 9: // Y-ECEF StDev [m]
100 case 10: // Z-ECEF StDev [m]
101 case 11: // XY-ECEF StDev [m]
102 case 12: // XZ-ECEF StDev [m]
103 case 13: // YZ-ECEF StDev [m]
104 case 14: // North StDev [m]
105 case 15: // East StDev [m]
106 case 16: // Down StDev [m]
107 case 17: // NE StDev [m]
108 case 18: // ND StDev [m]
109 case 19: // ED StDev [m]
110 case 20: // Velocity norm [m/s]
111 case 21: // X velocity ECEF [m/s]
112 case 22: // Y velocity ECEF [m/s]
113 case 23: // Z velocity ECEF [m/s]
114 case 24: // North velocity [m/s]
115 case 25: // East velocity [m/s]
116 case 26: // Down velocity [m/s]
117 case 27: // X velocity ECEF StDev [m/s]
118 case 28: // Y velocity ECEF StDev [m/s]
119 case 29: // Z velocity ECEF StDev [m/s]
120 case 30: // XY velocity StDev [m]
121 case 31: // XZ velocity StDev [m]
122 case 32: // YZ velocity StDev [m]
123 case 33: // North velocity StDev [m/s]
124 case 34: // East velocity StDev [m/s]
125 case 35: // Down velocity StDev [m/s]
126 case 36: // NE velocity StDev [m]
127 case 37: // ND velocity StDev [m]
128 case 38: // ED velocity StDev [m]
129 case 39: // Roll [deg]
130 case 40: // Pitch [deg]
131 case 41: // Yaw [deg]
132 case 42: // Quaternion::w
133 case 43: // Quaternion::x
134 case 44: // Quaternion::y
135 case 45: // Quaternion::z
136 return PosVelAtt::getValueAt(idx);
137 case 46: // KF State Roll error [deg]
138 if (frame == Frame::NED) { return rad2deg(attitudeError(0)); }
139 break;
140 case 47: // KF State Pitch error [deg]
141 if (frame == Frame::NED) { return rad2deg(attitudeError(1)); }
142 break;
143 case 48: // KF State Yaw error [deg]
144 if (frame == Frame::NED) { return rad2deg(attitudeError(2)); }
145 break;
146 case 49: // KF State Position North error [deg]
147 if (frame == Frame::NED) { return rad2deg(positionError(0)); }
148 break;
149 case 50: // KF State Position East error [deg]
150 if (frame == Frame::NED) { return rad2deg(positionError(1)); }
151 break;
152 case 51: // KF State Position Down error [m]
153 if (frame == Frame::NED) { return positionError(2); }
154 break;
155 case 52: // KF State Velocity North error [m/s]
156 if (frame == Frame::NED) { return velocityError(0); }
157 break;
158 case 53: // KF State Velocity East error [m/s]
159 if (frame == Frame::NED) { return velocityError(1); }
160 break;
161 case 54: // KF State Velocity Down error [m/s]
162 if (frame == Frame::NED) { return velocityError(2); }
163 break;
164 case 55: // KF State Alpha_eb [deg]
165 if (frame == Frame::ECEF) { return rad2deg(attitudeError(0)); }
166 break;
167 case 56: // KF State Beta_eb [deg]
168 if (frame == Frame::ECEF) { return rad2deg(attitudeError(1)); }
169 break;
170 case 57: // KF State Gamma_eb [deg]
171 if (frame == Frame::ECEF) { return rad2deg(attitudeError(2)); }
172 break;
173 case 58: // KF State Position ECEF X error [m]
174 if (frame == Frame::ECEF) { return positionError(0); }
175 break;
176 case 59: // KF State Position ECEF Y error [m]
177 if (frame == Frame::ECEF) { return positionError(1); }
178 break;
179 case 60: // KF State Position ECEF Z error [m]
180 if (frame == Frame::ECEF) { return positionError(2); }
181 break;
182 case 61: // KF State Velocity ECEF X error [m/s]
183 if (frame == Frame::ECEF) { return velocityError(0); }
184 break;
185 case 62: // KF State Velocity ECEF Y error [m/s]
186 if (frame == Frame::ECEF) { return velocityError(1); }
187 break;
188 case 63: // KF State Velocity ECEF Z error [m/s]
189 if (frame == Frame::ECEF) { return velocityError(2); }
190 break;
191 case 64: // Accelerometer bias b_X [m/s^2]
192 return b_biasAccel(0);
193 case 65: // Accelerometer bias b_Y [m/s^2]
194 return b_biasAccel(1);
195 case 66: // Accelerometer bias b_Z [m/s^2]
196 return b_biasAccel(2);
197 case 67: // Gyroscope bias b_X [rad/s]
198 return b_biasGyro(0);
199 case 68: // Gyroscope bias b_Y [rad/s]
200 return b_biasGyro(1);
201 case 69: // Gyroscope bias b_Z [rad/s]
202 return b_biasGyro(2);
203 default:
204 return std::nullopt;
205 }
206 return std::nullopt;
207 }
208
210 enum class Frame : int
211 {
212 ECEF,
213 NED,
214 };
216 Frame frame = Frame::NED;
217
219 Eigen::Vector3d attitudeError{ 0, 0, 0 };
221 Eigen::Vector3d velocityError{ 0, 0, 0 };
224 Eigen::Vector3d positionError{ 0, 0, 0 };
225
227 Eigen::Vector3d b_biasAccel{ 0, 0, 0 };
229 Eigen::Vector3d b_biasGyro{ 0, 0, 0 };
230};
231
232} // namespace NAV
INS_ASSERT
#define INS_ASSERT(_EXPR)
Assert function wrapper.
Definition Assert.h:19
Eigen.hpp
Vector space operations.
PosVelAtt.hpp
Position, Velocity and Attitude Storage Class.
NAV::InsGnssLCKFSolution
Loosely-coupled Kalman Filter INS/GNSS errors.
Definition InsGnssLCKFSolution.hpp:25
NAV::InsGnssLCKFSolution::GetStaticDataDescriptors
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition InsGnssLCKFSolution.hpp:44
NAV::InsGnssLCKFSolution::b_biasGyro
Eigen::Vector3d b_biasGyro
𝐛_g The gyroscope bias in body frame in [rad/s]
Definition InsGnssLCKFSolution.hpp:229
NAV::InsGnssLCKFSolution::b_biasAccel
Eigen::Vector3d b_biasAccel
𝐛_a The accelerometer bias in body frame in [m/s^2]
Definition InsGnssLCKFSolution.hpp:227
NAV::InsGnssLCKFSolution::velocityError
Eigen::Vector3d velocityError
δ𝐯_{i,e,n} The velocity error in {i,e,n} coordinates in [m/s]
Definition InsGnssLCKFSolution.hpp:221
NAV::InsGnssLCKFSolution::frame
Frame frame
Frame in which the errors are set.
Definition InsGnssLCKFSolution.hpp:216
NAV::InsGnssLCKFSolution::type
static std::string type()
Returns the type of the data class.
Definition InsGnssLCKFSolution.hpp:29
NAV::InsGnssLCKFSolution::Frame
Frame
Available Frames.
Definition InsGnssLCKFSolution.hpp:211
NAV::InsGnssLCKFSolution::Frame::NED
@ NED
Local North-East-Down frame.
NAV::InsGnssLCKFSolution::Frame::ECEF
@ ECEF
Earth-Centered Earth-Fixed frame.
NAV::InsGnssLCKFSolution::positionError
Eigen::Vector3d positionError
Definition InsGnssLCKFSolution.hpp:224
NAV::InsGnssLCKFSolution::attitudeError
Eigen::Vector3d attitudeError
δ𝛙_{i,e,n}b_{i,e,n} The attitude error in {i,e,n} frame coordinates in [rad]
Definition InsGnssLCKFSolution.hpp:219
NAV::InsGnssLCKFSolution::staticDataDescriptors
std::vector< std::string > staticDataDescriptors() const override
Returns a vector of data descriptors.
Definition InsGnssLCKFSolution.hpp:79
NAV::InsGnssLCKFSolution::GetStaticDescriptorCount
static constexpr size_t GetStaticDescriptorCount()
Get the number of descriptors.
Definition InsGnssLCKFSolution.hpp:76
NAV::InsGnssLCKFSolution::getValueAt
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition InsGnssLCKFSolution.hpp:84
NAV::InsGnssLCKFSolution::parentTypes
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
Definition InsGnssLCKFSolution.hpp:36
NAV::PosVelAtt
Position, Velocity and Attitude Storage Class.
Definition PosVelAtt.hpp:25
NAV::PosVelAtt::GetStaticDataDescriptors
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition PosVelAtt.hpp:44
NAV::PosVelAtt::getValueAt
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition PosVelAtt.hpp:70
NAV::PosVelAtt::type
static std::string type()
Returns the type of the data class.
Definition PosVelAtt.hpp:29
NAV::PosVelAtt::parentTypes
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
Definition PosVelAtt.hpp:36