0.3.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 "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
22namespace NAV
23{
25class InsGnssLCKFSolution : public PosVelAtt
26{
27 public:
30 [[nodiscard]] static std::string type()
31 {
32 return "InsGnssLCKFSolution";
33 }
34
37 [[nodiscard]] std::string getType() const override { return type(); }
38
41 [[nodiscard]] static std::vector<std::string> parentTypes()
42 {
43 auto parent = PosVelAtt::parentTypes();
44 parent.push_back(PosVelAtt::type());
45 return parent;
46 }
47
49 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
50 {
51 auto desc = PosVelAtt::GetStaticDataDescriptors();
52 desc.reserve(GetStaticDescriptorCount());
53 desc.emplace_back("KF State Roll error [deg]");
54 desc.emplace_back("KF State Pitch error [deg]");
55 desc.emplace_back("KF State Yaw error [deg]");
56 desc.emplace_back("KF State Position North error [deg]");
57 desc.emplace_back("KF State Position East error [deg]");
58 desc.emplace_back("KF State Position Down error [m]");
59 desc.emplace_back("KF State Velocity North error [m/s]");
60 desc.emplace_back("KF State Velocity East error [m/s]");
61 desc.emplace_back("KF State Velocity Down error [m/s]");
62 desc.emplace_back("KF State Alpha_eb [deg]");
63 desc.emplace_back("KF State Beta_eb [deg]");
64 desc.emplace_back("KF State Gamma_eb [deg]");
65 desc.emplace_back("KF State Position ECEF X error [m]");
66 desc.emplace_back("KF State Position ECEF Y error [m]");
67 desc.emplace_back("KF State Position ECEF Z error [m]");
68 desc.emplace_back("KF State Velocity ECEF X error [m/s]");
69 desc.emplace_back("KF State Velocity ECEF Y error [m/s]");
70 desc.emplace_back("KF State Velocity ECEF Z error [m/s]");
71 desc.emplace_back("Accelerometer bias b_X [m/s^2]");
72 desc.emplace_back("Accelerometer bias b_Y [m/s^2]");
73 desc.emplace_back("Accelerometer bias b_Z [m/s^2]");
74 desc.emplace_back("Accelerometer bias b_X StdDev [m/s^2]");
75 desc.emplace_back("Accelerometer bias b_Y StdDev [m/s^2]");
76 desc.emplace_back("Accelerometer bias b_Z StdDev [m/s^2]");
77 desc.emplace_back("Gyroscope bias b_X [rad/s]");
78 desc.emplace_back("Gyroscope bias b_Y [rad/s]");
79 desc.emplace_back("Gyroscope bias b_Z [rad/s]");
80 desc.emplace_back("Gyroscope bias b_X StdDev [rad/s]");
81 desc.emplace_back("Gyroscope bias b_Y StdDev [rad/s]");
82 desc.emplace_back("Gyroscope bias b_Z StdDev [rad/s]");
83 desc.emplace_back("Barometric height bias [m]");
84 desc.emplace_back("Barometric height bias StdDev [m]");
85 desc.emplace_back("Barometric height scale [m/m]");
86 desc.emplace_back("Barometric height scale StdDev [m/m]");
87
88 return desc;
89 }
90
92 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVelAtt::GetStaticDescriptorCount() + 34; }
93
95 [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
96
98 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
99
103 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
104 {
105 INS_ASSERT(idx < GetStaticDescriptorCount());
106 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
202 enum class Frame : uint8_t
203 {
204 ECEF,
205 NED,
206 };
207
208 Frame frame = Frame::NED;
209
211 Eigen::Vector3d attitudeError{ 0, 0, 0 };
213 Eigen::Vector3d velocityError{ 0, 0, 0 };
216 Eigen::Vector3d positionError{ 0, 0, 0 };
217
219 // Eigen::Vector3d b_biasAccel{ 0, 0, 0 };
220 UncertainValue<Eigen::Vector3d> b_biasAccel = { .value = Eigen::Vector3d::Zero(), .stdDev = Eigen::Vector3d::Zero() };
222 // Eigen::Vector3d b_biasGyro{ 0, 0, 0 };
223 UncertainValue<Eigen::Vector3d> b_biasGyro = { .value = Eigen::Vector3d::Zero(), .stdDev = Eigen::Vector3d::Zero() };
224
226 UncertainValue<double> heightBias = { .value = std::nan(""), .stdDev = std::nan("") };
227
229 UncertainValue<double> heightScale = { .value = std::nan(""), .stdDev = std::nan("") };
230};
231
232} // namespace NAV
INS_ASSERT
#define INS_ASSERT(_EXPR)
Assert function wrapper.
Definition Assert.h:19
PosVelAtt.hpp
Position, Velocity and Attitude Storage Class.
UncertainValue.hpp
Values with an uncertainty (Standard Deviation)
NAV::InsGnssLCKFSolution
Loosely-coupled Kalman Filter INS/GNSS errors.
Definition InsGnssLCKFSolution.hpp:26
NAV::InsGnssLCKFSolution::heightBias
UncertainValue< double > heightBias
Barometric height bias in [m].
Definition InsGnssLCKFSolution.hpp:226
NAV::InsGnssLCKFSolution::GetStaticDataDescriptors
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition InsGnssLCKFSolution.hpp:49
NAV::InsGnssLCKFSolution::Frame
Frame
Available Frames.
Definition InsGnssLCKFSolution.hpp:203
NAV::InsGnssLCKFSolution::Frame::NED
@ NED
Local North-East-Down frame.
Definition InsGnssLCKFSolution.hpp:205
NAV::InsGnssLCKFSolution::Frame::ECEF
@ ECEF
Earth-Centered Earth-Fixed frame.
Definition InsGnssLCKFSolution.hpp:204
NAV::InsGnssLCKFSolution::velocityError
Eigen::Vector3d velocityError
δ𝐯_{i,e,n} The velocity error in {i,e,n} coordinates in [m/s]
Definition InsGnssLCKFSolution.hpp:213
NAV::InsGnssLCKFSolution::b_biasAccel
UncertainValue< Eigen::Vector3d > b_biasAccel
𝐛_a The accelerometer bias in body frame in [m/s^2]
Definition InsGnssLCKFSolution.hpp:220
NAV::InsGnssLCKFSolution::frame
Frame frame
Frame in which the errors are set.
Definition InsGnssLCKFSolution.hpp:208
NAV::InsGnssLCKFSolution::type
static std::string type()
Returns the type of the data class.
Definition InsGnssLCKFSolution.hpp:30
NAV::InsGnssLCKFSolution::heightScale
UncertainValue< double > heightScale
Barometric height scale in [m/m].
Definition InsGnssLCKFSolution.hpp:229
NAV::InsGnssLCKFSolution::positionError
Eigen::Vector3d positionError
Definition InsGnssLCKFSolution.hpp:216
NAV::InsGnssLCKFSolution::staticDescriptorCount
size_t staticDescriptorCount() const override
Get the amount of descriptors.
Definition InsGnssLCKFSolution.hpp:98
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:211
NAV::InsGnssLCKFSolution::b_biasGyro
UncertainValue< Eigen::Vector3d > b_biasGyro
𝐛_g The gyroscope bias in body frame in [rad/s]
Definition InsGnssLCKFSolution.hpp:223
NAV::InsGnssLCKFSolution::getType
std::string getType() const override
Returns the type of the data class.
Definition InsGnssLCKFSolution.hpp:37
NAV::InsGnssLCKFSolution::staticDataDescriptors
std::vector< std::string > staticDataDescriptors() const override
Returns a vector of data descriptors.
Definition InsGnssLCKFSolution.hpp:95
NAV::InsGnssLCKFSolution::GetStaticDescriptorCount
static constexpr size_t GetStaticDescriptorCount()
Get the number of descriptors.
Definition InsGnssLCKFSolution.hpp:92
NAV::InsGnssLCKFSolution::getValueAt
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition InsGnssLCKFSolution.hpp:103
NAV::InsGnssLCKFSolution::parentTypes
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
Definition InsGnssLCKFSolution.hpp:41
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:48
NAV::PosVelAtt::GetStaticDescriptorCount
static constexpr size_t GetStaticDescriptorCount()
Get the amount of descriptors.
Definition PosVelAtt.hpp:70
NAV::PosVelAtt::getValueAt
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition PosVelAtt.hpp:81
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:40
NAV::UncertainValue
Value with standard deviation.
Definition UncertainValue.hpp:23