0.4.1
Loading...
Searching...
No Matches
ImuObsWDelta.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
9/// @file ImuObsWDelta.hpp
10/// @brief Data storage class for one VectorNavImu observation
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @date 2020-03-12
13
14#pragma once
15
16#include "ImuObs.hpp"
18
19namespace NAV
20{
21/// VectorNav Observation storage Class
22class ImuObsWDelta : public ImuObs
23{
24 public:
25 /// @brief Constructor
26 /// @param[in] imuPos Reference to the position and rotation info of the Imu
27 explicit ImuObsWDelta(const ImuPos& imuPos)
28 : ImuObs(imuPos) {}
29
30 /// @brief Returns the type of the data class
31 /// @return The data type
32 [[nodiscard]] static std::string type()
33 {
34 return "ImuObsWDelta";
35 }
36
37 /// @brief Returns the type of the data class
38 /// @return The data type
39 [[nodiscard]] std::string getType() const override { return type(); }
40
41 /// @brief Returns the parent types of the data class
42 /// @return The parent data types
43 [[nodiscard]] static std::vector<std::string> parentTypes()
44 {
45 return { ImuObs::type() };
46 }
47
48 /// @brief Returns a vector of data descriptors
49 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
50 {
52 desc.emplace_back("dTime [s]");
53 desc.emplace_back("dTheta X [deg]");
54 desc.emplace_back("dTheta Y [deg]");
55 desc.emplace_back("dTheta Z [deg]");
56 desc.emplace_back("dVelocity X [m/s]");
57 desc.emplace_back("dVelocity Y [m/s]");
58 desc.emplace_back("dVelocity Z [m/s]");
59 return desc;
60 }
61
62 /// @brief Get the amount of descriptors
63 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return ImuObs::GetStaticDescriptorCount() + 7; }
64
65 /// @brief Returns a vector of data descriptors
66 [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
67
68 /// @brief Get the amount of descriptors
69 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
70
71 /// @brief Get the value at the index
72 /// @param idx Index corresponding to data descriptor order
73 /// @return Value if in the observation
74 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
75 {
77 if (idx < ImuObs::GetStaticDescriptorCount()) { return ImuObs::getValueAt(idx); }
78 switch (idx)
79 {
80 case ImuObs::GetStaticDescriptorCount() + 0: // dTime [s]
81 return dtime;
82 case ImuObs::GetStaticDescriptorCount() + 1: // dTheta X [deg]
83 return rad2deg(dtheta.x());
84 case ImuObs::GetStaticDescriptorCount() + 2: // dTheta Y [deg]
85 return rad2deg(dtheta.y());
86 case ImuObs::GetStaticDescriptorCount() + 3: // dTheta Z [deg]
87 return rad2deg(dtheta.z());
88 case ImuObs::GetStaticDescriptorCount() + 4: // dVelocity X [m/s]
89 return dvel.x();
90 case ImuObs::GetStaticDescriptorCount() + 5: // dVelocity Y [m/s]
91 return dvel.y();
92 case ImuObs::GetStaticDescriptorCount() + 6: // dVelocity Z [m/s]
93 return dvel.z();
94 default:
95 return std::nullopt;
96 }
97 return std::nullopt;
98 }
99
100 /// @brief Set the value at the index
101 /// @param idx Index corresponding to data descriptor order
102 /// @param value Value to set
103 /// @return True if the value was updated
104 [[nodiscard]] bool setValueAt(size_t idx, double value) override
105 {
107 if (idx < ImuObs::GetStaticDescriptorCount()) { return ImuObs::setValueAt(idx, value); }
108 switch (idx)
109 {
110 case ImuObs::GetStaticDescriptorCount() + 0: // dTime [s]
111 dtime = value;
112 break;
113 case ImuObs::GetStaticDescriptorCount() + 1: // dTheta X [deg]
114 dtheta.x() = deg2rad(value);
115 break;
116 case ImuObs::GetStaticDescriptorCount() + 2: // dTheta Y [deg]
117 dtheta.y() = deg2rad(value);
118 break;
119 case ImuObs::GetStaticDescriptorCount() + 3: // dTheta Z [deg]
120 dtheta.z() = deg2rad(value);
121 break;
122 case ImuObs::GetStaticDescriptorCount() + 4: // dVelocity X [m/s]
123 dvel.x() = value;
124 break;
125 case ImuObs::GetStaticDescriptorCount() + 5: // dVelocity Y [m/s]
126 dvel.y() = value;
127 break;
128 case ImuObs::GetStaticDescriptorCount() + 6: // dVelocity Z [m/s]
129 dvel.z() = value;
130 break;
131 default:
132 return false;
133 }
134
135 return true;
136 }
137
138 /// The time interval that the delta angle and velocities are integrated over in [seconds].
139 double dtime = 0.0;
140 /// The delta rotation angles in [rad] incurred due to rotation, by the local platform reference frame,
141 /// since the last time the values were outputted by the device.
142 Eigen::Vector3d dtheta;
143 /// The delta velocity in [m/s] incurred due to motion, by the local platform reference frame,
144 /// since the last time the values were outputted by the device.
145 Eigen::Vector3d dvel;
146};
147
148} // namespace NAV
#define INS_ASSERT(_EXPR)
Assert function wrapper.
Definition Assert.h:19
Parent Class for all IMU Observations.
std::string getType() const override
Returns the type of the data class.
bool setValueAt(size_t idx, double value) override
Set the value at the index.
static constexpr size_t GetStaticDescriptorCount()
Get the amount of descriptors.
double dtime
The time interval that the delta angle and velocities are integrated over in [seconds].
size_t staticDescriptorCount() const override
Get the amount of descriptors.
static std::string type()
Returns the type of the data class.
Eigen::Vector3d dtheta
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
ImuObsWDelta(const ImuPos &imuPos)
Constructor.
Eigen::Vector3d dvel
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
std::vector< std::string > staticDataDescriptors() const override
Returns a vector of data descriptors.
bool setValueAt(size_t idx, double value) override
Set the value at the index.
Definition ImuObs.hpp:116
static std::string type()
Returns the type of the data class.
Definition ImuObs.hpp:33
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition ImuObs.hpp:50
static constexpr size_t GetStaticDescriptorCount()
Get the amount of descriptors.
Definition ImuObs.hpp:67
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition ImuObs.hpp:78
ImuObs(const ImuPos &imuPos)
Constructor.
Definition ImuObs.hpp:28
const ImuPos & imuPos
Position and rotation information for conversion from platform to body frame.
Definition ImuObs.hpp:172
IMU Position.
Definition ImuPos.hpp:26
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39