INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Transformations/CoordinateFrames.hpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 115 146 78.8%
Functions: 28 34 82.4%
Branches: 175 414 42.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 CoordinateFrames.hpp
10 /// @brief Transformation collection
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2020-09-08
13 ///
14 /// Coordinate Frames:
15 /// - Inertial frame (i-frame)
16 /// O_i: Earth center
17 /// x_i: Direction to Vernal equinox
18 /// y_i: In equatorial plane, complementing to Right-Hand-System
19 /// z_i: Vertical on equatorial plane (North)
20 /// - Earth-centered-Earth-fixed frame (e-frame)
21 /// O_e: Earth center of mass
22 /// x_e: Direction to Greenwich meridian (longitude = 0掳)
23 /// y_e: In equatorial plane, complementing to Right-Hand-System
24 /// z_e: Vertical on equatorial plane (North)
25 /// - Local Navigation frame (n-frame)
26 /// O_n: Vehicle center of mass
27 /// x_n: "North"
28 /// y_n: Right-Hand-System ("East")
29 /// z_n: Earth center ("Down")
30 /// - Body frame (b-frame)
31 /// O_b: Vehicle center of mass
32 /// x_b: Roll-axis ("Forward")
33 /// y_b: Pitch-axis ("Right")
34 /// z_b: Yaw-axis ("Down")
35 /// - Platform frame (p-frame)
36 /// O_b: Center of IMU
37 /// x_b: X-Axis Accelerometer/Gyrometer
38 /// y_b: Y-Axis Accelerometer/Gyrometer
39 /// z_b: Z-Axis Accelerometer/Gyrometer
40
41 #pragma once
42
43 #include "util/Eigen.hpp"
44 #include "util/Logger.hpp"
45
46 #include "Navigation/Constants.hpp"
47 #include "Navigation/Ellipsoid/Ellipsoid.hpp"
48 #include "Navigation/Transformations/Units.hpp"
49
50 namespace NAV::trafo
51 {
52 namespace internal
53 {
54 /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates
55 /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m]
56 /// @param[in] a Semi-major axis of the reference ellipsoid
57 /// @param[in] e_squared Square of the first eccentricity of the ellipsoid
58 /// @return The ECEF coordinates in [m]
59 /// @note See C. Jekeli, 2001, Inertial Navigation Systems with Geodetic Applications. pp. 23
60 template<typename Derived>
61 18853466 Eigen::Vector3<typename Derived::Scalar> lla2ecef(const Eigen::MatrixBase<Derived>& lla_position, double a, double e_squared)
62 {
63
1/2
✓ Branch 1 taken 135788 times.
✗ Branch 2 not taken.
 18853466 const typename Derived::Scalar& latitude = lla_position(0); // 饾湙 Geodetic latitude
64
1/2
✓ Branch 1 taken 135788 times.
✗ Branch 2 not taken.
 18857475 const typename Derived::Scalar& longitude = lla_position(1); // 位 Geodetic longitude
65
1/2
✓ Branch 1 taken 135788 times.
✗ Branch 2 not taken.
 18862990 const typename Derived::Scalar& altitude = lla_position(2); // Altitude (Height above ground)
66
67 // Radius of curvature of the ellipsoid in the prime vertical plane,
68 // i.e., the plane containing the normal at P and perpendicular to the meridian (eq. 1.81)
69
1/2
✓ Branch 1 taken 135788 times.
✗ Branch 2 not taken.
 18864397 auto R_E = calcEarthRadius_E(latitude, a, e_squared);
70
71 // Jekeli, 2001 (eq. 1.80) (see Torge, 1991, for further details)
72 37184140 return { (R_E + altitude) * std::cos(latitude) * std::cos(longitude),
73 ✗ (R_E + altitude) * std::cos(latitude) * std::sin(longitude),
74
1/2
✓ Branch 1 taken 18693092 times.
✗ Branch 2 not taken.
 18861303 (R_E * (1 - e_squared) + altitude) * std::sin(latitude) };
75 }
76
77 /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude
78 /// @param[in] e_position Vector with coordinates in ECEF frame in [m]
79 /// @param[in] a Semi-major axis of the reference ellipsoid
80 /// @param[in] b Semi-minor axis of the reference ellipsoid
81 /// @param[in] e_squared Square of the first eccentricity of the ellipsoid
82 /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m]
83 /// @note See See S. Gleason (2009) - GNSS Applications and Methods: Software example 'Chapter6_GNSS_INS_1/wgsxyz2lla.m' (J.A. Farrel and M. Barth, 1999, GPS & Inertal Navigation. McGraw-Hill. pp. 29.)
84 template<typename Derived>
85 19900965 Eigen::Vector3<typename Derived::Scalar> ecef2lla(const Eigen::MatrixBase<Derived>& e_position,
86 double a, double b, double e_squared)
87 {
88
3/4
✓ Branch 2 taken 19819774 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 726 times.
✓ Branch 5 taken 19819048 times.
 19900965 if (e_position.isZero())
89 {
90 return {
91 ✗ typename Derived::Scalar(0.0),
92 ✗ typename Derived::Scalar(0.0),
93 726 typename Derived::Scalar(-a)
94
1/2
✓ Branch 1 taken 726 times.
✗ Branch 2 not taken.
 726 };
95 }
96
97
1/2
✓ Branch 1 taken 19816345 times.
✗ Branch 2 not taken.
 19894822 const auto& x = e_position(0);
98
1/2
✓ Branch 1 taken 19822508 times.
✗ Branch 2 not taken.
 19892119 const auto& y = e_position(1);
99
1/2
✓ Branch 1 taken 19823961 times.
✗ Branch 2 not taken.
 19898282 const auto& z = e_position(2);
100
101 // Calculate longitude
102
103 19899735 auto lon = std::atan2(y, x);
104
105 // Start computing intermediate variables needed to compute altitude
106
107
2/4
✓ Branch 1 taken 19822455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19812402 times.
✗ Branch 5 not taken.
 19899735 auto p = e_position.head(2).norm();
108 19888176 auto E = std::sqrt(a * a - b * b);
109 19888176 auto F = 54.0 * std::pow(b * z, 2.0);
110 19888176 auto G = p * p + (1.0 - e_squared) * z * z - e_squared * E * E;
111 19888176 auto c = e_squared * e_squared * F * p * p / std::pow(G, 3.0);
112 19888176 auto s = std::pow(1.0 + c + std::sqrt(c * c + 2.0 * c), 1.0 / 3.0);
113 19888176 auto P = (F / (3.0 * G * G)) / std::pow(s + (1.0 / s) + 1.0, 2.0);
114 19888176 auto Q = std::sqrt(1.0 + 2.0 * e_squared * e_squared * P);
115 19888176 auto k_1 = -P * e_squared * p / (1.0 + Q);
116 19888176 auto k_2 = 0.5 * a * a * (1.0 + 1.0 / Q);
117 19888176 auto k_3 = -P * (1.0 - e_squared) * z * z / (Q * (1.0 + Q));
118 19888176 auto k_4 = -0.5 * P * p * p;
119 19888176 auto r_0 = k_1 + std::sqrt(k_2 + k_3 + k_4);
120 19888176 auto k_5 = (p - e_squared * r_0);
121 19888176 auto U = std::sqrt(k_5 * k_5 + z * z);
122 19888176 auto V = std::sqrt(k_5 * k_5 + (1.0 - e_squared) * z * z);
123
124 19888176 auto alt = U * (1.0 - (b * b / (a * V)));
125
126 // Compute additional values required for computing latitude
127
128 19888176 auto z_0 = (b * b * z) / (a * V);
129 19888176 auto e_p = (a / b) * std::sqrt(e_squared);
130
131 19888176 auto lat = std::atan((z + z_0 * (e_p * e_p)) / p);
132
133
1/2
✓ Branch 1 taken 19819667 times.
✗ Branch 2 not taken.
 19888176 return { lat, lon, alt };
134 }
135 } // namespace internal
136
137 /// @brief Converts the quaternion to Euler rotation angles with rotation sequence ZYX
138 /// @param[in] q Quaternion to convert
139 /// @return [angleX, angleY, angleZ]^T vector in [rad]. The returned angles are in the ranges (-pi:pi] x (-pi/2:pi/2] x (-pi:pi]
140 template<typename Derived>
141 807712 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> quat2eulerZYX(const Eigen::QuaternionBase<Derived>& q)
142 {
143 // Given range [-pi:pi] x [-pi:pi] x [0:pi]
144
4/8
✓ Branch 1 taken 807707 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 807703 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 807699 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 807674 times.
✗ Branch 11 not taken.
 807712 Eigen::Vector3<typename Derived::Scalar> XYZ = q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
145
146 // Wanted range (-pi:pi] x (-pi/2:pi/2] x (-pi:pi]
147
6/6
✓ Branch 1 taken 603561 times.
✓ Branch 2 taken 204118 times.
✓ Branch 4 taken 213585 times.
✓ Branch 5 taken 389991 times.
✓ Branch 6 taken 417703 times.
✓ Branch 7 taken 389991 times.
 807674 if (XYZ.y() >= M_PI / 2.0 || XYZ.y() <= -M_PI / 2.0)
148 {
149
5/8
✓ Branch 1 taken 417705 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 221166 times.
✓ Branch 4 taken 196539 times.
✓ Branch 6 taken 221164 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 196540 times.
✗ Branch 10 not taken.
 417703 typename Derived::Scalar x = XYZ.x() > 0 ? XYZ.x() - M_PI : XYZ.x() + M_PI;
150
5/8
✓ Branch 1 taken 417703 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 204116 times.
✓ Branch 4 taken 213587 times.
✓ Branch 6 taken 204115 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 213591 times.
✗ Branch 10 not taken.
 417704 typename Derived::Scalar y = XYZ.y() >= M_PI / 2.0 ? -(XYZ.y() - M_PI) : -(XYZ.y() + M_PI);
151
1/2
✓ Branch 1 taken 417705 times.
✗ Branch 2 not taken.
 417706 typename Derived::Scalar z = XYZ.z() - M_PI;
152
153
1/2
✓ Branch 1 taken 417702 times.
✗ Branch 2 not taken.
 417705 XYZ = { x, y, z };
154 }
155
156 807684 return XYZ;
157 }
158
159 /// @brief Calculates the Jacobian to convert an attitude represented in Euler angels (roll, pitch, yaw) into a covariance for a quaternion
160 /// @param n_quat_b Quaternion for rotations from body to navigation frame
161 template<typename Derived>
162 101032 [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 4, 3> covRPY2quatJacobian(const Eigen::QuaternionBase<Derived>& n_quat_b)
163 {
164
1/2
✓ Branch 1 taken 101029 times.
✗ Branch 2 not taken.
 101032 auto RPY = trafo::quat2eulerZYX(n_quat_b);
165
166
2/4
✓ Branch 1 taken 101030 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 101027 times.
 101029 if (std::abs(RPY.y()) > typename Derived::Scalar(deg2rad(90 - 1e-9))) // Gimbal Lock
167 {
168 ✗ Eigen::Quaternion<typename Derived::Scalar> n_quat2_b = n_quat_b * Eigen::AngleAxis<typename Derived::Scalar>(typename Derived::Scalar(deg2rad(1e-8)), Eigen::Vector3<typename Derived::Scalar>::UnitY());
169 ✗ RPY = trafo::quat2eulerZYX(n_quat2_b);
170 }
171
3/6
✓ Branch 1 taken 101027 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101031 times.
✗ Branch 8 not taken.
 101027 auto ccc = std::cos(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
172
3/6
✓ Branch 1 taken 101028 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101029 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101030 times.
✗ Branch 8 not taken.
 101031 auto scc = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
173
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101031 times.
✗ Branch 8 not taken.
 101030 auto csc = std::cos(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
174
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101030 times.
✗ Branch 8 not taken.
 101031 auto ccs = std::cos(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
175
3/6
✓ Branch 1 taken 101030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101030 times.
✗ Branch 8 not taken.
 101030 auto ssc = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
176
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101029 times.
✗ Branch 8 not taken.
 101030 auto scs = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
177
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101031 times.
✗ Branch 8 not taken.
 101029 auto css = std::cos(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
178
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101029 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101031 auto sss = std::sin(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
179
180
1/2
✓ Branch 1 taken 101029 times.
✗ Branch 2 not taken.
 101032 Eigen::Matrix<typename Derived::Scalar, 4, 3> J;
181 // clang-format off
182
3/6
✓ Branch 1 taken 101025 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101029 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101029 times.
✗ Branch 8 not taken.
 101029 J << (ccc + sss) / 2.0, -(ccs + ssc) / 2.0, -(csc + scs) / 2.0,
183
3/6
✓ Branch 1 taken 101028 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101029 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101030 times.
✗ Branch 8 not taken.
 101029 (ccs - ssc) / 2.0, (ccc - sss) / 2.0, (scc - css) / 2.0,
184
3/6
✓ Branch 1 taken 101029 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101030 times.
✗ Branch 8 not taken.
 101030 -(csc + scs) / 2.0, -(css + scc) / 2.0, (ccc + sss) / 2.0,
185
3/6
✓ Branch 1 taken 101032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101030 (css - scc) / 2.0, (scs - csc) / 2.0, (ssc - ccs) / 2.0;
186 // clang-format on
187 202058 return J;
188 }
189
190 /// @brief Converts a covariance for an attitude represented in Euler angels (roll, pitch, yaw) into a covariance for a quaternion
191 /// @param covRPY Covariance for the Euler angles
192 /// @param n_quat_b Quaternion for rotations from body to navigation frame
193 /// @return Covariance for the quaternion
194 template<typename Derived, typename DerivedQ>
195 [[nodiscard]] Eigen::Matrix4<typename Derived::Scalar> covRPY2quat(const Eigen::MatrixBase<Derived>& covRPY, const Eigen::QuaternionBase<DerivedQ>& n_quat_b)
196 {
197 auto J = covRPY2quatJacobian(n_quat_b);
198 return J * covRPY * J.transpose();
199 }
200
201 /// @brief Calculates the Jacobian to convert an attitude represented in quaternions into a covariance in Euler angels (roll, pitch, yaw)
202 /// @param n_quat_b Quaternion for rotations from body to navigation frame
203 template<typename Derived>
204 ✗ [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 4> covQuat2RPYJacobian(const Eigen::QuaternionBase<Derived>& n_quat_b)
205 {
206 ✗ auto disc = n_quat_b.w() * n_quat_b.y() - n_quat_b.x() * n_quat_b.z();
207 ✗ auto a = 2.0 * std::pow(n_quat_b.y(), 2) + 2.0 * std::pow(n_quat_b.z(), 2) - 1.0;
208 ✗ auto b = 2.0 * std::pow(n_quat_b.x(), 2) + 2.0 * std::pow(n_quat_b.y(), 2) - 1.0;
209 ✗ auto c = n_quat_b.w() * n_quat_b.z() + n_quat_b.x() * n_quat_b.y();
210 ✗ auto d = n_quat_b.w() * n_quat_b.x() + n_quat_b.y() * n_quat_b.z();
211 ✗ auto e = std::pow(b, 2) + 4.0 * std::pow(d, 2);
212 ✗ auto f = std::sqrt(1.0 - 4.0 * std::pow(disc, 2));
213 ✗ auto g = std::pow(a, 2) + 4.0 * std::pow(c, 2);
214
215 ✗ Eigen::Matrix<typename Derived::Scalar, 3, 4> J;
216 J << // clang-format off
217 ✗ (2.0 * (-b * n_quat_b.w() + 4.0 * d * n_quat_b.x())) / e,
218 ✗ (2.0 * (-b * n_quat_b.z() + 4.0 * d * n_quat_b.y())) / e,
219 ✗ -(2.0 * b * n_quat_b.y()) / e,
220 ✗ -(2.0 * b * n_quat_b.x()) / e,
221
222 ✗ -2.0 * n_quat_b.z() / f,
223 ✗ 2.0 * n_quat_b.w() / f,
224 ✗ -2.0 * n_quat_b.x() / f,
225 ✗ 2.0 * n_quat_b.y() / f,
226
227 ✗ -(2.0 * a * n_quat_b.y()) / g,
228 ✗ (2.0 * (-a * n_quat_b.x() + 4.0 * c * n_quat_b.y())) / g,
229 ✗ (2.0 * (-a * n_quat_b.w() + 4.0 * c * n_quat_b.z())) / g,
230 ✗ -(2.0 * a * n_quat_b.z()) / g;
231 // clang-format on
232
233 ✗ return J;
234 }
235
236 /// @brief Converts a covariance for an attitude represented in quaternion form into a covariance for Euler angels (yaw, pitch, roll)
237 /// @param covQuat Covariance for the quaternion
238 /// @param n_quat_b Quaternion for rotations from body to navigation frame
239 /// @return Covariance for the Euler angels (roll, pitch, yaw)
240 template<typename Derived, typename DerivedQ>
241 ✗ [[nodiscard]] Eigen::Matrix3<typename Derived::Scalar> covQuat2RPY(const Eigen::MatrixBase<Derived>& covQuat, const Eigen::QuaternionBase<DerivedQ>& n_quat_b)
242 {
243 ✗ auto J = covQuat2RPYJacobian(n_quat_b);
244 ✗ return J * covQuat * J.transpose();
245 }
246
247 /// @brief Calculates the Jacobian to convert an attitude represented in quaternions from one frame into another. Multiply: cov_beta = J * cov_alpha * J^T
248 /// @param beta_quat_alpha Quaternion for rotations from frame alpha to frame beta. e.g. n_q_e if converting e_cov_b to n_cov_b
249 template<typename Derived>
250 126806 [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 4, 4> covQuat2QuatJacobian(const Eigen::QuaternionBase<Derived>& beta_quat_alpha)
251 {
252 126806 Eigen::Matrix<typename Derived::Scalar, 4, 4> J;
253 J << // clang-format off
254
8/16
✓ Branch 1 taken 126789 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126790 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126791 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126796 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126794 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126795 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126802 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126800 times.
✗ Branch 23 not taken.
 126803 beta_quat_alpha.w(), -beta_quat_alpha.z(), beta_quat_alpha.y(), beta_quat_alpha.x(),
255
8/16
✓ Branch 1 taken 126799 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126797 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126798 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126803 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126802 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126806 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126804 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126804 times.
✗ Branch 23 not taken.
 126800 beta_quat_alpha.z(), beta_quat_alpha.w(), beta_quat_alpha.x(), beta_quat_alpha.y(),
256
8/16
✓ Branch 1 taken 126806 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126806 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126804 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126803 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126803 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126804 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126806 times.
✗ Branch 23 not taken.
 126804 -beta_quat_alpha.y(), beta_quat_alpha.x(), beta_quat_alpha.w(), beta_quat_alpha.z(),
257
8/16
✓ Branch 1 taken 126802 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126804 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126806 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126806 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126804 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126806 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126806 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126806 times.
✗ Branch 23 not taken.
 126806 -beta_quat_alpha.x(), -beta_quat_alpha.y(), -beta_quat_alpha.z(), beta_quat_alpha.w();
258 // clang-format on
259
260 126806 return J;
261 }
262
263 /// @brief Calculates the Jacobian to convert an attitude represented in quaternions into the difference of the vector part of two quaternions
264 /// @param quat Quaternion for which the Jacobian is wanted
265 template<typename Derived>
266 [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 4> covQuatDiffJacobian(const Eigen::QuaternionBase<Derived>& quat)
267 {
268 Eigen::Matrix<typename Derived::Scalar, 3, 4> J;
269 J << // clang-format off
270 2.0 * quat.w(), -2.0 * quat.z(), 2.0 * quat.y(), -2.0 * quat.x(),
271 2.0 * quat.z(), 2.0 * quat.w(), -2.0 * quat.x(), -2.0 * quat.y(),
272 -2.0 * quat.y(), 2.0 * quat.x(), 2.0 * quat.w(), -2.0 * quat.z();
273 // clang-format on
274 return J;
275 }
276
277 /// @brief Quaternion for rotations from inertial to Earth-centered-Earth-fixed frame
278 /// @param[in] time Time (t - t0)
279 /// @param[in] omega_ie Angular velocity in [rad/s] of earth frame with regard to the inertial frame
280 /// @return The rotation Quaternion representation
281 template<typename Scalar>
282 3 [[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_i(Scalar time, Scalar omega_ie = InsConst::omega_ie)
283 {
284 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
285
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 Eigen::AngleAxis<Scalar> zAngle(-omega_ie * time, Eigen::Vector3<Scalar>::UnitZ());
286
287
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 return Eigen::Quaternion<Scalar>(zAngle).normalized();
288 }
289
290 /// @brief Quaternion for rotations from Earth-centered-Earth-fixed to inertial frame
291 /// @param[in] time Time (t - t0)
292 /// @param[in] omega_ie Angular velocity in [rad/s] of earth frame with regard to the inertial frame
293 /// @return The rotation Quaternion representation
294 template<typename Scalar>
295 1 [[nodiscard]] Eigen::Quaternion<Scalar> i_Quat_e(Scalar time, Scalar omega_ie = InsConst::omega_ie)
296 {
297
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 return e_Quat_i(time, omega_ie).conjugate();
298 }
299
300 /// @brief Quaternion for rotations from navigation to Earth-fixed frame
301 /// @param[in] latitude 饾湙 Geodetic latitude in [rad]
302 /// @param[in] longitude 位 Geodetic longitude in [rad]
303 /// @return The rotation Quaternion representation
304 template<typename Scalar>
305 6395096 [[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_n(const Scalar& latitude, const Scalar& longitude)
306 {
307 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
308 // Eigen uses here a different sign convention as the physical system.
309
2/4
✓ Branch 1 taken 6390251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6395023 times.
✗ Branch 5 not taken.
 6395096 Eigen::AngleAxis<Scalar> longitudeAngle(longitude, Eigen::Vector3<Scalar>::UnitZ());
310
2/4
✓ Branch 1 taken 6390465 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6398262 times.
✗ Branch 5 not taken.
 6395023 Eigen::AngleAxis<Scalar> latitudeAngle(-M_PI_2 - latitude, Eigen::Vector3<Scalar>::UnitY());
311
312
2/4
✓ Branch 1 taken 6398278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6392053 times.
✗ Branch 5 not taken.
 6398262 return (longitudeAngle * latitudeAngle).normalized();
313 }
314
315 /// @brief Quaternion for rotations from Earth-fixed to navigation frame
316 /// @param[in] latitude 饾湙 Geodetic latitude in [rad]
317 /// @param[in] longitude 位 Geodetic longitude in [rad]
318 /// @return The rotation Quaternion representation
319 template<typename Scalar>
320 5402568 [[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_e(const Scalar& latitude, const Scalar& longitude)
321 {
322
2/4
✓ Branch 1 taken 5402076 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5403233 times.
✗ Branch 5 not taken.
 5402568 return e_Quat_n(latitude, longitude).conjugate();
323 }
324
325 /// @brief Quaternion for rotations from navigation to body frame
326 /// @param[in] roll Roll angle in [rad]
327 /// @param[in] pitch Pitch angle in [rad]
328 /// @param[in] yaw Yaw angle in [rad]
329 /// @return The rotation Quaternion representation
330 template<typename Scalar>
331 3254700 [[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_n(const Scalar& roll, const Scalar& pitch, const Scalar& yaw)
332 {
333 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
334 // Eigen uses here a different sign convention as the physical system.
335
2/4
✓ Branch 1 taken 3245430 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3253162 times.
✗ Branch 5 not taken.
 3254700 Eigen::AngleAxis<Scalar> rollAngle(-roll, Eigen::Vector3<Scalar>::UnitX());
336
2/4
✓ Branch 1 taken 3248823 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3255151 times.
✗ Branch 5 not taken.
 3253162 Eigen::AngleAxis<Scalar> pitchAngle(-pitch, Eigen::Vector3<Scalar>::UnitY());
337
2/4
✓ Branch 1 taken 3251139 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3255632 times.
✗ Branch 5 not taken.
 3255151 Eigen::AngleAxis<Scalar> yawAngle(-yaw, Eigen::Vector3<Scalar>::UnitZ());
338
339
3/6
✓ Branch 1 taken 3257723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3255908 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3253113 times.
✗ Branch 8 not taken.
 3255632 return (rollAngle * pitchAngle * yawAngle).normalized();
340 }
341
342 /// @brief Quaternion for rotations from navigation to body frame
343 /// @param[in] rollPitchYaw Roll, Pitch, Yaw angle in [rad]
344 /// @return The rotation Quaternion representation
345 template<typename Derived>
346 [[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> b_Quat_n(const Eigen::MatrixBase<Derived>& rollPitchYaw)
347 {
348 return b_Quat_n(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2));
349 }
350
351 /// @brief Quaternion for rotations from body to navigation frame
352 /// @param[in] roll Roll angle in [rad]
353 /// @param[in] pitch Pitch angle in [rad]
354 /// @param[in] yaw Yaw angle in [rad]
355 /// @return The rotation Quaternion representation
356 template<typename Scalar>
357 420123 [[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_b(Scalar roll, Scalar pitch, Scalar yaw)
358 {
359
2/4
✓ Branch 1 taken 420123 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 420123 times.
✗ Branch 5 not taken.
 420123 return b_Quat_n(roll, pitch, yaw).conjugate();
360 }
361
362 /// @brief Quaternion for rotations from body to navigation frame
363 /// @param[in] rollPitchYaw Roll, Pitch, Yaw angle in [rad]
364 /// @return The rotation Quaternion representation
365 template<typename Derived>
366 24222 [[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> n_Quat_b(const Eigen::MatrixBase<Derived>& rollPitchYaw)
367 {
368 24222 return n_Quat_b(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2));
369 }
370
371 /// @brief Quaternion for rotations from platform to body frame
372 /// @param[in] mountingAngleX Mounting angle to x axis in [rad]. First rotation. (-pi:pi]
373 /// @param[in] mountingAngleY Mounting angle to y axis in [rad]. Second rotation. (-pi/2:pi/2]
374 /// @param[in] mountingAngleZ Mounting angle to z axis in [rad]. Third rotation. (-pi:pi]
375 /// @return The rotation Quaternion representation
376 template<typename Scalar>
377 1 [[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_p(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
378 {
379 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
380
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::AngleAxis<Scalar> xAngle(-mountingAngleX, Eigen::Vector3<Scalar>::UnitX());
381
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::AngleAxis<Scalar> yAngle(-mountingAngleY, Eigen::Vector3<Scalar>::UnitY());
382
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::AngleAxis<Scalar> zAngle(-mountingAngleZ, Eigen::Vector3<Scalar>::UnitZ());
383
384
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 return (xAngle * yAngle * zAngle).normalized();
385 }
386
387 /// @brief Quaternion for rotations from body to platform frame
388 /// @param[in] mountingAngleX Mounting angle to x axis in [rad]. First rotation. (-pi:pi]
389 /// @param[in] mountingAngleY Mounting angle to y axis in [rad]. Second rotation. (-pi/2:pi/2]
390 /// @param[in] mountingAngleZ Mounting angle to z axis in [rad]. Third rotation. (-pi:pi]
391 /// @return The rotation Quaternion representation
392 template<typename Scalar>
393 [[nodiscard]] Eigen::Quaternion<Scalar> p_Quat_b(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
394 {
395 return b_Quat_p(mountingAngleX, mountingAngleY, mountingAngleZ).conjugate();
396 }
397
398 /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84
399 /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m]
400 /// @return The ECEF coordinates in [m]
401 template<typename Derived>
402 18857259 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_WGS84(const Eigen::MatrixBase<Derived>& lla_position)
403 {
404 18857259 return internal::lla2ecef(lla_position, InsConst::WGS84::a, InsConst::WGS84::e_squared);
405 }
406
407 /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using GRS90
408 /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m]
409 /// @return The ECEF coordinates in [m]
410 template<typename Derived>
411 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_GRS80(const Eigen::MatrixBase<Derived>& lla_position)
412 {
413 return internal::lla2ecef(lla_position, InsConst::GRS80::a, InsConst::GRS80::e_squared);
414 }
415
416 /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84
417 /// @param[in] e_position Vector with coordinates in ECEF frame
418 /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m]
419 template<typename Derived>
420 19826277 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_WGS84(const Eigen::MatrixBase<Derived>& e_position)
421 {
422 return internal::ecef2lla(e_position,
423 InsConst::WGS84::a,
424 InsConst::WGS84::b,
425 19826277 InsConst::WGS84::e_squared);
426 }
427
428 /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using GRS90
429 /// @param[in] e_position Vector with coordinates in ECEF frame in [m]
430 /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m]
431 template<typename Derived>
432 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_GRS80(const Eigen::MatrixBase<Derived>& e_position)
433 {
434 return internal::ecef2lla(e_position,
435 InsConst::GRS80::a,
436 InsConst::GRS80::b,
437 InsConst::GRS80::e_squared);
438 }
439
440 /// @brief Converts ECEF coordinates into local NED coordinates
441 /// @param[in] e_position ECEF coordinates in [m] to convert
442 /// @param[in] lla_position_ref Reference [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] which represents the origin of the local frame
443 /// @return [x_N, x_E, x_D]^T Local NED coordinates in [m]
444 /// @note See G. Cai, B.M. Chen, Lee, T.H. Lee, 2011, Unmanned Rotorcraft Systems. Springer. pp. 32
445 /// @attention This function does not take the sphericity of the Earth into account
446 template<typename DerivedA, typename DerivedB>
447 10002 [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ecef2ned(const Eigen::MatrixBase<DerivedA>& e_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref)
448 {
449
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 const auto& latitude_ref = lla_position_ref(0); // 饾湙 Geodetic latitude
450
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 const auto& longitude_ref = lla_position_ref(1); // 位 Geodetic longitude
451
452
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 auto e_position_ref = lla2ecef_WGS84(lla_position_ref);
453
454
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 Eigen::Matrix3<typename DerivedA::Scalar> R_ne;
455 // clang-format off
456
3/6
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10002 times.
✗ Branch 8 not taken.
 10002 R_ne << -std::sin(latitude_ref) * std::cos(longitude_ref), -std::sin(latitude_ref) * std::sin(longitude_ref), std::cos(latitude_ref),
457
3/6
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10002 times.
✗ Branch 8 not taken.
 10002 -std::sin(longitude_ref) , std::cos(longitude_ref) , 0 ,
458
3/6
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10002 times.
✗ Branch 8 not taken.
 10002 -std::cos(latitude_ref) * std::cos(longitude_ref), -std::cos(latitude_ref) * std::sin(longitude_ref), -std::sin(latitude_ref);
459 // clang-format on
460
461
3/6
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10002 times.
✗ Branch 8 not taken.
 10002 return R_ne * (e_position - e_position_ref);
462 }
463
464 /// @brief Converts local NED coordinates into ECEF coordinates
465 /// @param[in] n_position NED coordinates in [m] to convert
466 /// @param[in] lla_position_ref Reference [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] which represents the origin of the local frame
467 /// @return ECEF position in [m]
468 /// @note See G. Cai, B.M. Chen, Lee, T.H. Lee, 2011, Unmanned Rotorcraft Systems. Springer. pp. 32
469 /// @attention This function does not take the sphericity of the Earth into account
470 template<typename DerivedA, typename DerivedB>
471 10052 [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ned2ecef(const Eigen::MatrixBase<DerivedA>& n_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref)
472 {
473
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 const auto& latitude_ref = lla_position_ref(0); // 饾湙 Geodetic latitude
474
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 const auto& longitude_ref = lla_position_ref(1); // 位 Geodetic longitude
475
476
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 auto e_position_ref = lla2ecef_WGS84(lla_position_ref);
477
478
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 Eigen::Matrix3<typename DerivedA::Scalar> R_en;
479 // clang-format off
480
3/6
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10052 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10052 times.
✗ Branch 8 not taken.
 10052 R_en << -std::sin(latitude_ref) * std::cos(longitude_ref), -std::sin(longitude_ref), -std::cos(latitude_ref) * std::cos(longitude_ref),
481
3/6
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10052 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10052 times.
✗ Branch 8 not taken.
 10052 -std::sin(latitude_ref) * std::sin(longitude_ref), std::cos(longitude_ref), -std::cos(latitude_ref) * std::sin(longitude_ref),
482
3/6
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10052 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10052 times.
✗ Branch 8 not taken.
 10052 std::cos(latitude_ref) , 0 , -std::sin(latitude_ref) ;
483 // clang-format on
484
485
3/6
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10052 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10052 times.
✗ Branch 8 not taken.
 10052 return e_position_ref + R_en * n_position;
486 }
487
488 /// @brief Converts PZ-90.11 coordinates to WGS84 coordinates
489 /// @param[in] pz90_pos Position in PZ-90.11 coordinates
490 /// @return Position in WGS84 coordinates
491 /// @note See \cite PZ-90.11 PZ-90.11 Reference Document Appendix 4, p.34f
492 template<typename Derived>
493 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> pz90toWGS84_pos(const Eigen::MatrixBase<Derived>& pz90_pos)
494 {
495 typename Derived::Scalar m = -0.008e-6;
496 auto omega_x = static_cast<typename Derived::Scalar>(-2.3_mas);
497 auto omega_y = static_cast<typename Derived::Scalar>(3.54_mas);
498 auto omega_z = static_cast<typename Derived::Scalar>(-4.21_mas);
499 Eigen::Vector3<typename Derived::Scalar> dX{ -0.013, 0.106, 0.022 };
500
501 Eigen::Matrix3<typename Derived::Scalar> T;
502 T << 1, -omega_z, omega_y,
503 omega_z, 1, -omega_x,
504 -omega_y, omega_x, 1;
505
506 return 1.0 / (1.0 + m) * T * (pz90_pos - dX);
507 }
508
509 /// @brief Converts PZ-90.11 vectors to WGS84 frame
510 /// @param[in] pz90 Vector in PZ-90.11 frame
511 /// @param[in] pz90_pos Position in PZ-90.11 frame (needed for calculation)
512 /// @return Vector in WGS84 frame
513 template<typename DerivedA, typename DerivedB>
514 [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> pz90toWGS84(const Eigen::MatrixBase<DerivedA>& pz90, const Eigen::MatrixBase<DerivedB>& pz90_pos)
515 {
516 return pz90toWGS84_pos(pz90_pos + pz90) - pz90toWGS84_pos(pz90_pos);
517 }
518
519 /// @brief Converts spherical Earth-centered-Earth-fixed coordinates into cartesian coordinates
520 /// @param[in] position_s Position in spherical coordinates to convert
521 /// @param[in] elevation Elevation in [rad]
522 /// @param[in] azimuth Azimuth in [rad]
523 /// @return The ECEF coordinates in [m]
524 template<typename Derived>
525 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> sph2ecef(const Eigen::MatrixBase<Derived>& position_s,
526 const typename Derived::Scalar& elevation,
527 const typename Derived::Scalar& azimuth)
528 {
529 Eigen::Matrix3<typename Derived::Scalar> R_se;
530 R_se << std::sin(elevation) * std::cos(azimuth), std::cos(elevation) * std::cos(azimuth), -std::sin(azimuth),
531 std::sin(elevation) * std::sin(azimuth), std::cos(elevation) * std::sin(azimuth), std::cos(azimuth),
532 std::cos(elevation), -std::sin(elevation), 0.0;
533
534 return R_se * position_s;
535 }
536
537 } // namespace NAV::trafo
538