| 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 Observation.hpp | ||
| 10 | /// @brief Observation data used for calculations | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-12-20 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cstdint> | ||
| 17 | #include <memory> | ||
| 18 | #include <set> | ||
| 19 | #include <unordered_map> | ||
| 20 | #include <unordered_set> | ||
| 21 | #include <utility> | ||
| 22 | |||
| 23 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 24 | #include "Navigation/GNSS/Functions.hpp" | ||
| 25 | #include "Navigation/GNSS/Satellite/internal/SatNavData.hpp" | ||
| 26 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 27 | #include "Navigation/Transformations/Units.hpp" | ||
| 28 | #include "Navigation/Atmosphere/Troposphere/ZenithDelay.hpp" | ||
| 29 | |||
| 30 | #include "NodeData/GNSS/GnssObs.hpp" | ||
| 31 | |||
| 32 | #include "util/Container/Unordered_map.hpp" | ||
| 33 | #include "util/Eigen.hpp" | ||
| 34 | #include "util/Logger.hpp" | ||
| 35 | |||
| 36 | namespace NAV | ||
| 37 | { | ||
| 38 | |||
| 39 | /// Observation storage type | ||
| 40 | struct Observations | ||
| 41 | { | ||
| 42 | /// @brief Receiver specific observation of the signal | ||
| 43 | struct SignalObservation | ||
| 44 | { | ||
| 45 | /// @brief Destructor | ||
| 46 | 125394 | ~SignalObservation() = default; | |
| 47 | /// @brief Copy constructor | ||
| 48 | /// @param other The other object | ||
| 49 | SignalObservation(const SignalObservation& other) | ||
| 50 | : _navData(other._navData), _freqNum(other._freqNum) | ||
| 51 | { | ||
| 52 | for (const auto& [index, obs] : other.recvObs) | ||
| 53 | { | ||
| 54 | recvObs.insert(std::make_pair(index, std::make_shared<ReceiverSpecificData>(*obs))); | ||
| 55 | } | ||
| 56 | } | ||
| 57 | /// @brief Copy assignment operator | ||
| 58 | /// @param other The other object | ||
| 59 | SignalObservation& operator=(const SignalObservation& other) | ||
| 60 | { | ||
| 61 | if (this == &other) { return *this; } // Guard self assignment | ||
| 62 | |||
| 63 | for (const auto& [index, obs] : other.recvObs) | ||
| 64 | { | ||
| 65 | recvObs.insert(std::make_pair(index, std::make_shared<ReceiverSpecificData>(*obs))); | ||
| 66 | } | ||
| 67 | _navData = other._navData; | ||
| 68 | _freqNum = other._freqNum; | ||
| 69 | |||
| 70 | return *this; | ||
| 71 | } | ||
| 72 | /// @brief Move constructor | ||
| 73 | /// @param other The other object | ||
| 74 | 83596 | SignalObservation(SignalObservation&& other) noexcept | |
| 75 | 83596 | : recvObs(std::move(other.recvObs)), _navData(std::move(other._navData)), _freqNum(other._freqNum) {} | |
| 76 | /// @brief Move assignment operator | ||
| 77 | /// @param other The other object | ||
| 78 | ✗ | SignalObservation& operator=(SignalObservation&& other) noexcept | |
| 79 | { | ||
| 80 | ✗ | if (this == &other) { return *this; } // Guard self assignment | |
| 81 | |||
| 82 | ✗ | recvObs = std::move(other.recvObs); | |
| 83 | ✗ | _navData = std::move(other._navData); | |
| 84 | ✗ | _freqNum = other._freqNum; | |
| 85 | |||
| 86 | ✗ | return *this; | |
| 87 | } | ||
| 88 | |||
| 89 | /// Receiver specific data | ||
| 90 | struct ReceiverSpecificData | ||
| 91 | { | ||
| 92 | /// Observations | ||
| 93 | struct Observation | ||
| 94 | { | ||
| 95 | double estimate = 0.0; ///< Estimate (psr [m], carrier [m], range-rate [m/s]) | ||
| 96 | double measurement = 0.0; ///< Measurement (psr [m], carrier [m], range-rate [m/s]) | ||
| 97 | double measVar = 0.0; ///< Variance of the measurement (psr [m^2], carrier [m^2], range-rate [m^2/s^2]) | ||
| 98 | }; | ||
| 99 | |||
| 100 | /// @brief Constructor | ||
| 101 | /// @param[in] gnssObs GNSS observation | ||
| 102 | /// @param[in] obsIdx GNSS observation index for this measurement | ||
| 103 | /// @param[in] e_satPos Satellite position in e frame | ||
| 104 | /// @param[in] e_satVel Satellite velocity in e frame | ||
| 105 | /// @param[in] satClock Satellite clock information | ||
| 106 | 45298 | ReceiverSpecificData(std::shared_ptr<const GnssObs> gnssObs, | |
| 107 | size_t obsIdx, | ||
| 108 | Eigen::Vector3d e_satPos, | ||
| 109 | Eigen::Vector3d e_satVel, | ||
| 110 | Clock::Corrections satClock) | ||
| 111 | 45298 | : _gnssObs(std::move(gnssObs)), | |
| 112 | 45298 | _obsIdx(obsIdx), | |
| 113 | 45298 | _e_satPos(std::move(e_satPos)), | |
| 114 | 45298 | _e_satVel(std::move(e_satVel)), | |
| 115 | 45298 | _satClock(satClock) {} | |
| 116 | |||
| 117 | /// Receiver observation of the signal | ||
| 118 | unordered_map<GnssObs::ObservationType, Observation> obs; | ||
| 119 | |||
| 120 | /// @brief Returns the observation data | ||
| 121 | 208114 | [[nodiscard]] const GnssObs::ObservationData& gnssObsData() const { return _gnssObs->data.at(_obsIdx); } | |
| 122 | /// @brief Satellite position in ECEF frame coordinates [m] | ||
| 123 | 172013 | [[nodiscard]] const Eigen::Vector3d& e_satPos() const { return _e_satPos; } | |
| 124 | /// @brief Satellite velocity in ECEF frame coordinates [m/s] | ||
| 125 | 104019 | [[nodiscard]] const Eigen::Vector3d& e_satVel() const { return _e_satVel; } | |
| 126 | /// @brief Position Line-of-sight unit vector in ECEF frame coordinates | ||
| 127 | /// @param[in] e_recPos Receiver position in e frame | ||
| 128 | 315583 | [[nodiscard]] Eigen::Vector3d e_pLOS(const Eigen::Vector3d& e_recPos) const | |
| 129 | { | ||
| 130 | 315583 | return e_calcLineOfSightUnitVector(e_recPos, _e_satPos); | |
| 131 | } | ||
| 132 | /// @brief Velocity Line-of-sight unit vector in ECEF frame coordinates | ||
| 133 | /// @param[in] e_recPos Receiver position in e frame | ||
| 134 | /// @param[in] e_recVel Receiver velocity in e frame | ||
| 135 | 31423 | [[nodiscard]] Eigen::Vector3d e_vLOS(const Eigen::Vector3d& e_recPos, | |
| 136 | const Eigen::Vector3d& e_recVel) const | ||
| 137 | { | ||
| 138 |
5/10✓ Branch 1 taken 31423 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31423 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31423 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31423 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 31423 times.
✗ Branch 14 not taken.
|
31423 | return (_e_satVel - e_recVel) / (_e_satPos - e_recPos).norm(); |
| 139 | } | ||
| 140 | /// @brief Satellite Elevation [rad] | ||
| 141 | /// @param[in] e_recPos Receiver position in e frame | ||
| 142 | /// @param[in] lla_recPos Receiver position in lla frame | ||
| 143 | 126783 | [[nodiscard]] double satElevation(const Eigen::Vector3d& e_recPos, | |
| 144 | const Eigen::Vector3d& lla_recPos) const | ||
| 145 | { | ||
| 146 |
3/6✓ Branch 1 taken 126783 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126783 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126783 times.
✗ Branch 8 not taken.
|
126783 | Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recPos(0), lla_recPos(1)) |
| 147 |
2/4✓ Branch 1 taken 126783 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126783 times.
✗ Branch 5 not taken.
|
253566 | * e_pLOS(e_recPos); |
| 148 |
1/2✓ Branch 1 taken 126783 times.
✗ Branch 2 not taken.
|
253566 | return calcSatElevation(n_lineOfSightUnitVector); |
| 149 | } | ||
| 150 | /// @brief Satellite Azimuth [rad] | ||
| 151 | /// @param[in] e_recPos Receiver position in e frame | ||
| 152 | /// @param[in] lla_recPos Receiver position in lla frame | ||
| 153 | 83280 | [[nodiscard]] double satAzimuth(const Eigen::Vector3d& e_recPos, | |
| 154 | const Eigen::Vector3d& lla_recPos) const | ||
| 155 | { | ||
| 156 |
3/6✓ Branch 1 taken 83280 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83280 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 83280 times.
✗ Branch 8 not taken.
|
83280 | Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recPos(0), lla_recPos(1)) |
| 157 |
2/4✓ Branch 1 taken 83280 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83280 times.
✗ Branch 5 not taken.
|
166560 | * e_pLOS(e_recPos); |
| 158 |
1/2✓ Branch 1 taken 83280 times.
✗ Branch 2 not taken.
|
166560 | return calcSatAzimuth(n_lineOfSightUnitVector); |
| 159 | } | ||
| 160 | /// @brief Satellite clock information | ||
| 161 | 108040 | [[nodiscard]] const Clock::Corrections& satClock() const { return _satClock; } | |
| 162 | |||
| 163 | /// @brief Terms used in the calculation | ||
| 164 | struct CalcTerms | ||
| 165 | { | ||
| 166 | double rho_r_s = 0.0; ///< Receiver-Satellite Range [m] | ||
| 167 | ZenithDelay tropoZenithDelay; ///< Troposphere delay | ||
| 168 | double dpsr_T_r_s = 0.0; ///< Estimated troposphere propagation error [m] | ||
| 169 | double dpsr_I_r_s = 0.0; ///< Estimated ionosphere propagation error [m] | ||
| 170 | double dpsr_ie_r_s = 0.0; ///< Sagnac correction [m] | ||
| 171 | }; | ||
| 172 | CalcTerms terms; ///< Sub terms used in the calculation | ||
| 173 | |||
| 174 | private: | ||
| 175 | std::shared_ptr<const GnssObs> _gnssObs = nullptr; ///< GNSS observation | ||
| 176 | size_t _obsIdx = 0; ///< Gnss observation data index | ||
| 177 | Eigen::Vector3d _e_satPos; ///< Satellite position in ECEF frame coordinates [m] (has to be calculated per signal because of TGD) | ||
| 178 | Eigen::Vector3d _e_satVel; ///< Satellite velocity in ECEF frame coordinates [m/s] | ||
| 179 | Clock::Corrections _satClock; ///< Satellite clock information | ||
| 180 | }; | ||
| 181 | |||
| 182 | /// @brief Constructor | ||
| 183 | /// @param[in] navData Satellite Navigation data | ||
| 184 | /// @param[in] freqNum Frequency number. Only used for GLONASS G1 and G2 | ||
| 185 | 41798 | SignalObservation(std::shared_ptr<SatNavData> navData, | |
| 186 | int8_t freqNum) | ||
| 187 | 41798 | : _navData(std::move(navData)), | |
| 188 | 41798 | _freqNum(freqNum) {} | |
| 189 | |||
| 190 | /// @brief Receiver specific data | ||
| 191 | /// | ||
| 192 | /// This is a shared_ptr so that e.g. the FGO can hold on to it over multiple epochs | ||
| 193 | std::unordered_map<size_t, std::shared_ptr<ReceiverSpecificData>> recvObs; | ||
| 194 | |||
| 195 | /// @brief Satellite Navigation data | ||
| 196 | 41798 | [[nodiscard]] const std::shared_ptr<const SatNavData>& navData() const { return _navData; } | |
| 197 | /// @brief Frequency number. Only used for GLONASS G1 and G2 | ||
| 198 | 124518 | [[nodiscard]] int8_t freqNum() const { return _freqNum; } | |
| 199 | |||
| 200 | private: | ||
| 201 | std::shared_ptr<const SatNavData> _navData = nullptr; ///< Satellite Navigation data | ||
| 202 | int8_t _freqNum = -128; ///< Frequency number. Only used for GLONASS G1 and G2 | ||
| 203 | }; | ||
| 204 | |||
| 205 | unordered_map<SatSigId, SignalObservation> signals; ///< Observations and calculated data for each signal | ||
| 206 | |||
| 207 | std::unordered_set<size_t> receivers; ///< Receivers included | ||
| 208 | std::set<SatelliteSystem> systems; ///< Satellite systems used | ||
| 209 | std::unordered_set<SatId> satellites; ///< Satellites used | ||
| 210 | std::array<size_t, GnssObs::ObservationType_COUNT> nObservables{}; ///< Number of observables | ||
| 211 | std::array<size_t, GnssObs::ObservationType_COUNT> nObservablesUniqueSatellite{}; ///< Number of observables (counted once for each satellite) | ||
| 212 | |||
| 213 | /// @brief Counts how many observations for the specified signal ant type exist | ||
| 214 | /// @param[in] satSigId Signal identifier | ||
| 215 | /// @param[in] obsType Observation type | ||
| 216 | size_t countObservations(const SatSigId& satSigId, const GnssObs::ObservationType& obsType) const; | ||
| 217 | |||
| 218 | /// @brief Calculates/Recalculates the number of observables | ||
| 219 | /// @param[in] nameId Name and Id of the calling node for logging purposes | ||
| 220 | void recalcObservableCounts(const std::string& nameId); | ||
| 221 | |||
| 222 | /// @brief Remove the signal from the observations | ||
| 223 | /// @param[in] satSigId Signal identifier | ||
| 224 | /// @param[in] nameId Name and Id of the calling node for logging purposes | ||
| 225 | /// @return True if something was removed | ||
| 226 | bool removeSignal(const SatSigId& satSigId, const std::string& nameId); | ||
| 227 | |||
| 228 | /// @brief Remove the signal from the observations | ||
| 229 | /// @param[in] satSigId Signal identifier | ||
| 230 | /// @param[in] obsType Observation type | ||
| 231 | /// @param[in] nameId Name and Id of the calling node for logging purposes | ||
| 232 | /// @return True if something was removed | ||
| 233 | bool removeSignal(const SatSigId& satSigId, const GnssObs::ObservationType& obsType, const std::string& nameId); | ||
| 234 | |||
| 235 | /// @brief Remove all signals of the satellite | ||
| 236 | /// @param[in] satId Satellite identifier | ||
| 237 | /// @param[in] nameId Name and Id of the calling node for logging purposes | ||
| 238 | /// @return True if something was removed | ||
| 239 | bool removeSatellite(const SatId& satId, const std::string& nameId); | ||
| 240 | |||
| 241 | /// @brief Remove all measurements for the provided code and observation type | ||
| 242 | /// @param[in] code Code | ||
| 243 | /// @param[in] obsType Observation type | ||
| 244 | /// @param[in] nameId Name and Id of the calling node for logging purposes | ||
| 245 | /// @return True if something was removed | ||
| 246 | template<typename ReceiverType> | ||
| 247 | bool removeMeasurementsFor(const Code& code, const GnssObs::ObservationType& obsType, [[maybe_unused]] const std::string& nameId) | ||
| 248 | { | ||
| 249 | LOG_DATA("{}: Searching observations to remove on [{}][{}]", nameId, code, obsType); | ||
| 250 | bool somethingRemoved = false; | ||
| 251 | for (auto& [satSigId, sigObs] : signals) | ||
| 252 | { | ||
| 253 | if (satSigId.code != code) { continue; } | ||
| 254 | |||
| 255 | for (size_t i = 0; i < sigObs.recvObs.size(); i++) | ||
| 256 | { | ||
| 257 | if (!sigObs.recvObs.contains(i)) { continue; } | ||
| 258 | auto& recvObs = sigObs.recvObs.at(i); | ||
| 259 | if (recvObs->obs.contains(obsType)) | ||
| 260 | { | ||
| 261 | recvObs->obs.erase(obsType); | ||
| 262 | LOG_DATA("{}: Erasing observation [{}][{}] of receiver '{}'", nameId, satSigId, obsType, to_string(static_cast<ReceiverType>(i))); | ||
| 263 | somethingRemoved = true; | ||
| 264 | } | ||
| 265 | } | ||
| 266 | } | ||
| 267 | if (somethingRemoved) { recalcObservableCounts(nameId); } | ||
| 268 | return somethingRemoved; | ||
| 269 | } | ||
| 270 | }; | ||
| 271 | |||
| 272 | } // namespace NAV | ||
| 273 |