INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Positioning/ObservationFilter.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 111 226 49.1%
Functions: 9 16 56.2%
Branches: 133 457 29.1%
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 ObservationFilter.hpp
10 /// @brief Observation Filter
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-12-21
13
14 #pragma once
15
16 #include <algorithm>
17 #include <cstddef>
18 #include <cstdint>
19 #include <memory>
20 #include <unordered_map>
21 #include <unordered_set>
22 #include <utility>
23 #include <array>
24 #include <vector>
25
26 #include <imgui.h>
27 #include "Navigation/GNSS/Core/SatelliteSystem.hpp"
28 #include "internal/gui/widgets/imgui_ex.hpp"
29
30 #include "Navigation/GNSS/Core/Code.hpp"
31 #include "Navigation/GNSS/Core/Frequency.hpp"
32 #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
33 #include "Navigation/GNSS/Positioning/Observation.hpp"
34 #include "Navigation/GNSS/Positioning/Receiver.hpp"
35 #include "Navigation/GNSS/SNRMask.hpp"
36 #include "Navigation/GNSS/Satellite/Ephemeris/GLONASSEphemeris.hpp"
37 #include "Navigation/Transformations/Units.hpp"
38
39 #include "NodeData/GNSS/GnssNavInfo.hpp"
40 #include "NodeData/GNSS/GnssObs.hpp"
41
42 #include "util/Assert.h"
43 #include "util/Container/STL.hpp"
44 #include "util/Json.hpp"
45 #include "util/Logger.hpp"
46 #include <fmt/core.h>
47
48 namespace NAV
49 {
50
51 /// Observation Filter
52 class ObservationFilter
53 {
54 public:
55 /// @brief Constructor
56 /// @param[in] receiverCount Number of receivers
57 /// @param[in] availableObsTypes Available observation types (e.g. SPP does not have Carrier)
58 /// @param[in] neededObsTypes Needed observation types (cannot be unchecked)
59 120 explicit ObservationFilter(size_t receiverCount,
60 const std::unordered_set<GnssObs::ObservationType>& availableObsTypes = { GnssObs::Pseudorange, GnssObs::Carrier, GnssObs::Doppler },
61 std::unordered_set<GnssObs::ObservationType> neededObsTypes = {})
62
3/6
✓ Branch 11 taken 120 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 120 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 120 times.
✗ Branch 20 not taken.
 360 : _snrMask(receiverCount), _availableObsTypes(availableObsTypes), _neededObsTypes(std::move(neededObsTypes)), _usedObsTypes(availableObsTypes)
63 {
64 // Disable Geostationary satellites, as they not working correctly
65
3/4
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 840 times.
✓ Branch 9 taken 120 times.
 960 for (const auto& satSys : SatelliteSystem::GetAll())
66 {
67
3/4
✓ Branch 1 taken 840 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 21600 times.
✓ Branch 9 taken 840 times.
 22440 for (const auto& satNum : satSys.getSatellites())
68 {
69 21600 if (SatId satId(satSys, satNum);
70
4/6
✓ Branch 1 taken 21600 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1560 times.
✓ Branch 4 taken 20040 times.
✓ Branch 6 taken 1560 times.
✗ Branch 7 not taken.
 21600 satId.isGeo()) { _excludedSatellites.push_back(satId); }
71 840 }
72 120 }
73 120 }
74
75 /// @brief Destructor
76 316 ~ObservationFilter() = default;
77 /// @brief Copy constructor
78
5/10
✓ Branch 2 taken 196 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 196 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 196 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 196 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 196 times.
✗ Branch 15 not taken.
 196 ObservationFilter(const ObservationFilter& other) = default;
79 /// @brief Move constructor
80 ObservationFilter(ObservationFilter&& other) noexcept = default;
81 /// @brief Copy assignment operator
82 ObservationFilter& operator=(const ObservationFilter& other)
83 {
84 if (this != &other) // not a self-assignment
85 {
86 _filterFreq = other._filterFreq;
87 _filterCode = other._filterCode;
88 _excludedSatellites = other._excludedSatellites;
89 _elevationMask = other._elevationMask;
90 _snrMask = other._snrMask;
91 _sameSnrMaskForAllReceivers = other._sameSnrMaskForAllReceivers;
92 _neededObsTypes = other._neededObsTypes;
93 _usedObsTypes = other._usedObsTypes;
94 std::vector<GnssObs::ObservationType> obsTypeToRemove;
95 for (const auto& obsType : _usedObsTypes)
96 {
97 if (!_availableObsTypes.contains(obsType)) { obsTypeToRemove.push_back(obsType); }
98 }
99 for (const auto& obsType : obsTypeToRemove)
100 {
101 _usedObsTypes.erase(obsType);
102 }
103 }
104 return *this;
105 }
106 /// @brief Move assignment operator
107 ObservationFilter& operator=(ObservationFilter&& other) noexcept
108 {
109 if (this != &other) // not a self-assignment
110 {
111 _filterFreq = other._filterFreq;
112 _filterCode = other._filterCode;
113 _excludedSatellites = std::move(other._excludedSatellites);
114 _elevationMask = other._elevationMask;
115 _snrMask = std::move(other._snrMask);
116 _sameSnrMaskForAllReceivers = other._sameSnrMaskForAllReceivers;
117 _neededObsTypes = std::move(other._neededObsTypes);
118 _usedObsTypes = std::move(other._usedObsTypes);
119 std::vector<GnssObs::ObservationType> obsTypeToRemove;
120 for (const auto& obsType : _usedObsTypes)
121 {
122 if (!_availableObsTypes.contains(obsType)) { obsTypeToRemove.push_back(obsType); }
123 }
124 for (const auto& obsType : obsTypeToRemove)
125 {
126 _usedObsTypes.erase(obsType);
127 }
128 }
129 return *this;
130 }
131
132 /// @brief Reset the temporary settings
133 24 void reset()
134 {
135 24 _temporarilyExcludedSignalsSatellites.clear();
136 24 }
137
138 /// Filtered signals
139 struct Filtered
140 {
141 std::vector<SatSigId> frequencyFilter; ///< Signals excluded because the frequency is not used
142 std::vector<SatSigId> codeFilter; ///< Signals excluded because the code is not used
143 std::vector<SatSigId> excludedSatellites; ///< Signals excluded because the satellite is excluded
144 std::vector<SatSigId> tempExcludedSignal; ///< Signals temporarily excluded
145 std::vector<SatSigId> notAllReceiversObserved; ///< Signals not observed by all receivers
146 std::vector<SatSigId> singleObservation; ///< Only signal for this code/type (relevant for double differences)
147 std::vector<SatSigId> noPseudorangeMeasurement; ///< Signals without pseudorange measurement
148 std::vector<SatSigId> navigationDataMissing; ///< Signals without navigation data
149 std::vector<std::pair<SatSigId, double>> elevationMaskTriggered; ///< Signals triggering the elevation mask. Also includes elevation [rad]
150 std::vector<std::pair<SatSigId, double>> snrMaskTriggered; ///< Signals triggering the SNR mask. Also includes the Carrier-to-Noise density [dBHz]
151 };
152
153 /// @brief Returns a list of satellites and observations filtered by GUI settings & NAV data available & ...)
154 /// @param[in] receiverType Receiver type index to filter
155 /// @param[in] e_posMarker Marker Position in ECEF frame [m]
156 /// @param[in] lla_posMarker Marker Position in LLA frame [rad, rad, m]
157 /// @param[in] gnssObs GNSS observation
158 /// @param[in] gnssNavInfos Collection of navigation data providers
159 /// @param[in] nameId Name and Id of the node used for log messages only
160 /// @param[in] observations List of observations which will be filled. If you have multiple receivers, the observations list will be the same object
161 /// @param[in] filtered Optional Filtered object to get back the filtered signals
162 /// @param[in] ignoreElevationMask Flag wether the elevation mask should be ignored
163 template<typename ReceiverType, typename DerivedPe, typename DerivedPn>
164 1127 void selectObservationsForCalculation(ReceiverType receiverType,
165 const Eigen::MatrixBase<DerivedPe>& e_posMarker,
166 const Eigen::MatrixBase<DerivedPn>& lla_posMarker,
167 const std::shared_ptr<const GnssObs>& gnssObs,
168 const std::vector<const GnssNavInfo*>& gnssNavInfos,
169 Observations& observations,
170 Filtered* filtered,
171 [[maybe_unused]] const std::string& nameId,
172 bool ignoreElevationMask = false)
173 {
174 1127 bool firstReceiver = observations.receivers.empty();
175
1/2
✓ Branch 1 taken 1127 times.
✗ Branch 2 not taken.
 1127 observations.receivers.insert(receiverType);
176
177
1/2
✓ Branch 3 taken 1127 times.
✗ Branch 4 not taken.
 1127 observations.signals.reserve(gnssObs->data.size());
178
179
6/6
✓ Branch 1 taken 41798 times.
✓ Branch 2 taken 3500 times.
✓ Branch 4 taken 41798 times.
✓ Branch 5 taken 3500 times.
✓ Branch 8 taken 180656 times.
✓ Branch 9 taken 1127 times.
 230581 for (size_t obsIdx = 0; obsIdx < gnssObs->data.size(); obsIdx++)
180 {
181
1/2
✓ Branch 2 taken 180656 times.
✗ Branch 3 not taken.
 180656 const GnssObs::ObservationData& obsData = gnssObs->data.at(obsIdx);
182 180656 SatSigId satSigId = obsData.satSigId;
183
1/2
✓ Branch 1 taken 180656 times.
✗ Branch 2 not taken.
 180656 SatId satId = satSigId.toSatId();
184 LOG_DATA("{}: Considering [{}] for receiver {}", nameId, satSigId, receiverType);
185
186 // Decrease the temporary exclude counter
187
4/8
✓ Branch 0 taken 180656 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 180656 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 180656 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 180656 times.
 180656 if (firstReceiver && _temporarilyExcludedSignalsSatellites.contains(satSigId))
188 {
189 ✗ if (_temporarilyExcludedSignalsSatellites.at(satSigId)-- == 0)
190 {
191 ✗ _temporarilyExcludedSignalsSatellites.erase(satSigId);
192 }
193 }
194
195
3/4
✓ Branch 1 taken 180656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 132640 times.
✓ Branch 5 taken 48016 times.
 180656 if (!(satSigId.freq() & _filterFreq))
196 {
197 LOG_DATA("{}: [{}] Skipping obs due to GUI frequency filter", nameId, satSigId);
198
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 132640 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 132640 if (filtered) { filtered->frequencyFilter.push_back(satSigId); }
199 138858 continue;
200 }
201
3/4
✓ Branch 1 taken 48016 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2718 times.
✓ Branch 5 taken 45298 times.
 48016 if (!(satSigId.code & _filterCode))
202 {
203 LOG_DATA("{}: [{}] Skipping obs due to GUI code filter", nameId, satSigId);
204
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2718 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 2718 if (filtered) { filtered->codeFilter.push_back(satSigId); }
205 2718 continue;
206 }
207
2/4
✓ Branch 2 taken 45298 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 45298 times.
 45298 if (std::ranges::find(_excludedSatellites, satId) != _excludedSatellites.end())
208 {
209 LOG_DATA("{}: [{}] Skipping obs due to GUI excluded satellites", nameId, satSigId);
210 ✗ if (filtered) { filtered->excludedSatellites.push_back(satSigId); }
211 ✗ continue;
212 }
213
2/4
✓ Branch 1 taken 45298 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 45298 times.
 45298 if (_temporarilyExcludedSignalsSatellites.contains(satSigId))
214 {
215 LOG_DATA("{}: [{}] Skipping obs because temporarily excluded signal", nameId, satSigId);
216 ✗ if (filtered) { filtered->tempExcludedSignal.push_back(satSigId); }
217 ✗ continue;
218 }
219
220
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 45298 times.
 45298 if (!obsData.pseudorange)
221 {
222 LOG_DATA("{}: [{}] Skipping obs because no pseudorange measurement (needed for satellite position calculation)", nameId, satSigId);
223 ✗ if (filtered) { filtered->noPseudorangeMeasurement.push_back(satSigId); }
224 ✗ continue;
225 }
226
227
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 45298 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 45298 times.
 45298 if (!firstReceiver && !observations.signals.contains(satSigId)) // TODO:
228 {
229 ✗ bool signalWithSameFrequencyFound = false;
230 ✗ for (const auto& signals : observations.signals)
231 {
232 ✗ if (signals.first.toSatId() == satId && signals.first.freq() == satSigId.freq() // e.g. Rover has [G5Q], but Base has [G5X]
233 ✗ && signals.second.recvObs.size() != observations.receivers.size()) // But not: Rover has [G5Q], but Base has [G5Q] and [G5X]
234 {
235 LOG_DATA("{}: [{}] Not observed by all receivers, but other receivers have [{}]. Treating as such.",
236 nameId, satSigId, signals.first);
237 ✗ satSigId = signals.first;
238 ✗ satId = satSigId.toSatId();
239 ✗ signalWithSameFrequencyFound = true;
240 ✗ break;
241 }
242 }
243 ✗ if (!signalWithSameFrequencyFound)
244 {
245 LOG_DATA("{}: [{}] Skipping obs because not observed by all receivers", nameId, satSigId);
246 ✗ if (filtered) { filtered->notAllReceiversObserved.push_back((satSigId)); }
247 ✗ continue;
248 }
249 }
250
251 45298 std::shared_ptr<NAV::SatNavData> satNavData = nullptr;
252
2/4
✗ Branch 4 not taken.
✓ Branch 5 taken 45298 times.
✓ Branch 8 taken 45298 times.
✗ Branch 9 not taken.
 90596 for (const auto* gnssNavInfo : gnssNavInfos)
253 {
254
1/2
✓ Branch 2 taken 45298 times.
✗ Branch 3 not taken.
 45298 auto satNav = gnssNavInfo->searchNavigationData(satId, gnssObs->insTime);
255
4/8
✓ Branch 1 taken 45298 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45298 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 45298 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 45298 times.
✗ Branch 10 not taken.
 45298 if (satNav && satNav->isHealthy())
256 {
257 45298 satNavData = satNav;
258 45298 break;
259 }
260 }
261
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 45298 times.
 45298 if (satNavData == nullptr)
262 {
263 LOG_DATA("{}: [{}] Skipping obs because no navigation data available to calculaten the satellite position", nameId, satSigId);
264 ✗ if (filtered) { filtered->navigationDataMissing.push_back(satSigId); }
265 ✗ continue;
266 }
267
268 45298 int8_t freqNum = -128;
269
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 45298 times.
 45298 if (satId.satSys == GLO)
270 {
271 ✗ if (auto gloSatNavData = std::dynamic_pointer_cast<GLONASSEphemeris>(satNavData))
272 {
273 ✗ freqNum = gloSatNavData->frequencyNumber;
274 }
275 }
276
277
1/2
✓ Branch 2 taken 45298 times.
✗ Branch 3 not taken.
 90596 auto satClk = satNavData->calcClockCorrections(gnssObs->insTime,
278 45298 obsData.pseudorange->value,
279
1/2
✓ Branch 1 taken 45298 times.
✗ Branch 2 not taken.
 45298 satSigId.freq());
280
1/2
✓ Branch 2 taken 45298 times.
✗ Branch 3 not taken.
 45298 auto satPosVel = satNavData->calcSatellitePosVel(satClk.transmitTime);
281
282
1/2
✓ Branch 1 taken 45298 times.
✗ Branch 2 not taken.
 45298 auto recvData = std::make_shared<Observations::SignalObservation::ReceiverSpecificData>(
283 gnssObs, obsIdx,
284 satPosVel.e_pos, satPosVel.e_vel, satClk);
285
286
2/2
✓ Branch 0 taken 43503 times.
✓ Branch 1 taken 1795 times.
 45298 if (!ignoreElevationMask)
287 {
288
3/6
✓ Branch 2 taken 43503 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 43503 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 43503 times.
✗ Branch 9 not taken.
 43503 const auto& satElevation = recvData->satElevation(e_posMarker, lla_posMarker);
289
2/2
✓ Branch 0 taken 3500 times.
✓ Branch 1 taken 40003 times.
 43503 if (satElevation < _elevationMask)
290 {
291 LOG_DATA("{}: Signal {} is skipped because of elevation mask. ({} < {})", nameId, satSigId,
292 rad2deg(satElevation), rad2deg(_elevationMask));
293
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3500 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 3500 if (filtered) { filtered->elevationMaskTriggered.emplace_back(satSigId, satElevation); }
294 3500 continue;
295 }
296 40003 if (obsData.CN0 // If no CN0 available, we do not check the SNR mask, so we use the signal
297
2/4
✓ Branch 1 taken 40003 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 40003 times.
 80006 && !_snrMask
298
2/4
✓ Branch 0 taken 40003 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 40003 times.
✗ Branch 4 not taken.
 40003 .at(_sameSnrMaskForAllReceivers ? static_cast<ReceiverType>(0) : receiverType)
299
4/8
✓ Branch 1 taken 40003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40003 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40003 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 40003 times.
 40003 .checkSNRMask(satSigId.freq(), satElevation, obsData.CN0.value()))
300 {
301 LOG_DATA("{}: [{}] SNR mask triggered for [{}] on receiver [{}] with CN0 {} dbHz",
302 nameId, gnssObs->insTime.toYMDHMS(GPST), satSigId, receiverType, *obsData.CN0);
303 ✗ if (filtered) { filtered->snrMaskTriggered.emplace_back(satSigId, *obsData.CN0); }
304 ✗ continue;
305 }
306 }
307
308
2/2
✓ Branch 5 taken 83596 times.
✓ Branch 6 taken 41798 times.
 125394 for (const GnssObs::ObservationType& obsType : _usedObsTypes)
309 {
310 84472 auto removeObsTypeIfExist = [&]() {
311
1/2
✓ Branch 1 taken 876 times.
✗ Branch 2 not taken.
 876 if (!observations.signals.contains(satSigId)) { return; }
312 ✗ std::for_each(observations.signals.at(satSigId).recvObs.begin(),
313 ✗ observations.signals.at(satSigId).recvObs.end(),
314 ✗ [&](auto& r) {
315 ✗ if (r.second->obs.contains(obsType))
316 {
317 LOG_DATA("{}: [{}] Erasing previously added obs '{}' on this signal.", nameId, satSigId, obsType);
318 ✗ r.second->obs.erase(obsType);
319 }
320 });
321 };
322
323 167192 if (!firstReceiver
324
2/10
✗ Branch 0 not taken.
✓ Branch 1 taken 83596 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 83596 times.
 83596 && std::any_of(observations.signals.at(satSigId).recvObs.begin(),
325 ✗ observations.signals.at(satSigId).recvObs.end(),
326 ✗ [&](const auto& r) {
327 ✗ return !r.second->obs.contains(obsType);
328 }))
329 {
330 LOG_DATA("{}: [{}][{}] Skipping '{}' measurement. Not all receivers have this observation.", nameId, receiverType, satSigId, obsType);
331 ✗ removeObsTypeIfExist();
332 ✗ continue;
333 }
334
2/5
✓ Branch 0 taken 41798 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 41798 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 83596 switch (obsType)
335 {
336 41798 case GnssObs::Pseudorange:
337
2/4
✓ Branch 2 taken 41798 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 41798 times.
✗ Branch 6 not taken.
 41798 if (recvData->gnssObsData().pseudorange)
338 {
339
2/4
✓ Branch 2 taken 41798 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 41798 times.
✗ Branch 8 not taken.
 41798 recvData->obs[obsType].measurement = recvData->gnssObsData().pseudorange->value;
340 LOG_DATA("{}: [{}] Taking {:11} observation into account on {:5} receiver ({:.3f} [m])", nameId, satSigId,
341 obsType, receiverType, recvData->obs[obsType].measurement);
342 }
343 ✗ else { removeObsTypeIfExist(); }
344 41798 break;
345 ✗ case GnssObs::Carrier:
346 ✗ if (recvData->gnssObsData().carrierPhase)
347 {
348 ✗ recvData->obs[obsType].measurement = InsConst::C / satSigId.freq().getFrequency(freqNum)
349 ✗ * recvData->gnssObsData().carrierPhase->value;
350 LOG_DATA("{}: [{}] Taking {:11} observation into account on {:5} receiver ({:.3f} [m] = {:.3f} [cycles])", nameId, satSigId,
351 obsType, receiverType, recvData->obs[obsType].measurement, recvData->gnssObsData().carrierPhase->value);
352 }
353 ✗ else { removeObsTypeIfExist(); }
354 ✗ break;
355 41798 case GnssObs::Doppler:
356
3/4
✓ Branch 2 taken 41798 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40922 times.
✓ Branch 6 taken 876 times.
 41798 if (recvData->gnssObsData().doppler)
357 {
358
5/10
✓ Branch 1 taken 40922 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40922 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40922 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 40922 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 40922 times.
✗ Branch 16 not taken.
 40922 recvData->obs[obsType].measurement = doppler2rangeRate(recvData->gnssObsData().doppler.value(),
359 satSigId.freq(),
360 freqNum);
361 LOG_DATA("{}: [{}] Taking {:11} observation into account on {:5} receiver ({:.3f} [m/s] = {:.3f} [Hz])", nameId, satSigId,
362 obsType, receiverType, recvData->obs[obsType].measurement, recvData->gnssObsData().doppler.value());
363 }
364
1/2
✓ Branch 1 taken 876 times.
✗ Branch 2 not taken.
 876 else { removeObsTypeIfExist(); }
365 41798 break;
366 ✗ case GnssObs::ObservationType_COUNT:
367 ✗ break;
368 }
369 }
370
371 83596 if (!firstReceiver
372
2/10
✗ Branch 0 not taken.
✓ Branch 1 taken 41798 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 41798 times.
 41798 && std::any_of(observations.signals.at(satSigId).recvObs.begin(),
373 ✗ observations.signals.at(satSigId).recvObs.end(),
374 ✗ [&](const auto& r) {
375 ✗ return r.second->obs.empty();
376 }))
377 {
378 LOG_DATA("{}: [{}] Skipping obs because not observed by all receivers", nameId, satSigId);
379 ✗ if (filtered) { filtered->notAllReceiversObserved.push_back(satSigId); }
380 ✗ observations.signals.erase(satSigId);
381 ✗ continue;
382 }
383 LOG_DATA("{}: Adding satellite [{}] for receiver {}", nameId, satSigId, receiverType);
384
2/4
✓ Branch 1 taken 41798 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 41798 times.
✗ Branch 4 not taken.
 41798 if (!observations.signals.contains(satSigId))
385 {
386
1/2
✓ Branch 2 taken 41798 times.
✗ Branch 3 not taken.
 41798 observations.signals.insert(std::make_pair(satSigId,
387 83596 Observations::SignalObservation{ satNavData, freqNum }));
388 }
389
2/4
✓ Branch 1 taken 41798 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 41798 times.
✗ Branch 5 not taken.
 41798 observations.signals.at(satSigId).recvObs.emplace(receiverType, recvData);
390 }
391 1127 std::vector<SatSigId> sigToRemove;
392
2/2
✓ Branch 7 taken 41798 times.
✓ Branch 8 taken 1127 times.
 42925 for (const auto& [satSigId, sigObs] : observations.signals)
393 {
394
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 41798 times.
 41798 if (sigObs.recvObs.size() != observations.receivers.size())
395 {
396 ✗ sigToRemove.push_back(satSigId);
397 }
398 }
399
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 1127 times.
 1127 for (const auto& satSigId : sigToRemove)
400 {
401 LOG_DATA("{}: [{}] Removing signal because not observed by all receivers.", nameId, satSigId);
402 ✗ if (filtered) { filtered->notAllReceiversObserved.push_back(satSigId); }
403 ✗ observations.signals.erase(satSigId);
404 }
405
406
1/2
✓ Branch 1 taken 1127 times.
✗ Branch 2 not taken.
 1127 observations.recalcObservableCounts(nameId);
407
408 #if LOG_LEVEL <= LOG_LEVEL_DATA
409 LOG_DATA("{}: usedSatSystems = [{}]", nameId, joinToString(observations.systems));
410 size_t nMeasTotal = 0;
411 std::string nMeasStr;
412 for (size_t obsType = 0; obsType < GnssObs::ObservationType_COUNT; obsType++)
413 {
414 auto& nMeas = observations.nObservables.at(obsType);
415 nMeasStr += fmt::format("{} {}, ", nMeas, static_cast<GnssObs::ObservationType>(obsType));
416 nMeasTotal += nMeas;
417 }
418 if (nMeasStr.ends_with(", ")) { nMeasStr = nMeasStr.erase(nMeasStr.length() - 2); }
419
420 LOG_DATA("{}: Using {} measurements ({}) from {} satellites", nameId, nMeasTotal, nMeasStr, observations.satellites.size());
421
422 unordered_map<SatId, std::pair<Frequency, Code>> satData;
423 unordered_map<SatSigId, std::set<GnssObs::ObservationType>> sigData;
424 for (const auto& obs : observations.signals)
425 {
426 satData[obs.first.toSatId()].first |= obs.first.freq();
427 satData[obs.first.toSatId()].second |= obs.first.code;
428 for (size_t obsType = 0; obsType < GnssObs::ObservationType_COUNT; obsType++)
429 {
430 if (std::ranges::all_of(obs.second.recvObs, [&obsType](const auto& recvObs) {
431 return recvObs.second->obs.contains(static_cast<GnssObs::ObservationType>(obsType));
432 }))
433 {
434 sigData[obs.first].insert(static_cast<GnssObs::ObservationType>(obsType));
435 }
436 }
437 }
438 for ([[maybe_unused]] const auto& [satId, freqCode] : satData)
439 {
440 LOG_DATA("{}: [{}] on frequencies [{}] with codes [{}]", nameId, satId, freqCode.first, freqCode.second);
441 for (const auto& [satSigId, obs] : sigData)
442 {
443 if (satSigId.toSatId() != satId) { continue; }
444 std::string str;
445 for (const auto& o : obs)
446 {
447 if (!str.empty()) { str += ", "; }
448 str += fmt::format("{}", o);
449 }
450 LOG_DATA("{}: [{}] has obs: {}", nameId, satSigId.code, str);
451 }
452 }
453 #endif
454 1127 }
455
456 /// @brief Shows the GUI input to select the options
457 /// @param[in] id Unique id for ImGui.
458 /// @param[in] itemWidth Width of the widgets
459 template<typename ReceiverType>
460 ✗ bool ShowGuiWidgets(const char* id, float itemWidth)
461 {
462 ✗ bool changed = false;
463
464 ✗ ImGui::SetNextItemWidth(itemWidth);
465 ✗ if (ShowFrequencySelector(fmt::format("Satellite Frequencies##{}", id).c_str(), _filterFreq))
466 {
467 ✗ changed = true;
468 }
469
470 ✗ ImGui::SetNextItemWidth(itemWidth);
471 ✗ if (ShowCodeSelector(fmt::format("Signal Codes##{}", id).c_str(), _filterCode, _filterFreq))
472 {
473 ✗ changed = true;
474 }
475
476 ✗ ImGui::SetNextItemWidth(itemWidth);
477 ✗ if (ShowSatelliteSelector(fmt::format("Excluded satellites##{}", id).c_str(), _excludedSatellites))
478 {
479 ✗ changed = true;
480 }
481
482 ✗ double elevationMaskDeg = rad2deg(_elevationMask);
483 ✗ ImGui::SetNextItemWidth(itemWidth);
484 ✗ if (ImGui::InputDoubleL(fmt::format("Elevation mask##{}", id).c_str(), &elevationMaskDeg, 0.0, 90.0, 5.0, 5.0, "%.1f°", ImGuiInputTextFlags_AllowTabInput))
485 {
486 ✗ _elevationMask = deg2rad(elevationMaskDeg);
487 ✗ LOG_DEBUG("{}: Elevation mask changed to {}°", id, elevationMaskDeg);
488 ✗ changed = true;
489 }
490
491 ✗ for (size_t i = 0; i < _snrMask.size(); ++i)
492 {
493 ✗ if (i != 0)
494 {
495 ✗ ImGui::SameLine();
496 ✗ if (_sameSnrMaskForAllReceivers) { ImGui::BeginDisabled(); }
497 }
498 ✗ if (_snrMask.at(i).ShowGuiWidgets(fmt::format("{} SNR Mask", static_cast<ReceiverType>(i)).c_str()))
499 {
500 ✗ changed = true;
501 }
502 ✗ if (i != 0 && _sameSnrMaskForAllReceivers) { ImGui::EndDisabled(); }
503 }
504 ✗ if (_snrMask.size() > 1)
505 {
506 ✗ ImGui::SameLine();
507 ✗ if (ImGui::Checkbox(fmt::format("Use same SNR for all receivers##{}", id).c_str(), &_sameSnrMaskForAllReceivers))
508 {
509 ✗ changed = true;
510 }
511 }
512
513 ✗ ImGui::BeginHorizontal(fmt::format("Observables##{}", id).c_str(),
514 ✗ ImVec2(itemWidth - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x, 0.0F));
515 ✗ for (size_t i = 0; i < GnssObs::ObservationType_COUNT; i++)
516 {
517 ✗ auto obsType = static_cast<GnssObs::ObservationType>(i);
518 ✗ if (!_availableObsTypes.contains(obsType)) { continue; }
519 ✗ if (_neededObsTypes.contains(obsType)) { ImGui::BeginDisabled(); }
520 ✗ bool enabled = _usedObsTypes.contains(obsType);
521 ✗ if (ImGui::Checkbox(fmt::format("{}##{}", obsType, id).c_str(), &enabled))
522 {
523 ✗ LOG_DEBUG("{}: Using {}: {}", id, obsType, enabled);
524 ✗ if (enabled) { _usedObsTypes.insert(obsType); }
525 ✗ else { _usedObsTypes.erase(obsType); }
526 ✗ changed = true;
527 }
528 ✗ if (_neededObsTypes.contains(obsType)) { ImGui::EndDisabled(); }
529 }
530 ✗ ImGui::EndHorizontal();
531
532 ✗ ImGui::SameLine();
533 ✗ ImGui::TextUnformatted("Used observables");
534
535 ✗ return changed;
536 }
537
538 /// @brief Checks if the satellite is allowed. Does not check elevation or SNR mask
539 /// @param[in] satId Satellite Identifier
540 ✗ [[nodiscard]] bool isSatelliteAllowed(const SatId& satId) const
541 {
542 ✗ return (satId.satSys & _filterFreq)
543 ✗ && std::ranges::find(_excludedSatellites, satId) == _excludedSatellites.end();
544 }
545
546 /// @brief Checks if the Observation type is used by the GUI settings
547 /// @param[in] obsType Observation Type
548 5580 [[nodiscard]] bool isObsTypeUsed(GnssObs::ObservationType obsType) const
549 {
550 5580 return _usedObsTypes.contains(obsType);
551 }
552
553 /// @brief Set the observation type to use
554 /// @param obsType Observation Type
555 void useObsType(GnssObs::ObservationType obsType)
556 {
557 _usedObsTypes.insert(obsType);
558 }
559
560 /// @brief Set the observation type as needed (cannot be unchecked in the GUI) or unneeded
561 /// @param obsType Observation Type
562 /// @param needed Needed or unneeded
563 void markObsTypeAsNeeded(GnssObs::ObservationType obsType, bool needed = true)
564 {
565 if (needed) { _neededObsTypes.insert(obsType); }
566 else if (_neededObsTypes.contains(obsType)) { _neededObsTypes.erase(obsType); }
567 }
568
569 /// @brief Temporarily excludes a signal
570 /// @param[in] satSigId Satellite Signal Id
571 /// @param[in] count Amount of function calls to exclude
572 void excludeSignalTemporarily(const SatSigId& satSigId, size_t count)
573 {
574 if (count == 0) { return; }
575 _temporarilyExcludedSignalsSatellites[satSigId] = count;
576 }
577
578 /// @brief Get the Frequency Filter
579 ✗ [[nodiscard]] const Frequency& getFrequencyFilter() const
580 {
581 ✗ return _filterFreq;
582 }
583 /// @brief Get the Code Filter
584 [[nodiscard]] const Code& getCodeFilter() const
585 {
586 return _filterCode;
587 }
588
589 /// @brief Get the Satellite System Filter
590 168 [[nodiscard]] SatelliteSystem getSystemFilter() const
591 {
592 168 return _filterFreq.getSatSys();
593 }
594
595 /// @brief Get the used observation types
596 [[nodiscard]] const std::unordered_set<GnssObs::ObservationType>& getUsedObservationTypes() const
597 {
598 return _usedObsTypes;
599 }
600
601 /// @brief Opens all settings to the maximum, disabling the filter
602 void disableFilter()
603 {
604 for (const auto& freq : Frequency::GetAll()) { _filterFreq |= freq; }
605 _filterCode = Code_ALL;
606 _excludedSatellites.clear();
607 _elevationMask = 0.0;
608 for (auto& snrMask : _snrMask) { snrMask.disable(); }
609 _usedObsTypes = { GnssObs::Pseudorange, GnssObs::Carrier, GnssObs::Doppler };
610 _temporarilyExcludedSignalsSatellites.clear();
611 }
612
613 private:
614 /// Frequencies used for calculation (GUI filter)
615 Frequency _filterFreq = G01 | G02 | G05
616 | E01 | E05 | E06 | E07 | E08;
617 /// Codes used for calculation (GUI filter)
618 Code _filterCode = Code_Default;
619 /// List of satellites to exclude
620 std::vector<SatId> _excludedSatellites;
621 /// Elevation cut-off angle for satellites in [rad]
622 double _elevationMask = static_cast<double>(10.0_deg);
623 /// SNR Mask for all receivers
624 std::vector<SNRMask> _snrMask;
625 /// Flag wether to use the same SNR mask for all receivers
626 bool _sameSnrMaskForAllReceivers = true;
627 /// Available observation types (e.g. SPP does not have Carrier)
628 const std::unordered_set<GnssObs::ObservationType> _availableObsTypes;
629 /// Needed observation types (cannot be unchecked in GUI)
630 std::unordered_set<GnssObs::ObservationType> _neededObsTypes;
631 /// Utilized observations
632 std::unordered_set<GnssObs::ObservationType> _usedObsTypes;
633
634 /// List of signals to exclude temporarily
635 std::unordered_map<SatSigId, size_t> _temporarilyExcludedSignalsSatellites;
636
637 /// @brief Converts the provided object into json
638 /// @param[out] j Json object which gets filled with the info
639 /// @param[in] obj Object to convert into json
640 ✗ friend void to_json(json& j, const ObservationFilter& obj)
641 {
642 ✗ j = json{
643 ✗ { "frequencies", Frequency_(obj._filterFreq) },
644 ✗ { "codes", obj._filterCode },
645 ✗ { "excludedSatellites", obj._excludedSatellites },
646 ✗ { "elevationMask", rad2deg(obj._elevationMask) },
647 ✗ { "snrMask", obj._snrMask },
648 ✗ { "sameSnrMaskForAllReceivers", obj._sameSnrMaskForAllReceivers },
649 ✗ { "usedObsTypes", obj._usedObsTypes },
650 ✗ { "neededObsType", obj._neededObsTypes },
651 ✗ };
652 ✗ }
653 /// @brief Converts the provided json object into a node object
654 /// @param[in] j Json object with the needed values
655 /// @param[out] obj Object to fill from the json
656 8 friend void from_json(const json& j, ObservationFilter& obj)
657 {
658
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("frequencies"))
659 {
660 8 uint64_t value = 0;
661
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 j.at("frequencies").get_to(value);
662 8 obj._filterFreq = Frequency_(value);
663 }
664
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("codes")) { j.at("codes").get_to(obj._filterCode); }
665
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("excludedSatellites"))
666 {
667 8 j.at("excludedSatellites").get_to(obj._excludedSatellites);
668 // Disable Geostationary satellites, as they not working correctly
669
3/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 56 times.
✓ Branch 9 taken 8 times.
 64 for (const auto& satSys : SatelliteSystem::GetAll())
670 {
671
3/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 1440 times.
✓ Branch 9 taken 56 times.
 1496 for (const auto& satNum : satSys.getSatellites())
672 {
673 1440 if (SatId satId(satSys, satNum);
674
1/2
✓ Branch 1 taken 1440 times.
✗ Branch 2 not taken.
 1440 satId.isGeo()
675
7/8
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 1336 times.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✓ Branch 8 taken 91 times.
✓ Branch 9 taken 13 times.
✓ Branch 10 taken 1427 times.
 1440 && std::ranges::find(obj._excludedSatellites, satId) == obj._excludedSatellites.end())
676 {
677
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 obj._excludedSatellites.push_back(satId);
678 }
679 56 }
680 8 }
681 }
682
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("elevationMask"))
683 {
684 8 j.at("elevationMask").get_to(obj._elevationMask);
685 8 obj._elevationMask = deg2rad(obj._elevationMask);
686 }
687
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("snrMask")) { j.at("snrMask").get_to(obj._snrMask); }
688
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("sameSnrMaskForAllReceivers")) { j.at("sameSnrMaskForAllReceivers").get_to(obj._sameSnrMaskForAllReceivers); }
689
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("usedObsTypes")) { j.at("usedObsTypes").get_to(obj._usedObsTypes); }
690
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 if (j.contains("neededObsTypes")) { j.at("neededObsTypes").get_to(obj._neededObsTypes); }
691 8 }
692 };
693
694 } // namespace NAV
695