INSTINCT Code Coverage Report

Directory: src/
File: NodeData/GNSS/RtklibPosObs.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 49 70 70.0%
Functions: 6 10 60.0%
Branches: 104 215 48.4%
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 RtklibPosObs.hpp
10 /// @brief RTKLIB Pos Observation Class
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2020-06-02
13
14 #pragma once
15
16 #include "NodeData/State/PosVel.hpp"
17 #include "util/Eigen.hpp"
18 #include "Navigation/Transformations/Units.hpp"
19
20 namespace NAV
21 {
22 /// RTKLIB Observation Class
23 class RtklibPosObs : public PosVel
24 {
25 public:
26 #ifdef TESTING
27 /// Default constructor
28 364 RtklibPosObs() = default;
29
30 /// @brief Constructor
31 /// @param[in] insTime Epoch time
32 /// @param[in] e_position Position in ECEF coordinates
33 /// @param[in] lla_position Position in LatLonAlt coordinates [rad, rad, m]
34 /// @param[in] e_velocity Velocity in earth coordinates [m/s]
35 /// @param[in] n_velocity Velocity in navigation coordinates [m/s]
36 /// @param[in] Q 1:fix, 2:float, 3:sbas, 4:dgps, 5:single, 6:ppp
37 /// @param[in] ns Number of satellites
38 /// @param[in] sdXYZ Standard Deviation XYZ [m]
39 /// @param[in] sdNED Standard Deviation North East Down [m]
40 /// @param[in] sdxy Standard Deviation xy [m]
41 /// @param[in] sdyz Standard Deviation yz [m]
42 /// @param[in] sdzx Standard Deviation zx [m]
43 /// @param[in] sdne Standard Deviation ne [m]
44 /// @param[in] sded Standard Deviation ed [m]
45 /// @param[in] sddn Standard Deviation dn [m]
46 /// @param[in] age Age [s]
47 /// @param[in] ratio Ratio
48 /// @param[in] sdvNED Standard Deviation velocity NED [m/s]
49 /// @param[in] sdvne Standard Deviation velocity north-east [m/s]
50 /// @param[in] sdved Standard Deviation velocity east-down [m/s]
51 /// @param[in] sdvdn Standard Deviation velocity down-north [m/s]
52 /// @param[in] sdvXYZ Standard Deviation velocity XYZ [m/s]
53 /// @param[in] sdvxy Standard Deviation velocity xy [m/s]
54 /// @param[in] sdvyz Standard Deviation velocity yz [m/s]
55 /// @param[in] sdvzx Standard Deviation velocity zx [m/s]
56 2421 RtklibPosObs(const InsTime& insTime,
57 std::optional<Eigen::Vector3d> e_position,
58 std::optional<Eigen::Vector3d> lla_position,
59 std::optional<Eigen::Vector3d> e_velocity,
60 std::optional<Eigen::Vector3d> n_velocity,
61 uint8_t Q,
62 uint8_t ns,
63 std::optional<Eigen::Vector3d> sdXYZ,
64 std::optional<Eigen::Vector3d> sdNED,
65 std::optional<double> sdxy,
66 std::optional<double> sdyz,
67 std::optional<double> sdzx,
68 std::optional<double> sdne,
69 std::optional<double> sded,
70 std::optional<double> sddn,
71 double age,
72 double ratio,
73 std::optional<Eigen::Vector3d> sdvNED,
74 std::optional<double> sdvne,
75 std::optional<double> sdved,
76 std::optional<double> sdvdn,
77 std::optional<Eigen::Vector3d> sdvXYZ,
78 std::optional<double> sdvxy,
79 std::optional<double> sdvyz,
80 std::optional<double> sdvzx)
81 2421 : Q(Q), ns(ns), age(age), ratio(ratio)
82 {
83 2421 this->insTime = insTime;
84
85
6/10
✓ Branch 1 taken 1883 times.
✓ Branch 2 taken 538 times.
✓ Branch 5 taken 1883 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1883 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1883 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1883 times.
✗ Branch 16 not taken.
 2421 if (lla_position) { lla_position->head<2>() = deg2rad(lla_position->head<2>()); }
86
87
8/12
✓ Branch 1 taken 538 times.
✓ Branch 2 taken 1883 times.
✓ Branch 4 taken 538 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 538 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 538 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 538 times.
✓ Branch 16 taken 1883 times.
 2421 if (e_position && sdXYZ && sdxy && sdzx && sdyz)
88 {
89
1/2
✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
 538 Eigen::Matrix3d cov;
90
4/8
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 538 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 538 times.
✗ Branch 15 not taken.
 538 cov << std::pow(sdXYZ->x(), 2), *sdxy, -(*sdzx),
91
4/8
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 538 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 538 times.
✗ Branch 15 not taken.
 538 -(*sdxy), std::pow(sdXYZ->y(), 2), *sdyz,
92
4/8
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 538 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 538 times.
✗ Branch 15 not taken.
 538 *sdzx, -(*sdyz), std::pow(sdXYZ->z(), 2);
93
1/2
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
 538 this->setPositionAndStdDev_e(*e_position, cov);
94
1/2
✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
 538 this->setPosCovarianceMatrix_e(cov);
95 }
96
6/12
✓ Branch 1 taken 1883 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1883 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1883 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1883 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1883 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1883 times.
✗ Branch 16 not taken.
 1883 else if (lla_position && sdNED && sdne && sddn && sded)
97 {
98
1/2
✓ Branch 1 taken 1883 times.
✗ Branch 2 not taken.
 1883 Eigen::Matrix3d cov;
99
4/8
✓ Branch 2 taken 1883 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1883 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1883 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1883 times.
✗ Branch 15 not taken.
 1883 cov << std::pow(sdNED->x(), 2), *sdne, -(*sddn),
100
4/8
✓ Branch 2 taken 1883 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1883 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1883 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1883 times.
✗ Branch 15 not taken.
 1883 -(*sdne), std::pow(sdNED->y(), 2), *sded,
101
4/8
✓ Branch 2 taken 1883 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1883 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1883 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1883 times.
✗ Branch 15 not taken.
 1883 *sddn, -(*sded), std::pow(sdNED->z(), 2);
102
1/2
✓ Branch 2 taken 1883 times.
✗ Branch 3 not taken.
 1883 this->setPositionAndStdDev_lla(*lla_position, cov);
103
1/2
✓ Branch 1 taken 1883 times.
✗ Branch 2 not taken.
 1883 this->setPosCovarianceMatrix_n(cov);
104 }
105 ✗ else if (e_position) { this->setPosition_e(*e_position); }
106 ✗ else if (lla_position) { this->setPosition_lla(*lla_position); }
107
108
8/12
✓ Branch 1 taken 538 times.
✓ Branch 2 taken 1883 times.
✓ Branch 4 taken 538 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 538 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 538 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 538 times.
✓ Branch 16 taken 1883 times.
 2421 if (e_velocity && sdvXYZ && sdvxy && sdvzx && sdvyz)
109 {
110
1/2
✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
 538 Eigen::Matrix3d cov;
111
4/8
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 538 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 538 times.
✗ Branch 15 not taken.
 538 cov << std::pow(sdvXYZ->x(), 2), *sdvxy, -(*sdvzx),
112
4/8
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 538 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 538 times.
✗ Branch 15 not taken.
 538 -(*sdvxy), std::pow(sdvXYZ->y(), 2), *sdvyz,
113
4/8
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 538 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 538 times.
✗ Branch 15 not taken.
 538 *sdvzx, -(*sdvyz), std::pow(sdvXYZ->z(), 2);
114
1/2
✓ Branch 2 taken 538 times.
✗ Branch 3 not taken.
 538 this->setVelocityAndStdDev_e(*e_velocity, cov);
115 }
116
8/12
✓ Branch 1 taken 1345 times.
✓ Branch 2 taken 538 times.
✓ Branch 4 taken 1345 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1345 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1345 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1345 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1345 times.
✓ Branch 16 taken 538 times.
 1883 else if (n_velocity && sdvNED && sdvne && sdvdn && sdved)
117 {
118
1/2
✓ Branch 1 taken 1345 times.
✗ Branch 2 not taken.
 1345 Eigen::Matrix3d cov;
119
4/8
✓ Branch 2 taken 1345 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1345 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1345 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1345 times.
✗ Branch 15 not taken.
 1345 cov << std::pow(sdvNED->x(), 2), *sdvne, -(*sdvdn),
120
4/8
✓ Branch 2 taken 1345 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1345 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1345 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1345 times.
✗ Branch 15 not taken.
 1345 -(*sdvne), std::pow(sdvNED->y(), 2), *sdved,
121
4/8
✓ Branch 2 taken 1345 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1345 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1345 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1345 times.
✗ Branch 15 not taken.
 1345 *sdvdn, -(*sdved), std::pow(sdvNED->z(), 2);
122
1/2
✓ Branch 2 taken 1345 times.
✗ Branch 3 not taken.
 1345 this->setVelocityAndStdDev_n(*n_velocity, cov);
123 }
124
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 538 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 538 else if (e_velocity) { this->setVelocity_e(*e_velocity); }
125
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 538 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 538 else if (n_velocity) { this->setVelocity_n(*n_velocity); }
126 2421 }
127 #endif
128
129 /// @brief Returns the type of the data class
130 /// @return The data type
131 895 [[nodiscard]] static std::string type()
132 {
133
1/2
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
 1790 return "RtklibPosObs";
134 }
135
136 /// @brief Returns the type of the data class
137 /// @return The data type
138 ✗ [[nodiscard]] std::string getType() const override { return type(); }
139
140 /// @brief Returns the parent types of the data class
141 /// @return The parent data types
142 112 [[nodiscard]] static std::vector<std::string> parentTypes()
143 {
144 112 auto parent = PosVel::parentTypes();
145
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 112 parent.push_back(PosVel::type());
146 112 return parent;
147 ✗ }
148
149 /// @brief Returns a vector of data descriptors
150 7 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
151 {
152 7 auto desc = PosVel::GetStaticDataDescriptors();
153
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 desc.reserve(GetStaticDescriptorCount());
154
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("Q [-]");
155
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("ns [-]");
156
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("age [s]");
157
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("ratio [-]");
158
159 7 return desc;
160 ✗ }
161
162 /// @brief Get the amount of descriptors
163 7 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 4; }
164
165 /// @brief Returns a vector of data descriptors
166 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
167
168 /// @brief Get the amount of descriptors
169 ✗ [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
170
171 /// @brief Get the value at the index
172 /// @param idx Index corresponding to data descriptor order
173 /// @return Value if in the observation
174 ✗ [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
175 {
176 ✗ INS_ASSERT(idx < GetStaticDescriptorCount());
177 ✗ if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); }
178 ✗ switch (idx)
179 {
180 ✗ case PosVel::GetStaticDescriptorCount() + 0: // Q [-]
181 ✗ return Q;
182 ✗ case PosVel::GetStaticDescriptorCount() + 1: // ns [-]
183 ✗ return ns;
184 ✗ case PosVel::GetStaticDescriptorCount() + 2: // age [s]
185 ✗ return age;
186 ✗ case PosVel::GetStaticDescriptorCount() + 3: // ratio [-]
187 ✗ return ratio;
188 ✗ default:
189 ✗ return std::nullopt;
190 }
191 return std::nullopt;
192 }
193
194 /// 1:fix, 2:float, 3:sbas, 4:dgps, 5:single, 6:ppp
195 uint8_t Q = 0;
196 /// Number of satellites
197 uint8_t ns = 0;
198 /// Age [s]
199 double age = std::nan("");
200 /// Ratio
201 double ratio = std::nan("");
202 };
203
204 } // namespace NAV
205