INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Transformations/Antenna.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 14 24 58.3%
Functions: 2 2 100.0%
Branches: 20 66 30.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 Antenna.hpp
10 /// @brief GNSS Antenna related transformations
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2024-08-01
13
14 #pragma once
15
16 #include <Eigen/Dense>
17 #include <memory>
18
19 #include "CoordinateFrames.hpp"
20 #include "NodeData/GNSS/GnssObs.hpp"
21 #include "Navigation/GNSS/Positioning/AntexReader.hpp"
22
23 namespace NAV::trafo
24 {
25
26 /// @brief Converts a marker position to the antenna reference point position
27 /// @param[in] e_posMarker Marker position in ECEF coordinates [m]
28 /// @param[in] gnssObs GNSS observation with antenna info
29 /// @param[in] hen_delta Additional height, east, north in [m]
30 /// @return Antenna Reference Point position in ECEF frame [m] (Marker + antennaDeltaNEU)
31 template<typename Derived>
32 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar>
33 160461 e_posMarker2ARP(const Eigen::MatrixBase<Derived>& e_posMarker,
34 const std::shared_ptr<const GnssObs>& gnssObs,
35 const Eigen::Vector3d& hen_delta = Eigen::Vector3d::Zero())
36 {
37
1/2
✓ Branch 1 taken 160462 times.
✗ Branch 2 not taken.
 160461 Eigen::Vector3<typename Derived::Scalar> e_posARP = e_posMarker;
38
39
1/2
✓ Branch 1 taken 160462 times.
✗ Branch 2 not taken.
 160462 auto lla_posMarker = trafo::ecef2lla_WGS84(e_posMarker);
40
3/6
✓ Branch 1 taken 160462 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160463 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160460 times.
✗ Branch 8 not taken.
 160462 auto e_Quat_n = trafo::e_Quat_n(lla_posMarker(0), lla_posMarker(1));
41
42
3/6
✓ Branch 1 taken 160460 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 160458 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 160458 times.
✗ Branch 8 not taken.
 160460 if (gnssObs && gnssObs->receiverInfo)
43 {
44 ✗ Eigen::Vector3<typename Derived::Scalar> n_antennaDelta(typename Derived::Scalar(0.0),
45 ✗ typename Derived::Scalar(0.0),
46
2/4
✓ Branch 4 taken 160460 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160460 times.
✗ Branch 8 not taken.
 160458 typename Derived::Scalar(-gnssObs->receiverInfo->get().antennaDeltaNEU.z()));
47
2/4
✓ Branch 1 taken 160459 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160461 times.
✗ Branch 5 not taken.
 160460 e_posARP += e_Quat_n * n_antennaDelta;
48 }
49 return e_posARP
50 + e_Quat_n
51 ✗ * Eigen::Vector3<typename Derived::Scalar>(typename Derived::Scalar(0.0),
52 ✗ typename Derived::Scalar(0.0),
53
5/10
✓ Branch 1 taken 160459 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160460 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160459 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 160460 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 160463 times.
✗ Branch 14 not taken.
 320924 typename Derived::Scalar(hen_delta(0)));
54 }
55
56 /// @brief Converts a marker position to the antenna reference point position
57 /// @param[in] lla_posMarker Marker position in LLA coordinates [rad, rad, m]
58 /// @param[in] gnssObs GNSS observation with antenna info
59 /// @param[in] hen_delta Additional height, east, north in [m]
60 /// @return Antenna Reference Point position in LLA coordinates [rad, rad, m] (Marker + antennaDeltaNEU)
61 template<typename Derived>
62 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar>
63 lla_posMarker2ARP(const Eigen::MatrixBase<Derived>& lla_posMarker,
64 const std::shared_ptr<const GnssObs>& gnssObs,
65 const Eigen::Vector3d& hen_delta = Eigen::Vector3d::Zero())
66 {
67 Eigen::Vector3<typename Derived::Scalar> lla_posARP = lla_posMarker;
68 auto e_Quat_n = trafo::e_Quat_n(lla_posMarker(0), lla_posMarker(1));
69 if (gnssObs && gnssObs->receiverInfo)
70 {
71 lla_posARP.z() += gnssObs->receiverInfo->get().antennaDeltaNEU.z();
72 }
73 return lla_posARP
74 + e_Quat_n
75 * Eigen::Vector3<typename Derived::Scalar>(typename Derived::Scalar(0.0),
76 typename Derived::Scalar(0.0),
77 typename Derived::Scalar(hen_delta(0)));
78 }
79
80 /// @brief Converts a antenna reference point position position to the antenna phase center position
81 /// @param[in] e_posARP Antenna reference point position in ECEF coordinates [m]
82 /// @param[in] gnssObs GNSS observation with antenna info
83 /// @param[in] freq Frequency of the observation
84 /// @param[in] antennaType Antenna type
85 /// @param[in] nameId NameId of the calling node for Log output
86 /// @return Antenna phase center position in ECEF frame [m] (ARP + antenna phase center)
87 template<typename Derived>
88 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar>
89 34903 e_posARP2APC(const Eigen::MatrixBase<Derived>& e_posARP,
90 const std::shared_ptr<const GnssObs>& gnssObs,
91 Frequency freq,
92 const std::string& antennaType,
93 const std::string& nameId)
94 {
95 34903 Eigen::Vector3<typename Derived::Scalar> e_posAPC = e_posARP;
96
1/2
✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
 34903 if (gnssObs)
97 {
98
2/4
✓ Branch 2 taken 34903 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 34903 times.
 69806 if (auto neu_antennaPhaseCenterOffset = AntexReader::Get().getAntennaPhaseCenterOffsetToARP(antennaType,
99 Frequency_(freq),
100 34903 gnssObs->insTime,
101 nameId))
102 {
103 ✗ Eigen::Vector3<typename Derived::Scalar> n_antennaPhaseCenterOffset(typename Derived::Scalar(0.0),
104 ✗ typename Derived::Scalar(0.0),
105 ✗ typename Derived::Scalar(-neu_antennaPhaseCenterOffset->z()));
106 ✗ auto lla_posARP = trafo::ecef2lla_WGS84(e_posARP);
107 ✗ if (!std::isnan(lla_posARP(0)) && !std::isnan(lla_posARP(1)))
108 {
109 ✗ e_posAPC += trafo::e_Quat_n(lla_posARP(0), lla_posARP(1)) * n_antennaPhaseCenterOffset;
110 }
111 }
112 }
113 34903 return e_posAPC;
114 }
115
116 /// @brief Converts a antenna reference point position to the antenna phase center position
117 /// @param[in] lla_posARP Antenna reference point position in LLA coordinates [rad, rad, m]
118 /// @param[in] gnssObs GNSS observation with antenna info
119 /// @param[in] freq Frequency of the observation
120 /// @param[in] antennaType Antenna type
121 /// @param[in] nameId NameId of the calling node for Log output
122 /// @return Antenna phase center position in LLA coordinates [rad, rad, m] (ARP + antenna phase center)
123 template<typename Derived>
124 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar>
125 lla_posARP2APC(const Eigen::MatrixBase<Derived>& lla_posARP,
126 const std::shared_ptr<const GnssObs>& gnssObs,
127 Frequency freq,
128 const std::string& antennaType,
129 const std::string& nameId)
130 {
131 Eigen::Vector3<typename Derived::Scalar> lla_posAPC = lla_posARP;
132 if (gnssObs)
133 {
134 if (auto neu_antennaPhaseCenterOffset =
135 AntexReader::Get().getAntennaPhaseCenterOffsetToARP(antennaType,
136 Frequency_(freq),
137 gnssObs->insTime,
138 nameId))
139 {
140 lla_posAPC.z() += neu_antennaPhaseCenterOffset->z();
141 }
142 }
143 return lla_posAPC;
144 }
145
146 } // namespace NAV::trafo
147