| 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 Functions.hpp | ||
| 10 | /// @brief GNSS helper functions | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-05-19 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cmath> | ||
| 17 | #include <Eigen/Core> | ||
| 18 | #include "Navigation/Constants.hpp" | ||
| 19 | #include "Navigation/GNSS/Core/Frequency.hpp" | ||
| 20 | |||
| 21 | namespace NAV | ||
| 22 | { | ||
| 23 | |||
| 24 | /// @brief Calculates the line-of-sight unit vector from the user antenna to the satellite | ||
| 25 | /// @param[in] e_posAnt Position of the user antenna in ECEF frame coordinates | ||
| 26 | /// @param[in] e_posSat Position of the satellite in ECEF frame coordinates | ||
| 27 | /// @return The line-of-sight unit vector in ECEF frame coordinates | ||
| 28 | /// | ||
| 29 | /// @note See \cite Groves2013 Groves, ch. 8.5.3, eq. 8.41, p. 341 | ||
| 30 | template<typename DerivedA, typename DerivedB> | ||
| 31 | 315583 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcLineOfSightUnitVector(const Eigen::MatrixBase<DerivedA>& e_posAnt, | |
| 32 | const Eigen::MatrixBase<DerivedB>& e_posSat) | ||
| 33 | { | ||
| 34 |
5/10✓ Branch 1 taken 315583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 315583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 315583 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 315583 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 315583 times.
✗ Branch 14 not taken.
|
315583 | return (e_posSat - e_posAnt) / (e_posSat - e_posAnt).norm(); |
| 35 | } | ||
| 36 | |||
| 37 | /// @brief Calculates the elevation of the satellite from the antenna | ||
| 38 | /// @param[in] n_lineOfSightUnitVector Line-of-sight unit vector from the antenna to the satellite in NED frame coordinates | ||
| 39 | /// @return Elevation [rad] | ||
| 40 | /// | ||
| 41 | /// @note See \cite Groves2013 Groves, ch. 8.5.4, eq. 8.57, p. 344 | ||
| 42 | template<typename Derived> | ||
| 43 | 126783 | [[nodiscard]] typename Derived::Scalar calcSatElevation(const Eigen::MatrixBase<Derived>& n_lineOfSightUnitVector) | |
| 44 | { | ||
| 45 | 126783 | return -asin(n_lineOfSightUnitVector(2)); | |
| 46 | } | ||
| 47 | |||
| 48 | /// @brief Calculates the azimuth of the satellite from the antenna | ||
| 49 | /// @param[in] n_lineOfSightUnitVector Line-of-sight unit vector from the antenna to the satellite in NED frame coordinates | ||
| 50 | /// @return Azimuth [rad] | ||
| 51 | /// | ||
| 52 | /// @note See \cite Groves2013 Groves, ch. 8.5.4, eq. 8.57, p. 344 | ||
| 53 | template<typename Derived> | ||
| 54 | 83280 | [[nodiscard]] typename Derived::Scalar calcSatAzimuth(const Eigen::MatrixBase<Derived>& n_lineOfSightUnitVector) | |
| 55 | { | ||
| 56 | 83280 | return atan2(n_lineOfSightUnitVector(1), n_lineOfSightUnitVector(0)); | |
| 57 | } | ||
| 58 | |||
| 59 | /// @brief Calculates the Earth rotation/Sagnac correction | ||
| 60 | /// @param[in] e_posAnt Position of the user antenna in ECEF frame coordinates | ||
| 61 | /// @param[in] e_satPos Position of the satellite in ECEF frame coordinates | ||
| 62 | /// @return Earth rotation/Sagnac correction [m] | ||
| 63 | /// | ||
| 64 | /// @note See \cite SpringerHandbookGNSS2017 Springer Handbook ch. 19.1.1, eq. 19.7, p. 562 | ||
| 65 | template<typename DerivedA, typename DerivedB> | ||
| 66 | 41798 | [[nodiscard]] typename DerivedA::Scalar calcSagnacCorrection(const Eigen::MatrixBase<DerivedA>& e_posAnt, | |
| 67 | const Eigen::MatrixBase<DerivedB>& e_satPos) | ||
| 68 | { | ||
| 69 |
3/6✓ Branch 1 taken 41798 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 41798 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 41798 times.
✗ Branch 8 not taken.
|
41798 | return 1.0 / InsConst::C * (e_posAnt - e_satPos).dot(InsConst::e_omega_ie.cross(e_posAnt)); |
| 70 | } | ||
| 71 | |||
| 72 | /// @brief Calculates the Range-rate Earth rotation/Sagnac correction | ||
| 73 | /// @param[in] e_recvPos Position of the user antenna in ECEF frame coordinates | ||
| 74 | /// @param[in] e_satPos Position of the satellite in ECEF frame coordinates | ||
| 75 | /// @param[in] e_recvVel Velocity of the user antenna in ECEF frame coordinates | ||
| 76 | /// @param[in] e_satVel Velocity of the satellite in ECEF frame coordinates | ||
| 77 | /// @return Range-rate Earth rotation/Sagnac correction [m/s] | ||
| 78 | /// | ||
| 79 | /// @note See \cite Groves2013 Groves ch. 8.5.3, eq. 8.46, p. 342 | ||
| 80 | template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD> | ||
| 81 | 40922 | [[nodiscard]] typename DerivedA::Scalar calcSagnacRateCorrection(const Eigen::MatrixBase<DerivedA>& e_recvPos, | |
| 82 | const Eigen::MatrixBase<DerivedB>& e_satPos, | ||
| 83 | const Eigen::MatrixBase<DerivedC>& e_recvVel, | ||
| 84 | const Eigen::MatrixBase<DerivedD>& e_satVel) | ||
| 85 | { | ||
| 86 | return InsConst::omega_ie / InsConst::C | ||
| 87 | 40922 | * (e_satVel.y() * e_recvPos.x() + e_satPos.y() * e_recvVel.x() | |
| 88 | 40922 | - e_satVel.x() * e_recvPos.y() - e_satPos.x() * e_recvVel.y()); | |
| 89 | } | ||
| 90 | |||
| 91 | /// @brief Transforms a doppler-shift into a range-rate | ||
| 92 | /// @param[in] doppler The doppler-shift to transform [Hz] | ||
| 93 | /// @param[in] freq Frequency | ||
| 94 | /// @param[in] num Frequency number. Only used for GLONASS G1 and G2 | ||
| 95 | /// @return The corresponding range-rate [m/s] | ||
| 96 | /// | ||
| 97 | /// @note See \cite Groves2013 Groves, ch. 9.2.7, eq. 9.70, p. 388 | ||
| 98 | [[nodiscard]] double doppler2rangeRate(double doppler, Frequency freq, int8_t num); | ||
| 99 | |||
| 100 | /// @brief Transforms a range-rate into a doppler-shift | ||
| 101 | /// @param[in] rangeRate The range-rate to transform [m/s] | ||
| 102 | /// @param[in] freq Frequency | ||
| 103 | /// @param[in] num Frequency number. Only used for GLONASS G1 and G2 | ||
| 104 | /// @return The corresponding doppler-shift [Hz] | ||
| 105 | /// | ||
| 106 | /// @note See \cite Groves2013 Groves, ch. 9.2.7, eq. 9.70, p. 388 | ||
| 107 | [[nodiscard]] double rangeRate2doppler(double rangeRate, Frequency freq, int8_t num); | ||
| 108 | |||
| 109 | /// @brief Calculates the ration of the frequencies squared γ | ||
| 110 | /// | ||
| 111 | /// \anchor eq-GNSS-freqRatio \f{equation}{ \label{eq:eq-GNSS-freqRatio} | ||
| 112 | /// \gamma = (f_1 / f_2)^2 | ||
| 113 | /// \f} | ||
| 114 | /// | ||
| 115 | /// @param[in] f1 First frequency (usually L1) | ||
| 116 | /// @param[in] f2 Second frequency (usually L2) | ||
| 117 | /// @param[in] num1 First frequency number. Only used for GLONASS G1 and G2 | ||
| 118 | /// @param[in] num2 Second frequency number. Only used for GLONASS G1 and G2 | ||
| 119 | /// @return The ratio (f1/f2)^2 [-] | ||
| 120 | /// @note See \cite IS-GPS-200M IS-GPS-200 ch. 20.3.3.3.3.2 p.99 | ||
| 121 | [[nodiscard]] double ratioFreqSquared(Frequency f1, Frequency f2, int8_t num1, int8_t num2); | ||
| 122 | |||
| 123 | /// @brief Converts a GALILEO SISA (Signal in space accuracy) value to it's index | ||
| 124 | /// @param[in] val SISA value in [m] | ||
| 125 | /// @return The SISA index | ||
| 126 | /// | ||
| 127 | /// @note See \cite GAL-ICD-2.0 GAL ICD ch. 5.1.12, p.58 | ||
| 128 | [[nodiscard]] uint8_t galSisaVal2Idx(double val); | ||
| 129 | |||
| 130 | /// @brief Converts a GALILEO SISA (Signal in space accuracy) index to it's value | ||
| 131 | /// @param[in] idx The SISA index | ||
| 132 | /// @return SISA value in [m] | ||
| 133 | /// | ||
| 134 | /// @note See \cite GAL-ICD-2.0 GAL ICD ch. 5.1.12, p.58 | ||
| 135 | [[nodiscard]] double galSisaIdx2Val(uint8_t idx); | ||
| 136 | |||
| 137 | /// @brief Converts a GPS URA (user range accuracy) value to it's index | ||
| 138 | /// @param[in] val URA value in [m] | ||
| 139 | /// @return The URA index | ||
| 140 | /// | ||
| 141 | /// @note See \cite IS-GPS-200M GPS ICD ch. 20.3.3.3.1.3, p.92ff | ||
| 142 | [[nodiscard]] uint8_t gpsUraVal2Idx(double val); | ||
| 143 | |||
| 144 | /// @brief Converts a GPS URA (user range accuracy) index to it's value | ||
| 145 | /// @param[in] idx The URA index | ||
| 146 | /// @return URA value in [m] | ||
| 147 | /// | ||
| 148 | /// @note See \cite IS-GPS-200M GPS ICD ch. 20.3.3.3.1.3, p.92ff | ||
| 149 | [[nodiscard]] double gpsUraIdx2Val(uint8_t idx); | ||
| 150 | |||
| 151 | } // namespace NAV | ||
| 152 |