INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Transformations/CoordinateFrames.hpp
Date: 2025-11-25 23:34:18
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 18848250 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.
 18848250 const typename Derived::Scalar& latitude = lla_position(0); // 饾湙 Geodetic latitude
64
1/2
✓ Branch 1 taken 135788 times.
✗ Branch 2 not taken.
 18857400 const typename Derived::Scalar& longitude = lla_position(1); // 位 Geodetic longitude
65
1/2
✓ Branch 1 taken 135788 times.
✗ Branch 2 not taken.
 18862609 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.
 18863992 auto R_E = calcEarthRadius_E(latitude, a, e_squared);
70
71 // Jekeli, 2001 (eq. 1.80) (see Torge, 1991, for further details)
72 37182818 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 18690984 times.
✗ Branch 2 not taken.
 18862089 (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 19902025 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 19816461 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 726 times.
✓ Branch 5 taken 19815735 times.
 19902025 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 19817014 times.
✗ Branch 2 not taken.
 19891509 const auto& x = e_position(0);
98
1/2
✓ Branch 1 taken 19822653 times.
✗ Branch 2 not taken.
 19892788 const auto& y = e_position(1);
99
1/2
✓ Branch 1 taken 19823163 times.
✗ Branch 2 not taken.
 19898427 const auto& z = e_position(2);
100
101 // Calculate longitude
102
103 19898937 auto lon = std::atan2(y, x);
104
105 // Start computing intermediate variables needed to compute altitude
106
107
2/4
✓ Branch 1 taken 19822188 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19806672 times.
✗ Branch 5 not taken.
 19898937 auto p = e_position.head(2).norm();
108 19882446 auto E = std::sqrt(a * a - b * b);
109 19882446 auto F = 54.0 * std::pow(b * z, 2.0);
110 19882446 auto G = p * p + (1.0 - e_squared) * z * z - e_squared * E * E;
111 19882446 auto c = e_squared * e_squared * F * p * p / std::pow(G, 3.0);
112 19882446 auto s = std::pow(1.0 + c + std::sqrt(c * c + 2.0 * c), 1.0 / 3.0);
113 19882446 auto P = (F / (3.0 * G * G)) / std::pow(s + (1.0 / s) + 1.0, 2.0);
114 19882446 auto Q = std::sqrt(1.0 + 2.0 * e_squared * e_squared * P);
115 19882446 auto k_1 = -P * e_squared * p / (1.0 + Q);
116 19882446 auto k_2 = 0.5 * a * a * (1.0 + 1.0 / Q);
117 19882446 auto k_3 = -P * (1.0 - e_squared) * z * z / (Q * (1.0 + Q));
118 19882446 auto k_4 = -0.5 * P * p * p;
119 19882446 auto r_0 = k_1 + std::sqrt(k_2 + k_3 + k_4);
120 19882446 auto k_5 = (p - e_squared * r_0);
121 19882446 auto U = std::sqrt(k_5 * k_5 + z * z);
122 19882446 auto V = std::sqrt(k_5 * k_5 + (1.0 - e_squared) * z * z);
123
124 19882446 auto alt = U * (1.0 - (b * b / (a * V)));
125
126 // Compute additional values required for computing latitude
127
128 19882446 auto z_0 = (b * b * z) / (a * V);
129 19882446 auto e_p = (a / b) * std::sqrt(e_squared);
130
131 19882446 auto lat = std::atan((z + z_0 * (e_p * e_p)) / p);
132
133
1/2
✓ Branch 1 taken 19817496 times.
✗ Branch 2 not taken.
 19882446 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 807709 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 807703 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 807695 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 807670 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 603550 times.
✓ Branch 2 taken 204124 times.
✓ Branch 4 taken 213579 times.
✓ Branch 5 taken 389996 times.
✓ Branch 6 taken 417703 times.
✓ Branch 7 taken 389996 times.
 807670 if (XYZ.y() >= M_PI / 2.0 || XYZ.y() <= -M_PI / 2.0)
148 {
149
5/8
✓ Branch 1 taken 417704 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 221164 times.
✓ Branch 4 taken 196540 times.
✓ Branch 6 taken 221163 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 196542 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 417701 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 204115 times.
✓ Branch 4 taken 213586 times.
✓ Branch 6 taken 204117 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 213590 times.
✗ Branch 10 not taken.
 417705 typename Derived::Scalar y = XYZ.y() >= M_PI / 2.0 ? -(XYZ.y() - M_PI) : -(XYZ.y() + M_PI);
151
1/2
✓ Branch 1 taken 417704 times.
✗ Branch 2 not taken.
 417707 typename Derived::Scalar z = XYZ.z() - M_PI;
152
153
1/2
✓ Branch 1 taken 417703 times.
✗ Branch 2 not taken.
 417704 XYZ = { x, y, z };
154 }
155
156 807694 return XYZ;
157
158 // TODO: When Eigen 5.0.0 is usable (conflict with ceres 2.2.0)
159 // For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi].
160 // return q.toRotationMatrix().canonicalEulerAngles(2, 1, 0).reverse();
161 }
162
163 /// @brief Calculates the Jacobian to convert an attitude represented in Euler angels (roll, pitch, yaw) into a covariance for a quaternion
164 /// @param n_quat_b Quaternion for rotations from body to navigation frame
165 template<typename Derived>
166 101032 [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 4, 3> covRPY2quatJacobian(const Eigen::QuaternionBase<Derived>& n_quat_b)
167 {
168
1/2
✓ Branch 1 taken 101032 times.
✗ Branch 2 not taken.
 101032 auto RPY = trafo::quat2eulerZYX(n_quat_b);
169
170
2/4
✓ Branch 1 taken 101030 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 101029 times.
 101032 if (std::abs(RPY.y()) > typename Derived::Scalar(deg2rad(90 - 1e-9))) // Gimbal Lock
171 {
172 ✗ 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());
173 ✗ RPY = trafo::quat2eulerZYX(n_quat2_b);
174 }
175
3/6
✓ Branch 1 taken 101032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101029 auto ccc = std::cos(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
176
3/6
✓ Branch 1 taken 101030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101032 auto scc = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(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 101032 times.
✗ Branch 8 not taken.
 101032 auto csc = std::cos(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
178
3/6
✓ Branch 1 taken 101032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101031 times.
✗ Branch 8 not taken.
 101032 auto ccs = std::cos(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
179
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101031 auto ssc = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
180
3/6
✓ Branch 1 taken 101032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101031 times.
✗ Branch 8 not taken.
 101032 auto scs = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
181
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101030 times.
✗ Branch 8 not taken.
 101031 auto css = std::cos(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
182
3/6
✓ Branch 1 taken 101030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101031 times.
✗ Branch 8 not taken.
 101030 auto sss = std::sin(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
183
184
1/2
✓ Branch 1 taken 101029 times.
✗ Branch 2 not taken.
 101031 Eigen::Matrix<typename Derived::Scalar, 4, 3> J;
185 // clang-format off
186
3/6
✓ Branch 1 taken 101032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101030 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,
187
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 (ccs - ssc) / 2.0, (ccc - sss) / 2.0, (scc - css) / 2.0,
188
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101031 -(csc + scs) / 2.0, -(css + scc) / 2.0, (ccc + sss) / 2.0,
189
3/6
✓ Branch 1 taken 101031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 101032 times.
✗ Branch 8 not taken.
 101032 (css - scc) / 2.0, (scs - csc) / 2.0, (ssc - ccs) / 2.0;
190 // clang-format on
191 202062 return J;
192 }
193
194 /// @brief Converts a covariance for an attitude represented in Euler angels (roll, pitch, yaw) into a covariance for a quaternion
195 /// @param covRPY Covariance for the Euler angles
196 /// @param n_quat_b Quaternion for rotations from body to navigation frame
197 /// @return Covariance for the quaternion
198 template<typename Derived, typename DerivedQ>
199 [[nodiscard]] Eigen::Matrix4<typename Derived::Scalar> covRPY2quat(const Eigen::MatrixBase<Derived>& covRPY, const Eigen::QuaternionBase<DerivedQ>& n_quat_b)
200 {
201 auto J = covRPY2quatJacobian(n_quat_b);
202 return J * covRPY * J.transpose();
203 }
204
205 /// @brief Calculates the Jacobian to convert an attitude represented in quaternions into a covariance in Euler angels (roll, pitch, yaw)
206 /// @param n_quat_b Quaternion for rotations from body to navigation frame
207 template<typename Derived>
208 ✗ [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 4> covQuat2RPYJacobian(const Eigen::QuaternionBase<Derived>& n_quat_b)
209 {
210 ✗ auto disc = n_quat_b.w() * n_quat_b.y() - n_quat_b.x() * n_quat_b.z();
211 ✗ auto a = 2.0 * std::pow(n_quat_b.y(), 2) + 2.0 * std::pow(n_quat_b.z(), 2) - 1.0;
212 ✗ auto b = 2.0 * std::pow(n_quat_b.x(), 2) + 2.0 * std::pow(n_quat_b.y(), 2) - 1.0;
213 ✗ auto c = n_quat_b.w() * n_quat_b.z() + n_quat_b.x() * n_quat_b.y();
214 ✗ auto d = n_quat_b.w() * n_quat_b.x() + n_quat_b.y() * n_quat_b.z();
215 ✗ auto e = std::pow(b, 2) + 4.0 * std::pow(d, 2);
216 ✗ auto f = std::sqrt(1.0 - 4.0 * std::pow(disc, 2));
217 ✗ auto g = std::pow(a, 2) + 4.0 * std::pow(c, 2);
218
219 ✗ Eigen::Matrix<typename Derived::Scalar, 3, 4> J;
220 J << // clang-format off
221 ✗ (2.0 * (-b * n_quat_b.w() + 4.0 * d * n_quat_b.x())) / e,
222 ✗ (2.0 * (-b * n_quat_b.z() + 4.0 * d * n_quat_b.y())) / e,
223 ✗ -(2.0 * b * n_quat_b.y()) / e,
224 ✗ -(2.0 * b * n_quat_b.x()) / e,
225
226 ✗ -2.0 * n_quat_b.z() / f,
227 ✗ 2.0 * n_quat_b.w() / f,
228 ✗ -2.0 * n_quat_b.x() / f,
229 ✗ 2.0 * n_quat_b.y() / f,
230
231 ✗ -(2.0 * a * n_quat_b.y()) / g,
232 ✗ (2.0 * (-a * n_quat_b.x() + 4.0 * c * n_quat_b.y())) / g,
233 ✗ (2.0 * (-a * n_quat_b.w() + 4.0 * c * n_quat_b.z())) / g,
234 ✗ -(2.0 * a * n_quat_b.z()) / g;
235 // clang-format on
236
237 ✗ return J;
238 }
239
240 /// @brief Converts a covariance for an attitude represented in quaternion form into a covariance for Euler angels (yaw, pitch, roll)
241 /// @param covQuat Covariance for the quaternion
242 /// @param n_quat_b Quaternion for rotations from body to navigation frame
243 /// @return Covariance for the Euler angels (roll, pitch, yaw)
244 template<typename Derived, typename DerivedQ>
245 ✗ [[nodiscard]] Eigen::Matrix3<typename Derived::Scalar> covQuat2RPY(const Eigen::MatrixBase<Derived>& covQuat, const Eigen::QuaternionBase<DerivedQ>& n_quat_b)
246 {
247 ✗ auto J = covQuat2RPYJacobian(n_quat_b);
248 ✗ return J * covQuat * J.transpose();
249 }
250
251 /// @brief Calculates the Jacobian to convert an attitude represented in quaternions from one frame into another. Multiply: cov_beta = J * cov_alpha * J^T
252 /// @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
253 template<typename Derived>
254 126802 [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 4, 4> covQuat2QuatJacobian(const Eigen::QuaternionBase<Derived>& beta_quat_alpha)
255 {
256 126802 Eigen::Matrix<typename Derived::Scalar, 4, 4> J;
257 J << // clang-format off
258
8/16
✓ Branch 1 taken 126791 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126782 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126795 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126794 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126804 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126801 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126804 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126802 times.
✗ Branch 23 not taken.
 126788 beta_quat_alpha.w(), -beta_quat_alpha.z(), beta_quat_alpha.y(), beta_quat_alpha.x(),
259
8/16
✓ Branch 1 taken 126802 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126799 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126796 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126801 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126807 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126800 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126801 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126801 times.
✗ Branch 23 not taken.
 126802 beta_quat_alpha.z(), beta_quat_alpha.w(), beta_quat_alpha.x(), beta_quat_alpha.y(),
260
8/16
✓ Branch 1 taken 126800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126800 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 126799 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126803 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126803 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126804 times.
✗ Branch 23 not taken.
 126801 -beta_quat_alpha.y(), beta_quat_alpha.x(), beta_quat_alpha.w(), beta_quat_alpha.z(),
261
8/16
✓ Branch 1 taken 126804 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126804 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126802 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126802 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126806 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126805 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126805 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126808 times.
✗ Branch 23 not taken.
 126804 -beta_quat_alpha.x(), -beta_quat_alpha.y(), -beta_quat_alpha.z(), beta_quat_alpha.w();
262 // clang-format on
263
264 126804 return J;
265 }
266
267 /// @brief Calculates the Jacobian to convert an attitude represented in quaternions into the difference of the vector part of two quaternions
268 /// @param quat Quaternion for which the Jacobian is wanted
269 template<typename Derived>
270 [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 4> covQuatDiffJacobian(const Eigen::QuaternionBase<Derived>& quat)
271 {
272 Eigen::Matrix<typename Derived::Scalar, 3, 4> J;
273 J << // clang-format off
274 2.0 * quat.w(), -2.0 * quat.z(), 2.0 * quat.y(), -2.0 * quat.x(),
275 2.0 * quat.z(), 2.0 * quat.w(), -2.0 * quat.x(), -2.0 * quat.y(),
276 -2.0 * quat.y(), 2.0 * quat.x(), 2.0 * quat.w(), -2.0 * quat.z();
277 // clang-format on
278 return J;
279 }
280
281 /// @brief Quaternion for rotations from inertial to Earth-centered-Earth-fixed frame
282 /// @param[in] time Time (t - t0)
283 /// @param[in] omega_ie Angular velocity in [rad/s] of earth frame with regard to the inertial frame
284 /// @return The rotation Quaternion representation
285 template<typename Scalar>
286 3 [[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_i(Scalar time, Scalar omega_ie = InsConst::omega_ie)
287 {
288 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
289
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());
290
291
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();
292 }
293
294 /// @brief Quaternion for rotations from Earth-centered-Earth-fixed to inertial frame
295 /// @param[in] time Time (t - t0)
296 /// @param[in] omega_ie Angular velocity in [rad/s] of earth frame with regard to the inertial frame
297 /// @return The rotation Quaternion representation
298 template<typename Scalar>
299 1 [[nodiscard]] Eigen::Quaternion<Scalar> i_Quat_e(Scalar time, Scalar omega_ie = InsConst::omega_ie)
300 {
301
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();
302 }
303
304 /// @brief Quaternion for rotations from navigation to Earth-fixed frame
305 /// @param[in] latitude 饾湙 Geodetic latitude in [rad]
306 /// @param[in] longitude 位 Geodetic longitude in [rad]
307 /// @return The rotation Quaternion representation
308 template<typename Scalar>
309 6394258 [[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_n(const Scalar& latitude, const Scalar& longitude)
310 {
311 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
312 // Eigen uses here a different sign convention as the physical system.
313
2/4
✓ Branch 1 taken 6387264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6392893 times.
✗ Branch 5 not taken.
 6394258 Eigen::AngleAxis<Scalar> longitudeAngle(longitude, Eigen::Vector3<Scalar>::UnitZ());
314
2/4
✓ Branch 1 taken 6388676 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6396521 times.
✗ Branch 5 not taken.
 6392893 Eigen::AngleAxis<Scalar> latitudeAngle(-M_PI_2 - latitude, Eigen::Vector3<Scalar>::UnitY());
315
316
2/4
✓ Branch 1 taken 6397034 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6390923 times.
✗ Branch 5 not taken.
 6396521 return (longitudeAngle * latitudeAngle).normalized();
317 }
318
319 /// @brief Quaternion for rotations from Earth-fixed to navigation frame
320 /// @param[in] latitude 饾湙 Geodetic latitude in [rad]
321 /// @param[in] longitude 位 Geodetic longitude in [rad]
322 /// @return The rotation Quaternion representation
323 template<typename Scalar>
324 5402342 [[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_e(const Scalar& latitude, const Scalar& longitude)
325 {
326
2/4
✓ Branch 1 taken 5400528 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5402659 times.
✗ Branch 5 not taken.
 5402342 return e_Quat_n(latitude, longitude).conjugate();
327 }
328
329 /// @brief Quaternion for rotations from navigation to body frame
330 /// @param[in] roll Roll angle in [rad]
331 /// @param[in] pitch Pitch angle in [rad]
332 /// @param[in] yaw Yaw angle in [rad]
333 /// @return The rotation Quaternion representation
334 template<typename Scalar>
335 3250784 [[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_n(const Scalar& roll, const Scalar& pitch, const Scalar& yaw)
336 {
337 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
338 // Eigen uses here a different sign convention as the physical system.
339
2/4
✓ Branch 1 taken 3241950 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3249584 times.
✗ Branch 5 not taken.
 3250784 Eigen::AngleAxis<Scalar> rollAngle(-roll, Eigen::Vector3<Scalar>::UnitX());
340
2/4
✓ Branch 1 taken 3246777 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3253082 times.
✗ Branch 5 not taken.
 3249584 Eigen::AngleAxis<Scalar> pitchAngle(-pitch, Eigen::Vector3<Scalar>::UnitY());
341
2/4
✓ Branch 1 taken 3250377 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3253903 times.
✗ Branch 5 not taken.
 3253082 Eigen::AngleAxis<Scalar> yawAngle(-yaw, Eigen::Vector3<Scalar>::UnitZ());
342
343
3/6
✓ Branch 1 taken 3257685 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3252388 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3253596 times.
✗ Branch 8 not taken.
 3253903 return (rollAngle * pitchAngle * yawAngle).normalized();
344 }
345
346 /// @brief Quaternion for rotations from navigation to body frame
347 /// @param[in] rollPitchYaw Roll, Pitch, Yaw angle in [rad]
348 /// @return The rotation Quaternion representation
349 template<typename Derived>
350 [[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> b_Quat_n(const Eigen::MatrixBase<Derived>& rollPitchYaw)
351 {
352 return b_Quat_n(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2));
353 }
354
355 /// @brief Quaternion for rotations from body to navigation frame
356 /// @param[in] roll Roll angle in [rad]
357 /// @param[in] pitch Pitch angle in [rad]
358 /// @param[in] yaw Yaw angle in [rad]
359 /// @return The rotation Quaternion representation
360 template<typename Scalar>
361 420123 [[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_b(Scalar roll, Scalar pitch, Scalar yaw)
362 {
363
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();
364 }
365
366 /// @brief Quaternion for rotations from body to navigation frame
367 /// @param[in] rollPitchYaw Roll, Pitch, Yaw angle in [rad]
368 /// @return The rotation Quaternion representation
369 template<typename Derived>
370 24222 [[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> n_Quat_b(const Eigen::MatrixBase<Derived>& rollPitchYaw)
371 {
372 24222 return n_Quat_b(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2));
373 }
374
375 /// @brief Quaternion for rotations from platform to body frame
376 /// @param[in] mountingAngleX Mounting angle to x axis in [rad]. First rotation. (-pi:pi]
377 /// @param[in] mountingAngleY Mounting angle to y axis in [rad]. Second rotation. (-pi/2:pi/2]
378 /// @param[in] mountingAngleZ Mounting angle to z axis in [rad]. Third rotation. (-pi:pi]
379 /// @return The rotation Quaternion representation
380 template<typename Scalar>
381 1 [[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_p(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
382 {
383 // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
384
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());
385
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());
386
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());
387
388
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();
389 }
390
391 /// @brief Quaternion for rotations from body to platform frame
392 /// @param[in] mountingAngleX Mounting angle to x axis in [rad]. First rotation. (-pi:pi]
393 /// @param[in] mountingAngleY Mounting angle to y axis in [rad]. Second rotation. (-pi/2:pi/2]
394 /// @param[in] mountingAngleZ Mounting angle to z axis in [rad]. Third rotation. (-pi:pi]
395 /// @return The rotation Quaternion representation
396 template<typename Scalar>
397 [[nodiscard]] Eigen::Quaternion<Scalar> p_Quat_b(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
398 {
399 return b_Quat_p(mountingAngleX, mountingAngleY, mountingAngleZ).conjugate();
400 }
401
402 /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84
403 /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m]
404 /// @return The ECEF coordinates in [m]
405 template<typename Derived>
406 18850769 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_WGS84(const Eigen::MatrixBase<Derived>& lla_position)
407 {
408 18850769 return internal::lla2ecef(lla_position, InsConst::WGS84::a, InsConst::WGS84::e_squared);
409 }
410
411 /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using GRS90
412 /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m]
413 /// @return The ECEF coordinates in [m]
414 template<typename Derived>
415 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_GRS80(const Eigen::MatrixBase<Derived>& lla_position)
416 {
417 return internal::lla2ecef(lla_position, InsConst::GRS80::a, InsConst::GRS80::e_squared);
418 }
419
420 /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84
421 /// @param[in] e_position Vector with coordinates in ECEF frame
422 /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m]
423 template<typename Derived>
424 19826328 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_WGS84(const Eigen::MatrixBase<Derived>& e_position)
425 {
426 return internal::ecef2lla(e_position,
427 InsConst::WGS84::a,
428 InsConst::WGS84::b,
429 19826328 InsConst::WGS84::e_squared);
430 }
431
432 /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using GRS90
433 /// @param[in] e_position Vector with coordinates in ECEF frame in [m]
434 /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m]
435 template<typename Derived>
436 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_GRS80(const Eigen::MatrixBase<Derived>& e_position)
437 {
438 return internal::ecef2lla(e_position,
439 InsConst::GRS80::a,
440 InsConst::GRS80::b,
441 InsConst::GRS80::e_squared);
442 }
443
444 /// @brief Converts ECEF coordinates into local NED coordinates
445 /// @param[in] e_position ECEF coordinates in [m] to convert
446 /// @param[in] lla_position_ref Reference [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] which represents the origin of the local frame
447 /// @return [x_N, x_E, x_D]^T Local NED coordinates in [m]
448 /// @note See G. Cai, B.M. Chen, Lee, T.H. Lee, 2011, Unmanned Rotorcraft Systems. Springer. pp. 32
449 /// @attention This function does not take the sphericity of the Earth into account
450 template<typename DerivedA, typename DerivedB>
451 10002 [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ecef2ned(const Eigen::MatrixBase<DerivedA>& e_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref)
452 {
453
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 const auto& latitude_ref = lla_position_ref(0); // 饾湙 Geodetic latitude
454
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 const auto& longitude_ref = lla_position_ref(1); // 位 Geodetic longitude
455
456
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 auto e_position_ref = lla2ecef_WGS84(lla_position_ref);
457
458
1/2
✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
 10002 Eigen::Matrix3<typename DerivedA::Scalar> R_ne;
459 // clang-format off
460
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),
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 -std::sin(longitude_ref) , std::cos(longitude_ref) , 0 ,
462
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);
463 // clang-format on
464
465
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);
466 }
467
468 /// @brief Converts local NED coordinates into ECEF coordinates
469 /// @param[in] n_position NED coordinates in [m] to convert
470 /// @param[in] lla_position_ref Reference [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] which represents the origin of the local frame
471 /// @return ECEF position in [m]
472 /// @note See G. Cai, B.M. Chen, Lee, T.H. Lee, 2011, Unmanned Rotorcraft Systems. Springer. pp. 32
473 /// @attention This function does not take the sphericity of the Earth into account
474 template<typename DerivedA, typename DerivedB>
475 10052 [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ned2ecef(const Eigen::MatrixBase<DerivedA>& n_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref)
476 {
477
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 const auto& latitude_ref = lla_position_ref(0); // 饾湙 Geodetic latitude
478
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 const auto& longitude_ref = lla_position_ref(1); // 位 Geodetic longitude
479
480
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 auto e_position_ref = lla2ecef_WGS84(lla_position_ref);
481
482
1/2
✓ Branch 1 taken 10052 times.
✗ Branch 2 not taken.
 10052 Eigen::Matrix3<typename DerivedA::Scalar> R_en;
483 // clang-format off
484
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),
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 -std::sin(latitude_ref) * std::sin(longitude_ref), std::cos(longitude_ref), -std::cos(latitude_ref) * std::sin(longitude_ref),
486
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) ;
487 // clang-format on
488
489
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;
490 }
491
492 /// @brief Converts PZ-90.11 coordinates to WGS84 coordinates
493 /// @param[in] pz90_pos Position in PZ-90.11 coordinates
494 /// @return Position in WGS84 coordinates
495 /// @note See \cite PZ-90.11 PZ-90.11 Reference Document Appendix 4, p.34f
496 template<typename Derived>
497 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> pz90toWGS84_pos(const Eigen::MatrixBase<Derived>& pz90_pos)
498 {
499 typename Derived::Scalar m = -0.008e-6;
500 auto omega_x = static_cast<typename Derived::Scalar>(-2.3_mas);
501 auto omega_y = static_cast<typename Derived::Scalar>(3.54_mas);
502 auto omega_z = static_cast<typename Derived::Scalar>(-4.21_mas);
503 Eigen::Vector3<typename Derived::Scalar> dX{ -0.013, 0.106, 0.022 };
504
505 Eigen::Matrix3<typename Derived::Scalar> T;
506 T << 1, -omega_z, omega_y,
507 omega_z, 1, -omega_x,
508 -omega_y, omega_x, 1;
509
510 return 1.0 / (1.0 + m) * T * (pz90_pos - dX);
511 }
512
513 /// @brief Converts PZ-90.11 vectors to WGS84 frame
514 /// @param[in] pz90 Vector in PZ-90.11 frame
515 /// @param[in] pz90_pos Position in PZ-90.11 frame (needed for calculation)
516 /// @return Vector in WGS84 frame
517 template<typename DerivedA, typename DerivedB>
518 [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> pz90toWGS84(const Eigen::MatrixBase<DerivedA>& pz90, const Eigen::MatrixBase<DerivedB>& pz90_pos)
519 {
520 return pz90toWGS84_pos(pz90_pos + pz90) - pz90toWGS84_pos(pz90_pos);
521 }
522
523 /// @brief Converts spherical Earth-centered-Earth-fixed coordinates into cartesian coordinates
524 /// @param[in] position_s Position in spherical coordinates to convert
525 /// @param[in] elevation Elevation in [rad]
526 /// @param[in] azimuth Azimuth in [rad]
527 /// @return The ECEF coordinates in [m]
528 template<typename Derived>
529 [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> sph2ecef(const Eigen::MatrixBase<Derived>& position_s,
530 const typename Derived::Scalar& elevation,
531 const typename Derived::Scalar& azimuth)
532 {
533 Eigen::Matrix3<typename Derived::Scalar> R_se;
534 R_se << std::sin(elevation) * std::cos(azimuth), std::cos(elevation) * std::cos(azimuth), -std::sin(azimuth),
535 std::sin(elevation) * std::sin(azimuth), std::cos(elevation) * std::sin(azimuth), std::cos(azimuth),
536 std::cos(elevation), -std::sin(elevation), 0.0;
537
538 return R_se * position_s;
539 }
540
541 } // namespace NAV::trafo
542