0.4.1
Loading...
Searching...
No Matches
Functions.hpp
Go to the documentation of this file.
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>
20
21namespace 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
30template<typename DerivedA, typename DerivedB>
31[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcLineOfSightUnitVector(const Eigen::MatrixBase<DerivedA>& e_posAnt,
32 const Eigen::MatrixBase<DerivedB>& e_posSat)
33{
34 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
42template<typename Derived>
43[[nodiscard]] typename Derived::Scalar calcSatElevation(const Eigen::MatrixBase<Derived>& n_lineOfSightUnitVector)
44{
45 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
53template<typename Derived>
54[[nodiscard]] typename Derived::Scalar calcSatAzimuth(const Eigen::MatrixBase<Derived>& n_lineOfSightUnitVector)
55{
56 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
65template<typename DerivedA, typename DerivedB>
66[[nodiscard]] typename DerivedA::Scalar calcSagnacCorrection(const Eigen::MatrixBase<DerivedA>& e_posAnt,
67 const Eigen::MatrixBase<DerivedB>& e_satPos)
68{
69 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
80template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
81[[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{
87 * (e_satVel.y() * e_recvPos.x() + e_satPos.y() * e_recvVel.x()
88 - 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
Holds all Constants.
Frequency definition for different satellite systems.
static constexpr double C
Speed of light [m/s].
Definition Constants.hpp:34
static constexpr double omega_ie
Nominal mean angular velocity of the Earth in [rad/s].
static const Eigen::Vector3< double > e_omega_ie
ω_ie_e = ω_ie_i Nominal mean angular velocity of the Earth in [rad/s], in earth coordinates
double galSisaIdx2Val(uint8_t idx)
Converts a GALILEO SISA (Signal in space accuracy) index to it's value.
Definition Functions.cpp:38
double ratioFreqSquared(Frequency f1, Frequency f2, int8_t num1, int8_t num2)
Calculates the ration of the frequencies squared γ
Definition Functions.cpp:24
double gpsUraIdx2Val(uint8_t idx)
Converts a GPS URA (user range accuracy) index to it's value.
Definition Functions.cpp:54
Derived::Scalar calcSatElevation(const Eigen::MatrixBase< Derived > &n_lineOfSightUnitVector)
Calculates the elevation of the satellite from the antenna.
Definition Functions.hpp:43
DerivedA::Scalar calcSagnacCorrection(const Eigen::MatrixBase< DerivedA > &e_posAnt, const Eigen::MatrixBase< DerivedB > &e_satPos)
Calculates the Earth rotation/Sagnac correction.
Definition Functions.hpp:66
uint8_t galSisaVal2Idx(double val)
Converts a GALILEO SISA (Signal in space accuracy) value to it's index.
Definition Functions.cpp:29
DerivedA::Scalar calcSagnacRateCorrection(const Eigen::MatrixBase< DerivedA > &e_recvPos, const Eigen::MatrixBase< DerivedB > &e_satPos, const Eigen::MatrixBase< DerivedC > &e_recvVel, const Eigen::MatrixBase< DerivedD > &e_satVel)
Calculates the Range-rate Earth rotation/Sagnac correction.
Definition Functions.hpp:81
Derived::Scalar calcSatAzimuth(const Eigen::MatrixBase< Derived > &n_lineOfSightUnitVector)
Calculates the azimuth of the satellite from the antenna.
Definition Functions.hpp:54
uint8_t gpsUraVal2Idx(double val)
Converts a GPS URA (user range accuracy) value to it's index.
Definition Functions.cpp:47
double doppler2rangeRate(double doppler, Frequency freq, int8_t num)
Transforms a doppler-shift into a range-rate.
Definition Functions.cpp:14
double rangeRate2doppler(double rangeRate, Frequency freq, int8_t num)
Transforms a range-rate into a doppler-shift.
Definition Functions.cpp:19
Eigen::Vector3< typename DerivedA::Scalar > e_calcLineOfSightUnitVector(const Eigen::MatrixBase< DerivedA > &e_posAnt, const Eigen::MatrixBase< DerivedB > &e_posSat)
Calculates the line-of-sight unit vector from the user antenna to the satellite.
Definition Functions.hpp:31