INSTINCT Code Coverage Report

Directory: src/
File: NodeData/GNSS/RtklibPosObs.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 46 70 65.7%
Functions: 6 10 60.0%
Branches: 105 227 46.3%
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 366 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 2475 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 2475 : Q(Q), ns(ns), age(age), ratio(ratio)
82 {
83 2475 this->insTime = insTime;
84
85
6/10
✓ Branch 1 taken 1925 times.
✓ Branch 2 taken 550 times.
✓ Branch 5 taken 1925 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1925 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1925 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1925 times.
✗ Branch 16 not taken.
 2475 if (lla_position) { lla_position->head<2>() = deg2rad(lla_position->head<2>()); }
86
87
8/12
✓ Branch 1 taken 550 times.
✓ Branch 2 taken 1925 times.
✓ Branch 4 taken 550 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 550 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 550 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 550 times.
✓ Branch 16 taken 1925 times.
 2475 if (e_position && sdXYZ && sdxy && sdzx && sdyz)
88 {
89
1/2
✓ Branch 1 taken 550 times.
✗ Branch 2 not taken.
 550 Eigen::Matrix3d covPos;
90
4/8
✓ Branch 2 taken 550 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 550 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 550 times.
✗ Branch 15 not taken.
 550 covPos << std::pow(sdXYZ->x(), 2), *sdxy, -(*sdzx),
91
4/8
✓ Branch 2 taken 550 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 550 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 550 times.
✗ Branch 15 not taken.
 550 -(*sdxy), std::pow(sdXYZ->y(), 2), *sdyz,
92
4/8
✓ Branch 2 taken 550 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 550 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 550 times.
✗ Branch 15 not taken.
 550 *sdzx, -(*sdyz), std::pow(sdXYZ->z(), 2);
93
6/12
✓ Branch 1 taken 550 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 550 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 550 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 550 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 550 times.
✗ Branch 16 not taken.
 550 if (e_velocity && sdvXYZ && sdvxy && sdvzx && sdvyz)
94 {
95
2/4
✓ Branch 1 taken 550 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 550 times.
✗ Branch 5 not taken.
 550 Eigen::Matrix6d cov = Eigen::Matrix6d::Zero();
96
2/4
✓ Branch 1 taken 550 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 550 times.
✗ Branch 5 not taken.
 550 cov.topLeftCorner<3, 3>() = covPos;
97
5/10
✓ Branch 1 taken 550 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 550 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 550 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 550 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 550 times.
✗ Branch 18 not taken.
 550 cov.bottomRightCorner<3, 3>() << std::pow(sdvXYZ->x(), 2), *sdvxy, -(*sdvzx),
98
4/8
✓ Branch 2 taken 550 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 550 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 550 times.
✗ Branch 15 not taken.
 550 -(*sdvxy), std::pow(sdvXYZ->y(), 2), *sdvyz,
99
4/8
✓ Branch 2 taken 550 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 550 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 550 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 550 times.
✗ Branch 15 not taken.
 550 *sdvzx, -(*sdvyz), std::pow(sdvXYZ->z(), 2);
100
101
1/2
✓ Branch 3 taken 550 times.
✗ Branch 4 not taken.
 550 setPosVelAndCov_e(*e_position, *e_velocity, cov);
102 }
103 else
104 {
105 ✗ this->setPositionAndCov_e(*e_position, covPos);
106 }
107 }
108
6/12
✓ Branch 1 taken 1925 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1925 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1925 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1925 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1925 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1925 times.
✗ Branch 16 not taken.
 1925 else if (lla_position && sdNED && sdne && sddn && sded)
109 {
110
1/2
✓ Branch 1 taken 1925 times.
✗ Branch 2 not taken.
 1925 Eigen::Matrix3d covPos;
111
4/8
✓ Branch 2 taken 1925 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1925 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1925 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1925 times.
✗ Branch 15 not taken.
 1925 covPos << std::pow(sdNED->x(), 2), *sdne, -(*sddn),
112
4/8
✓ Branch 2 taken 1925 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1925 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1925 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1925 times.
✗ Branch 15 not taken.
 1925 -(*sdne), std::pow(sdNED->y(), 2), *sded,
113
4/8
✓ Branch 2 taken 1925 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1925 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1925 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1925 times.
✗ Branch 15 not taken.
 1925 *sddn, -(*sded), std::pow(sdNED->z(), 2);
114
115
8/12
✓ Branch 1 taken 1375 times.
✓ Branch 2 taken 550 times.
✓ Branch 4 taken 1375 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1375 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1375 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1375 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1375 times.
✓ Branch 16 taken 550 times.
 1925 if (n_velocity && sdvNED && sdvne && sdvdn && sdved)
116 {
117
2/4
✓ Branch 1 taken 1375 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1375 times.
✗ Branch 5 not taken.
 1375 Eigen::Matrix6d cov = Eigen::Matrix6d::Zero();
118
2/4
✓ Branch 1 taken 1375 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1375 times.
✗ Branch 5 not taken.
 1375 cov.topLeftCorner<3, 3>() = covPos;
119
5/10
✓ Branch 1 taken 1375 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1375 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1375 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1375 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1375 times.
✗ Branch 18 not taken.
 1375 cov.bottomRightCorner<3, 3>() << std::pow(sdvNED->x(), 2), *sdvne, -(*sdvdn),
120
4/8
✓ Branch 2 taken 1375 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1375 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1375 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1375 times.
✗ Branch 15 not taken.
 1375 -(*sdvne), std::pow(sdvNED->y(), 2), *sdved,
121
4/8
✓ Branch 2 taken 1375 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1375 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1375 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1375 times.
✗ Branch 15 not taken.
 1375 *sdvdn, -(*sdved), std::pow(sdvNED->z(), 2);
122
1/2
✓ Branch 3 taken 1375 times.
✗ Branch 4 not taken.
 1375 this->setPosVelAndCov_n(*lla_position, *n_velocity, cov);
123 }
124 else
125 {
126
1/2
✓ Branch 2 taken 550 times.
✗ Branch 3 not taken.
 550 this->setPositionAndCov_n(*lla_position, covPos);
127 }
128 }
129 ✗ else if (e_position) { this->setPosition_e(*e_position); }
130 ✗ else if (lla_position) { this->setPosition_lla(*lla_position); }
131
132 ✗ else if (e_velocity) { this->setVelocity_e(*e_velocity); }
133 ✗ else if (n_velocity) { this->setVelocity_n(*n_velocity); }
134 2475 }
135 #endif
136
137 /// @brief Returns the type of the data class
138 /// @return The data type
139 899 [[nodiscard]] static std::string type()
140 {
141
1/2
✓ Branch 1 taken 899 times.
✗ Branch 2 not taken.
 1798 return "RtklibPosObs";
142 }
143
144 /// @brief Returns the type of the data class
145 /// @return The data type
146 ✗ [[nodiscard]] std::string getType() const override { return type(); }
147
148 /// @brief Returns the parent types of the data class
149 /// @return The parent data types
150 112 [[nodiscard]] static std::vector<std::string> parentTypes()
151 {
152 112 auto parent = PosVel::parentTypes();
153
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());
154 112 return parent;
155 ✗ }
156
157 /// @brief Returns a vector of data descriptors
158 7 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
159 {
160 7 auto desc = PosVel::GetStaticDataDescriptors();
161
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 desc.reserve(GetStaticDescriptorCount());
162
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("Q [-]");
163
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("ns [-]");
164
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("age [s]");
165
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 desc.emplace_back("ratio [-]");
166
167 7 return desc;
168 ✗ }
169
170 /// @brief Get the amount of descriptors
171 7 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 4; }
172
173 /// @brief Returns a vector of data descriptors
174 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
175
176 /// @brief Get the amount of descriptors
177 ✗ [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
178
179 /// @brief Get the value at the index
180 /// @param idx Index corresponding to data descriptor order
181 /// @return Value if in the observation
182 ✗ [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
183 {
184 ✗ INS_ASSERT(idx < GetStaticDescriptorCount());
185 ✗ if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); }
186 ✗ switch (idx)
187 {
188 ✗ case PosVel::GetStaticDescriptorCount() + 0: // Q [-]
189 ✗ return Q;
190 ✗ case PosVel::GetStaticDescriptorCount() + 1: // ns [-]
191 ✗ return ns;
192 ✗ case PosVel::GetStaticDescriptorCount() + 2: // age [s]
193 ✗ return age;
194 ✗ case PosVel::GetStaticDescriptorCount() + 3: // ratio [-]
195 ✗ return ratio;
196 ✗ default:
197 ✗ return std::nullopt;
198 }
199 return std::nullopt;
200 }
201
202 /// 1:fix, 2:float, 3:sbas, 4:dgps, 5:single, 6:ppp
203 uint8_t Q = 0;
204 /// Number of satellites
205 uint8_t ns = 0;
206 /// Age [s]
207 double age = std::nan("");
208 /// Ratio
209 double ratio = std::nan("");
210 };
211
212 } // namespace NAV
213