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 "util/Eigen.hpp"
17#include "PosVelAtt.hpp"
18#include <cstdint>
19
20#include "Navigation/Transformations/Units.hpp"
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("Gyroscope bias b_X [rad/s]");
75 desc.emplace_back("Gyroscope bias b_Y [rad/s]");
76 desc.emplace_back("Gyroscope bias b_Z [rad/s]");
77 return desc;
78 }
79
81 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVelAtt::GetStaticDescriptorCount() + 24; }
82
84 [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
85
89 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
90 {
91 INS_ASSERT(idx < GetStaticDescriptorCount());
92 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
168 enum class Frame : uint8_t
169 {
170 ECEF,
171 NED,
172 };
174 Frame frame = Frame::NED;
175
177 Eigen::Vector3d attitudeError{ 0, 0, 0 };
179 Eigen::Vector3d velocityError{ 0, 0, 0 };
182 Eigen::Vector3d positionError{ 0, 0, 0 };
183
185 Eigen::Vector3d b_biasAccel{ 0, 0, 0 };
187 Eigen::Vector3d b_biasGyro{ 0, 0, 0 };
188};
189
190} // 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:26
NAV::InsGnssLCKFSolution::GetStaticDataDescriptors
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition InsGnssLCKFSolution.hpp:49
NAV::InsGnssLCKFSolution::b_biasGyro
Eigen::Vector3d b_biasGyro
𝐛_g The gyroscope bias in body frame in [rad/s]
Definition InsGnssLCKFSolution.hpp:187
NAV::InsGnssLCKFSolution::b_biasAccel
Eigen::Vector3d b_biasAccel
𝐛_a The accelerometer bias in body frame in [m/s^2]
Definition InsGnssLCKFSolution.hpp:185
NAV::InsGnssLCKFSolution::Frame
Frame
Available Frames.
Definition InsGnssLCKFSolution.hpp:169
NAV::InsGnssLCKFSolution::Frame::NED
@ NED
Local North-East-Down frame.
NAV::InsGnssLCKFSolution::Frame::ECEF
@ ECEF
Earth-Centered Earth-Fixed frame.
NAV::InsGnssLCKFSolution::velocityError
Eigen::Vector3d velocityError
δ𝐯_{i,e,n} The velocity error in {i,e,n} coordinates in [m/s]
Definition InsGnssLCKFSolution.hpp:179
NAV::InsGnssLCKFSolution::frame
Frame frame
Frame in which the errors are set.
Definition InsGnssLCKFSolution.hpp:174
NAV::InsGnssLCKFSolution::type
static std::string type()
Returns the type of the data class.
Definition InsGnssLCKFSolution.hpp:30
NAV::InsGnssLCKFSolution::positionError
Eigen::Vector3d positionError
Definition InsGnssLCKFSolution.hpp:182
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:177
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:84
NAV::InsGnssLCKFSolution::GetStaticDescriptorCount
static constexpr size_t GetStaticDescriptorCount()
Get the number of descriptors.
Definition InsGnssLCKFSolution.hpp:81
NAV::InsGnssLCKFSolution::getValueAt
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition InsGnssLCKFSolution.hpp:89
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:63
NAV::PosVelAtt::getValueAt
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition PosVelAtt.hpp:74
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