| 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 | 19137481 | Eigen::Vector3<typename Derived::Scalar> lla2ecef(const Eigen::MatrixBase<Derived>& lla_position, double a, double e_squared) | |
| 62 | { | ||
| 63 |
1/2✓ Branch 1 taken 205460 times.
✗ Branch 2 not taken.
|
19137481 | const auto& latitude = lla_position(0); // 饾湙 Geodetic latitude |
| 64 |
1/2✓ Branch 1 taken 205460 times.
✗ Branch 2 not taken.
|
19146257 | const auto& longitude = lla_position(1); // 位 Geodetic longitude |
| 65 |
1/2✓ Branch 1 taken 205460 times.
✗ Branch 2 not taken.
|
19151396 | const auto& 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 205460 times.
✗ Branch 2 not taken.
|
19153155 | auto R_E = calcEarthRadius_E(latitude, a, e_squared); |
| 70 | |||
| 71 | // Jekeli, 2001 (eq. 1.80) (see Torge, 1991, for further details) | ||
| 72 | 37483198 | 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 18910204 times.
✗ Branch 2 not taken.
|
19151480 | (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 | 19552740 | 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 19471885 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 564 times.
✓ Branch 5 taken 19471321 times.
|
19552740 | if (e_position.isZero()) |
| 89 | { | ||
| 90 | return { | ||
| 91 | ✗ | typename Derived::Scalar(0.0), | |
| 92 | ✗ | typename Derived::Scalar(0.0), | |
| 93 | 564 | typename Derived::Scalar(-a) | |
| 94 |
1/2✓ Branch 1 taken 564 times.
✗ Branch 2 not taken.
|
564 | }; |
| 95 | } | ||
| 96 | |||
| 97 |
1/2✓ Branch 1 taken 19472232 times.
✗ Branch 2 not taken.
|
19548391 | const auto& x = e_position(0); |
| 98 |
1/2✓ Branch 1 taken 19476269 times.
✗ Branch 2 not taken.
|
19549302 | const auto& y = e_position(1); |
| 99 |
1/2✓ Branch 1 taken 19477624 times.
✗ Branch 2 not taken.
|
19553339 | const auto& z = e_position(2); |
| 100 | |||
| 101 | // Calculate longitude | ||
| 102 | |||
| 103 | 19554694 | auto lon = atan2(y, x); | |
| 104 | |||
| 105 | // Start computing intermediate variables needed to compute altitude | ||
| 106 | |||
| 107 |
2/4✓ Branch 1 taken 19476146 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19461066 times.
✗ Branch 5 not taken.
|
19554694 | auto p = e_position.head(2).norm(); |
| 108 | 19538136 | auto E = sqrt(a * a - b * b); | |
| 109 | 19538136 | auto F = 54.0 * pow(b * z, 2.0); | |
| 110 | 19538136 | auto G = p * p + (1.0 - e_squared) * z * z - e_squared * E * E; | |
| 111 | 19538136 | auto c = e_squared * e_squared * F * p * p / pow(G, 3.0); | |
| 112 | 19538136 | auto s = pow(1.0 + c + sqrt(c * c + 2.0 * c), 1.0 / 3.0); | |
| 113 | 19538136 | auto P = (F / (3.0 * G * G)) / pow(s + (1.0 / s) + 1.0, 2.0); | |
| 114 | 19538136 | auto Q = sqrt(1.0 + 2.0 * e_squared * e_squared * P); | |
| 115 | 19538136 | auto k_1 = -P * e_squared * p / (1.0 + Q); | |
| 116 | 19538136 | auto k_2 = 0.5 * a * a * (1.0 + 1.0 / Q); | |
| 117 | 19538136 | auto k_3 = -P * (1.0 - e_squared) * z * z / (Q * (1.0 + Q)); | |
| 118 | 19538136 | auto k_4 = -0.5 * P * p * p; | |
| 119 | 19538136 | auto r_0 = k_1 + sqrt(k_2 + k_3 + k_4); | |
| 120 | 19538136 | auto k_5 = (p - e_squared * r_0); | |
| 121 | 19538136 | auto U = sqrt(k_5 * k_5 + z * z); | |
| 122 | 19538136 | auto V = sqrt(k_5 * k_5 + (1.0 - e_squared) * z * z); | |
| 123 | |||
| 124 | 19538136 | auto alt = U * (1.0 - (b * b / (a * V))); | |
| 125 | |||
| 126 | // Compute additional values required for computing latitude | ||
| 127 | |||
| 128 | 19538136 | auto z_0 = (b * b * z) / (a * V); | |
| 129 | 19538136 | auto e_p = (a / b) * sqrt(e_squared); | |
| 130 | |||
| 131 | 19538136 | auto lat = atan((z + z_0 * (e_p * e_p)) / p); | |
| 132 | |||
| 133 |
1/2✓ Branch 1 taken 19471195 times.
✗ Branch 2 not taken.
|
19538136 | 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 | 707469 | [[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 707466 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 707468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 707468 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 707465 times.
✗ Branch 11 not taken.
|
707469 | 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 541172 times.
✓ Branch 2 taken 166295 times.
✓ Branch 4 taken 195090 times.
✓ Branch 5 taken 346081 times.
✓ Branch 6 taken 361385 times.
✓ Branch 7 taken 346081 times.
|
707465 | if (XYZ.y() >= M_PI / 2.0 || XYZ.y() <= -M_PI / 2.0) |
| 148 | { | ||
| 149 |
5/8✓ Branch 1 taken 361385 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 199145 times.
✓ Branch 4 taken 162240 times.
✓ Branch 6 taken 199146 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 162241 times.
✗ Branch 10 not taken.
|
361385 | typename Derived::Scalar x = XYZ.x() > 0 ? XYZ.x() - M_PI : XYZ.x() + M_PI; |
| 150 |
5/8✓ Branch 1 taken 361386 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 166296 times.
✓ Branch 4 taken 195090 times.
✓ Branch 6 taken 166296 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 195091 times.
✗ Branch 10 not taken.
|
361387 | typename Derived::Scalar y = XYZ.y() >= M_PI / 2.0 ? -(XYZ.y() - M_PI) : -(XYZ.y() + M_PI); |
| 151 |
1/2✓ Branch 1 taken 361385 times.
✗ Branch 2 not taken.
|
361387 | typename Derived::Scalar z = XYZ.z() - M_PI; |
| 152 | |||
| 153 |
1/2✓ Branch 1 taken 361386 times.
✗ Branch 2 not taken.
|
361385 | XYZ = { x, y, z }; |
| 154 | } | ||
| 155 | |||
| 156 | 707466 | return XYZ; | |
| 157 | } | ||
| 158 | |||
| 159 | /// @brief Quaternion for rotations from inertial to Earth-centered-Earth-fixed frame | ||
| 160 | /// @param[in] time Time (t - t0) | ||
| 161 | /// @param[in] omega_ie Angular velocity in [rad/s] of earth frame with regard to the inertial frame | ||
| 162 | /// @return The rotation Quaternion representation | ||
| 163 | template<typename Scalar> | ||
| 164 | 3 | [[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_i(Scalar time, Scalar omega_ie = InsConst::omega_ie) | |
| 165 | { | ||
| 166 | // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized. | ||
| 167 |
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()); |
| 168 | |||
| 169 |
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(); |
| 170 | } | ||
| 171 | |||
| 172 | /// @brief Quaternion for rotations from Earth-centered-Earth-fixed to inertial frame | ||
| 173 | /// @param[in] time Time (t - t0) | ||
| 174 | /// @param[in] omega_ie Angular velocity in [rad/s] of earth frame with regard to the inertial frame | ||
| 175 | /// @return The rotation Quaternion representation | ||
| 176 | template<typename Scalar> | ||
| 177 | 1 | [[nodiscard]] Eigen::Quaternion<Scalar> i_Quat_e(Scalar time, Scalar omega_ie = InsConst::omega_ie) | |
| 178 | { | ||
| 179 |
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(); |
| 180 | } | ||
| 181 | |||
| 182 | /// @brief Quaternion for rotations from navigation to Earth-fixed frame | ||
| 183 | /// @param[in] latitude 饾湙 Geodetic latitude in [rad] | ||
| 184 | /// @param[in] longitude 位 Geodetic longitude in [rad] | ||
| 185 | /// @return The rotation Quaternion representation | ||
| 186 | template<typename Scalar> | ||
| 187 | 6141675 | [[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_n(Scalar latitude, Scalar longitude) | |
| 188 | { | ||
| 189 | // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized. | ||
| 190 | // Eigen uses here a different sign convention as the physical system. | ||
| 191 |
2/4✓ Branch 1 taken 6133184 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6140262 times.
✗ Branch 5 not taken.
|
6141675 | Eigen::AngleAxis<Scalar> longitudeAngle(longitude, Eigen::Vector3<Scalar>::UnitZ()); |
| 192 |
2/4✓ Branch 1 taken 6136237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6143046 times.
✗ Branch 5 not taken.
|
6140262 | Eigen::AngleAxis<Scalar> latitudeAngle(-M_PI_2 - latitude, Eigen::Vector3<Scalar>::UnitY()); |
| 193 | |||
| 194 |
2/4✓ Branch 1 taken 6141389 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6138992 times.
✗ Branch 5 not taken.
|
6143046 | return (longitudeAngle * latitudeAngle).normalized(); |
| 195 | } | ||
| 196 | |||
| 197 | /// @brief Quaternion for rotations from Earth-fixed to navigation frame | ||
| 198 | /// @param[in] latitude 饾湙 Geodetic latitude in [rad] | ||
| 199 | /// @param[in] longitude 位 Geodetic longitude in [rad] | ||
| 200 | /// @return The rotation Quaternion representation | ||
| 201 | template<typename Scalar> | ||
| 202 | 4703341 | [[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_e(Scalar latitude, Scalar longitude) | |
| 203 | { | ||
| 204 |
2/4✓ Branch 1 taken 4699088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4700237 times.
✗ Branch 5 not taken.
|
4703341 | return e_Quat_n(latitude, longitude).conjugate(); |
| 205 | } | ||
| 206 | |||
| 207 | /// @brief Quaternion for rotations from navigation to body frame | ||
| 208 | /// @param[in] roll Roll angle in [rad] | ||
| 209 | /// @param[in] pitch Pitch angle in [rad] | ||
| 210 | /// @param[in] yaw Yaw angle in [rad] | ||
| 211 | /// @return The rotation Quaternion representation | ||
| 212 | template<typename Scalar> | ||
| 213 | 3279940 | [[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_n(Scalar roll, Scalar pitch, Scalar yaw) | |
| 214 | { | ||
| 215 | // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized. | ||
| 216 | // Eigen uses here a different sign convention as the physical system. | ||
| 217 |
2/4✓ Branch 1 taken 3271362 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3279334 times.
✗ Branch 5 not taken.
|
3279940 | Eigen::AngleAxis<Scalar> rollAngle(-roll, Eigen::Vector3<Scalar>::UnitX()); |
| 218 |
2/4✓ Branch 1 taken 3274422 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3281118 times.
✗ Branch 5 not taken.
|
3279334 | Eigen::AngleAxis<Scalar> pitchAngle(-pitch, Eigen::Vector3<Scalar>::UnitY()); |
| 219 |
2/4✓ Branch 1 taken 3276556 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3281521 times.
✗ Branch 5 not taken.
|
3281118 | Eigen::AngleAxis<Scalar> yawAngle(-yaw, Eigen::Vector3<Scalar>::UnitZ()); |
| 220 | |||
| 221 |
3/6✓ Branch 1 taken 3284546 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3283958 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3279238 times.
✗ Branch 8 not taken.
|
3281521 | return (rollAngle * pitchAngle * yawAngle).normalized(); |
| 222 | } | ||
| 223 | |||
| 224 | /// @brief Quaternion for rotations from navigation to body frame | ||
| 225 | /// @param[in] rollPitchYaw Roll, Pitch, Yaw angle in [rad] | ||
| 226 | /// @return The rotation Quaternion representation | ||
| 227 | template<typename Derived> | ||
| 228 | [[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> b_Quat_n(const Eigen::MatrixBase<Derived>& rollPitchYaw) | ||
| 229 | { | ||
| 230 | return b_Quat_n(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2)); | ||
| 231 | } | ||
| 232 | |||
| 233 | /// @brief Quaternion for rotations from body to navigation frame | ||
| 234 | /// @param[in] roll Roll angle in [rad] | ||
| 235 | /// @param[in] pitch Pitch angle in [rad] | ||
| 236 | /// @param[in] yaw Yaw angle in [rad] | ||
| 237 | /// @return The rotation Quaternion representation | ||
| 238 | template<typename Scalar> | ||
| 239 | 415691 | [[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_b(Scalar roll, Scalar pitch, Scalar yaw) | |
| 240 | { | ||
| 241 |
2/4✓ Branch 1 taken 415691 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 415691 times.
✗ Branch 5 not taken.
|
415691 | return b_Quat_n(roll, pitch, yaw).conjugate(); |
| 242 | } | ||
| 243 | |||
| 244 | /// @brief Quaternion for rotations from body to navigation frame | ||
| 245 | /// @param[in] rollPitchYaw Roll, Pitch, Yaw angle in [rad] | ||
| 246 | /// @return The rotation Quaternion representation | ||
| 247 | template<typename Derived> | ||
| 248 | 25008 | [[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> n_Quat_b(const Eigen::MatrixBase<Derived>& rollPitchYaw) | |
| 249 | { | ||
| 250 | 25008 | return n_Quat_b(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2)); | |
| 251 | } | ||
| 252 | |||
| 253 | /// @brief Quaternion for rotations from platform to body frame | ||
| 254 | /// @param[in] mountingAngleX Mounting angle to x axis in [rad]. First rotation. (-pi:pi] | ||
| 255 | /// @param[in] mountingAngleY Mounting angle to y axis in [rad]. Second rotation. (-pi/2:pi/2] | ||
| 256 | /// @param[in] mountingAngleZ Mounting angle to z axis in [rad]. Third rotation. (-pi:pi] | ||
| 257 | /// @return The rotation Quaternion representation | ||
| 258 | template<typename Scalar> | ||
| 259 | 1 | [[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_p(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ) | |
| 260 | { | ||
| 261 | // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized. | ||
| 262 |
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()); |
| 263 |
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()); |
| 264 |
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()); |
| 265 | |||
| 266 |
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(); |
| 267 | } | ||
| 268 | |||
| 269 | /// @brief Quaternion for rotations from body to platform frame | ||
| 270 | /// @param[in] mountingAngleX Mounting angle to x axis in [rad]. First rotation. (-pi:pi] | ||
| 271 | /// @param[in] mountingAngleY Mounting angle to y axis in [rad]. Second rotation. (-pi/2:pi/2] | ||
| 272 | /// @param[in] mountingAngleZ Mounting angle to z axis in [rad]. Third rotation. (-pi:pi] | ||
| 273 | /// @return The rotation Quaternion representation | ||
| 274 | template<typename Scalar> | ||
| 275 | [[nodiscard]] Eigen::Quaternion<Scalar> p_Quat_b(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ) | ||
| 276 | { | ||
| 277 | return b_Quat_p(mountingAngleX, mountingAngleY, mountingAngleZ).conjugate(); | ||
| 278 | } | ||
| 279 | |||
| 280 | /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84 | ||
| 281 | /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] | ||
| 282 | /// @return The ECEF coordinates in [m] | ||
| 283 | template<typename Derived> | ||
| 284 | 19140497 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_WGS84(const Eigen::MatrixBase<Derived>& lla_position) | |
| 285 | { | ||
| 286 | 19140497 | return internal::lla2ecef(lla_position, InsConst::WGS84::a, InsConst::WGS84::e_squared); | |
| 287 | } | ||
| 288 | |||
| 289 | /// @brief Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using GRS90 | ||
| 290 | /// @param[in] lla_position [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] | ||
| 291 | /// @return The ECEF coordinates in [m] | ||
| 292 | template<typename Derived> | ||
| 293 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_GRS80(const Eigen::MatrixBase<Derived>& lla_position) | ||
| 294 | { | ||
| 295 | return internal::lla2ecef(lla_position, InsConst::GRS80::a, InsConst::GRS80::e_squared); | ||
| 296 | } | ||
| 297 | |||
| 298 | /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84 | ||
| 299 | /// @param[in] e_position Vector with coordinates in ECEF frame | ||
| 300 | /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m] | ||
| 301 | template<typename Derived> | ||
| 302 | 19477785 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_WGS84(const Eigen::MatrixBase<Derived>& e_position) | |
| 303 | { | ||
| 304 | return internal::ecef2lla(e_position, | ||
| 305 | InsConst::WGS84::a, | ||
| 306 | InsConst::WGS84::b, | ||
| 307 | 19477785 | InsConst::WGS84::e_squared); | |
| 308 | } | ||
| 309 | |||
| 310 | /// @brief Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using GRS90 | ||
| 311 | /// @param[in] e_position Vector with coordinates in ECEF frame in [m] | ||
| 312 | /// @return Vector containing [latitude 饾湙, longitude 位, altitude]^T in [rad, rad, m] | ||
| 313 | template<typename Derived> | ||
| 314 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_GRS80(const Eigen::MatrixBase<Derived>& e_position) | ||
| 315 | { | ||
| 316 | return internal::ecef2lla(e_position, | ||
| 317 | InsConst::GRS80::a, | ||
| 318 | InsConst::GRS80::b, | ||
| 319 | InsConst::GRS80::e_squared); | ||
| 320 | } | ||
| 321 | |||
| 322 | /// @brief Converts ECEF coordinates into local NED coordinates | ||
| 323 | /// @param[in] e_position ECEF coordinates in [m] to convert | ||
| 324 | /// @param[in] lla_position_ref Reference [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] which represents the origin of the local frame | ||
| 325 | /// @return [x_N, x_E, x_D]^T Local NED coordinates in [m] | ||
| 326 | /// @note See G. Cai, B.M. Chen, Lee, T.H. Lee, 2011, Unmanned Rotorcraft Systems. Springer. pp. 32 | ||
| 327 | /// @attention This function does not take the sphericity of the Earth into account | ||
| 328 | template<typename DerivedA, typename DerivedB> | ||
| 329 | 10002 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ecef2ned(const Eigen::MatrixBase<DerivedA>& e_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref) | |
| 330 | { | ||
| 331 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
10002 | const auto& latitude_ref = lla_position_ref(0); // 饾湙 Geodetic latitude |
| 332 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
10002 | const auto& longitude_ref = lla_position_ref(1); // 位 Geodetic longitude |
| 333 | |||
| 334 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
10002 | auto e_position_ref = lla2ecef_WGS84(lla_position_ref); |
| 335 | |||
| 336 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
10002 | Eigen::Matrix3<typename DerivedA::Scalar> R_ne; |
| 337 | // clang-format off | ||
| 338 |
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), |
| 339 |
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 , |
| 340 |
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); |
| 341 | // clang-format on | ||
| 342 | |||
| 343 |
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); |
| 344 | } | ||
| 345 | |||
| 346 | /// @brief Converts local NED coordinates into ECEF coordinates | ||
| 347 | /// @param[in] n_position NED coordinates in [m] to convert | ||
| 348 | /// @param[in] lla_position_ref Reference [饾湙 latitude, 位 longitude, altitude]^T in [rad, rad, m] which represents the origin of the local frame | ||
| 349 | /// @return ECEF position in [m] | ||
| 350 | /// @note See G. Cai, B.M. Chen, Lee, T.H. Lee, 2011, Unmanned Rotorcraft Systems. Springer. pp. 32 | ||
| 351 | /// @attention This function does not take the sphericity of the Earth into account | ||
| 352 | template<typename DerivedA, typename DerivedB> | ||
| 353 | 36865 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ned2ecef(const Eigen::MatrixBase<DerivedA>& n_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref) | |
| 354 | { | ||
| 355 |
1/2✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
|
36865 | const auto& latitude_ref = lla_position_ref(0); // 饾湙 Geodetic latitude |
| 356 |
1/2✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
|
36865 | const auto& longitude_ref = lla_position_ref(1); // 位 Geodetic longitude |
| 357 | |||
| 358 |
1/2✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
|
36865 | auto e_position_ref = lla2ecef_WGS84(lla_position_ref); |
| 359 | |||
| 360 |
1/2✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
|
36865 | Eigen::Matrix3<typename DerivedA::Scalar> R_en; |
| 361 | // clang-format off | ||
| 362 |
3/6✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36865 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36865 times.
✗ Branch 8 not taken.
|
36865 | R_en << -std::sin(latitude_ref) * std::cos(longitude_ref), -std::sin(longitude_ref), -std::cos(latitude_ref) * std::cos(longitude_ref), |
| 363 |
3/6✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36865 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36865 times.
✗ Branch 8 not taken.
|
36865 | -std::sin(latitude_ref) * std::sin(longitude_ref), std::cos(longitude_ref), -std::cos(latitude_ref) * std::sin(longitude_ref), |
| 364 |
3/6✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36865 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36865 times.
✗ Branch 8 not taken.
|
36865 | std::cos(latitude_ref) , 0 , -std::sin(latitude_ref) ; |
| 365 | // clang-format on | ||
| 366 | |||
| 367 |
3/6✓ Branch 1 taken 36865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36865 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36865 times.
✗ Branch 8 not taken.
|
36865 | return e_position_ref + R_en * n_position; |
| 368 | } | ||
| 369 | |||
| 370 | /// @brief Converts PZ-90.11 coordinates to WGS84 coordinates | ||
| 371 | /// @param[in] pz90_pos Position in PZ-90.11 coordinates | ||
| 372 | /// @return Position in WGS84 coordinates | ||
| 373 | /// @note See \cite PZ-90.11 PZ-90.11 Reference Document Appendix 4, p.34f | ||
| 374 | template<typename Derived> | ||
| 375 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> pz90toWGS84_pos(const Eigen::MatrixBase<Derived>& pz90_pos) | ||
| 376 | { | ||
| 377 | typename Derived::Scalar m = -0.008e-6; | ||
| 378 | auto omega_x = static_cast<typename Derived::Scalar>(-2.3_mas); | ||
| 379 | auto omega_y = static_cast<typename Derived::Scalar>(3.54_mas); | ||
| 380 | auto omega_z = static_cast<typename Derived::Scalar>(-4.21_mas); | ||
| 381 | Eigen::Vector3<typename Derived::Scalar> dX{ -0.013, 0.106, 0.022 }; | ||
| 382 | |||
| 383 | Eigen::Matrix3<typename Derived::Scalar> T; | ||
| 384 | T << 1, -omega_z, omega_y, | ||
| 385 | omega_z, 1, -omega_x, | ||
| 386 | -omega_y, omega_x, 1; | ||
| 387 | |||
| 388 | return 1.0 / (1.0 + m) * T * (pz90_pos - dX); | ||
| 389 | } | ||
| 390 | |||
| 391 | /// @brief Converts PZ-90.11 vectors to WGS84 frame | ||
| 392 | /// @param[in] pz90 Vector in PZ-90.11 frame | ||
| 393 | /// @param[in] pz90_pos Position in PZ-90.11 frame (needed for calculation) | ||
| 394 | /// @return Vector in WGS84 frame | ||
| 395 | template<typename DerivedA, typename DerivedB> | ||
| 396 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> pz90toWGS84(const Eigen::MatrixBase<DerivedA>& pz90, const Eigen::MatrixBase<DerivedB>& pz90_pos) | ||
| 397 | { | ||
| 398 | return pz90toWGS84_pos(pz90_pos + pz90) - pz90toWGS84_pos(pz90_pos); | ||
| 399 | } | ||
| 400 | |||
| 401 | /// @brief Converts spherical Earth-centered-Earth-fixed coordinates into cartesian coordinates | ||
| 402 | /// @param[in] position_s Position in spherical coordinates to convert | ||
| 403 | /// @param[in] elevation Elevation in [rad] | ||
| 404 | /// @param[in] azimuth Azimuth in [rad] | ||
| 405 | /// @return The ECEF coordinates in [m] | ||
| 406 | template<typename Derived> | ||
| 407 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> sph2ecef(const Eigen::MatrixBase<Derived>& position_s, | ||
| 408 | const typename Derived::Scalar& elevation, | ||
| 409 | const typename Derived::Scalar& azimuth) | ||
| 410 | { | ||
| 411 | Eigen::Matrix3<typename Derived::Scalar> R_se; | ||
| 412 | R_se << std::sin(elevation) * std::cos(azimuth), std::cos(elevation) * std::cos(azimuth), -std::sin(azimuth), | ||
| 413 | std::sin(elevation) * std::sin(azimuth), std::cos(elevation) * std::sin(azimuth), std::cos(azimuth), | ||
| 414 | std::cos(elevation), -std::sin(elevation), 0.0; | ||
| 415 | |||
| 416 | return R_se * position_s; | ||
| 417 | } | ||
| 418 | |||
| 419 | } // namespace NAV::trafo | ||
| 420 |