CoordinateFrames.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.

 
#pragma once
 
#include "util/Eigen.hpp"
#include "util/Logger.hpp"
 
#include "Navigation/Constants.hpp"
#include "Navigation/Ellipsoid/Ellipsoid.hpp"
#include "Navigation/Transformations/Units.hpp"
 
namespace NAV::trafo
{
namespace internal
{
template<typename Derived>
Eigen::Vector3<typename Derived::Scalar> lla2ecef(const Eigen::MatrixBase<Derived>& lla_position, double a, double e_squared)
{
    const typename Derived::Scalar& latitude = lla_position(0);  // 𝜙 Geodetic latitude
    const typename Derived::Scalar& longitude = lla_position(1); // λ Geodetic longitude
    const typename Derived::Scalar& altitude = lla_position(2);  // Altitude (Height above ground)
 
    // Radius of curvature of the ellipsoid in the prime vertical plane,
    // i.e., the plane containing the normal at P and perpendicular to the meridian (eq. 1.81)
    auto R_E = calcEarthRadius_E(latitude, a, e_squared);
 
    // Jekeli, 2001 (eq. 1.80) (see  Torge, 1991, for further details)
    return { (R_E + altitude) * std::cos(latitude) * std::cos(longitude),
             (R_E + altitude) * std::cos(latitude) * std::sin(longitude),
             (R_E * (1 - e_squared) + altitude) * std::sin(latitude) };
}

template<typename Derived>
Eigen::Vector3<typename Derived::Scalar> ecef2lla(const Eigen::MatrixBase<Derived>& e_position,
                                                  double a, double b, double e_squared)
{
    if (e_position.isZero())
    {
        return {
            typename Derived::Scalar(0.0),
            typename Derived::Scalar(0.0),
            typename Derived::Scalar(-a)
        };
    }
 
    const auto& x = e_position(0);
    const auto& y = e_position(1);
    const auto& z = e_position(2);
 
    // Calculate longitude
 
    auto lon = std::atan2(y, x);
 
    // Start computing intermediate variables needed to compute altitude
 
    auto p = e_position.head(2).norm();
    auto E = std::sqrt(a * a - b * b);
    auto F = 54.0 * std::pow(b * z, 2.0);
    auto G = p * p + (1.0 - e_squared) * z * z - e_squared * E * E;
    auto c = e_squared * e_squared * F * p * p / std::pow(G, 3.0);
    auto s = std::pow(1.0 + c + std::sqrt(c * c + 2.0 * c), 1.0 / 3.0);
    auto P = (F / (3.0 * G * G)) / std::pow(s + (1.0 / s) + 1.0, 2.0);
    auto Q = std::sqrt(1.0 + 2.0 * e_squared * e_squared * P);
    auto k_1 = -P * e_squared * p / (1.0 + Q);
    auto k_2 = 0.5 * a * a * (1.0 + 1.0 / Q);
    auto k_3 = -P * (1.0 - e_squared) * z * z / (Q * (1.0 + Q));
    auto k_4 = -0.5 * P * p * p;
    auto r_0 = k_1 + std::sqrt(k_2 + k_3 + k_4);
    auto k_5 = (p - e_squared * r_0);
    auto U = std::sqrt(k_5 * k_5 + z * z);
    auto V = std::sqrt(k_5 * k_5 + (1.0 - e_squared) * z * z);
 
    auto alt = U * (1.0 - (b * b / (a * V)));
 
    // Compute additional values required for computing latitude
 
    auto z_0 = (b * b * z) / (a * V);
    auto e_p = (a / b) * std::sqrt(e_squared);
 
    auto lat = std::atan((z + z_0 * (e_p * e_p)) / p);
 
    return { lat, lon, alt };
}
} // namespace internal

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> quat2eulerZYX(const Eigen::QuaternionBase<Derived>& q)
{
    // Given range [-pi:pi] x [-pi:pi] x [0:pi]
    Eigen::Vector3<typename Derived::Scalar> XYZ = q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
 
    // Wanted range (-pi:pi] x (-pi/2:pi/2] x (-pi:pi]
    if (XYZ.y() >= M_PI / 2.0 || XYZ.y() <= -M_PI / 2.0)
    {
        typename Derived::Scalar x = XYZ.x() > 0 ? XYZ.x() - M_PI : XYZ.x() + M_PI;
        typename Derived::Scalar y = XYZ.y() >= M_PI / 2.0 ? -(XYZ.y() - M_PI) : -(XYZ.y() + M_PI);
        typename Derived::Scalar z = XYZ.z() - M_PI;
 
        XYZ = { x, y, z };
    }
 
    return XYZ;
}

template<typename Derived>
[[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 4, 3> covRPY2quatJacobian(const Eigen::QuaternionBase<Derived>& n_quat_b)
{
    auto RPY = trafo::quat2eulerZYX(n_quat_b);
 
    if (std::abs(RPY.y()) > typename Derived::Scalar(deg2rad(90 - 1e-9))) // Gimbal Lock
    {
        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());
        RPY = trafo::quat2eulerZYX(n_quat2_b);
    }
    auto ccc = std::cos(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
    auto scc = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
    auto csc = std::cos(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
    auto ccs = std::cos(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
    auto ssc = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::cos(RPY(2) / 2.0);
    auto scs = std::sin(RPY(0) / 2.0) * std::cos(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
    auto css = std::cos(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
    auto sss = std::sin(RPY(0) / 2.0) * std::sin(RPY(1) / 2.0) * std::sin(RPY(2) / 2.0);
 
    Eigen::Matrix<typename Derived::Scalar, 4, 3> J;
    // clang-format off
    J <<  (ccc + sss) / 2.0, -(ccs + ssc) / 2.0, -(csc + scs) / 2.0,
          (ccs - ssc) / 2.0,  (ccc - sss) / 2.0,  (scc - css) / 2.0,
         -(csc + scs) / 2.0, -(css + scc) / 2.0,  (ccc + sss) / 2.0,
          (css - scc) / 2.0,  (scs - csc) / 2.0,  (ssc - ccs) / 2.0;
    // clang-format on
    return J;
}

template<typename Derived, typename DerivedQ>
[[nodiscard]] Eigen::Matrix4<typename Derived::Scalar> covRPY2quat(const Eigen::MatrixBase<Derived>& covRPY, const Eigen::QuaternionBase<DerivedQ>& n_quat_b)
{
    auto J = covRPY2quatJacobian(n_quat_b);
    return J * covRPY * J.transpose();
}

template<typename Derived>
[[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 4> covQuat2RPYJacobian(const Eigen::QuaternionBase<Derived>& n_quat_b)
{
    auto disc = n_quat_b.w() * n_quat_b.y() - n_quat_b.x() * n_quat_b.z();
    auto a = 2.0 * std::pow(n_quat_b.y(), 2) + 2.0 * std::pow(n_quat_b.z(), 2) - 1.0;
    auto b = 2.0 * std::pow(n_quat_b.x(), 2) + 2.0 * std::pow(n_quat_b.y(), 2) - 1.0;
    auto c = n_quat_b.w() * n_quat_b.z() + n_quat_b.x() * n_quat_b.y();
    auto d = n_quat_b.w() * n_quat_b.x() + n_quat_b.y() * n_quat_b.z();
    auto e = std::pow(b, 2) + 4.0 * std::pow(d, 2);
    auto f = std::sqrt(1.0 - 4.0 * std::pow(disc, 2));
    auto g = std::pow(a, 2) + 4.0 * std::pow(c, 2);
 
    Eigen::Matrix<typename Derived::Scalar, 3, 4> J;
    J << // clang-format off
         (2.0 * (-b * n_quat_b.w() + 4.0 * d * n_quat_b.x())) / e,
         (2.0 * (-b * n_quat_b.z() + 4.0 * d * n_quat_b.y())) / e,
        -(2.0 * b * n_quat_b.y()) / e,
        -(2.0 * b * n_quat_b.x()) / e,
 
        -2.0 * n_quat_b.z() / f,
         2.0 * n_quat_b.w() / f,
        -2.0 * n_quat_b.x() / f,
         2.0 * n_quat_b.y() / f,
 
        -(2.0 * a * n_quat_b.y()) / g,
         (2.0 * (-a * n_quat_b.x() + 4.0 * c * n_quat_b.y())) / g,
         (2.0 * (-a * n_quat_b.w() + 4.0 * c * n_quat_b.z())) / g,
        -(2.0 * a * n_quat_b.z()) / g;
         // clang-format on
 
    return J;
}

template<typename Derived, typename DerivedQ>
[[nodiscard]] Eigen::Matrix3<typename Derived::Scalar> covQuat2RPY(const Eigen::MatrixBase<Derived>& covQuat, const Eigen::QuaternionBase<DerivedQ>& n_quat_b)
{
    auto J = covQuat2RPYJacobian(n_quat_b);
    return J * covQuat * J.transpose();
}

template<typename Derived>
[[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 4, 4> covQuat2QuatJacobian(const Eigen::QuaternionBase<Derived>& beta_quat_alpha)
{
    Eigen::Matrix<typename Derived::Scalar, 4, 4> J;
    J << // clang-format off
         beta_quat_alpha.w(), -beta_quat_alpha.z(),  beta_quat_alpha.y(), beta_quat_alpha.x(),
         beta_quat_alpha.z(),  beta_quat_alpha.w(),  beta_quat_alpha.x(), beta_quat_alpha.y(),
        -beta_quat_alpha.y(),  beta_quat_alpha.x(),  beta_quat_alpha.w(), beta_quat_alpha.z(),
        -beta_quat_alpha.x(), -beta_quat_alpha.y(), -beta_quat_alpha.z(), beta_quat_alpha.w();
         // clang-format on
 
    return J;
}

template<typename Derived>
[[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 4> covQuatDiffJacobian(const Eigen::QuaternionBase<Derived>& quat)
{
    Eigen::Matrix<typename Derived::Scalar, 3, 4> J;
    J << // clang-format off
    2.0 * quat.w(), -2.0 * quat.z(),  2.0 * quat.y(), -2.0 * quat.x(),
    2.0 * quat.z(),  2.0 * quat.w(), -2.0 * quat.x(), -2.0 * quat.y(),
   -2.0 * quat.y(),  2.0 * quat.x(),  2.0 * quat.w(), -2.0 * quat.z();
         // clang-format on
    return J;
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_i(Scalar time, Scalar omega_ie = InsConst::omega_ie)
{
    // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
    Eigen::AngleAxis<Scalar> zAngle(-omega_ie * time, Eigen::Vector3<Scalar>::UnitZ());
 
    return Eigen::Quaternion<Scalar>(zAngle).normalized();
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> i_Quat_e(Scalar time, Scalar omega_ie = InsConst::omega_ie)
{
    return e_Quat_i(time, omega_ie).conjugate();
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> e_Quat_n(const Scalar& latitude, const Scalar& longitude)
{
    // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
    // Eigen uses here a different sign convention as the physical system.
    Eigen::AngleAxis<Scalar> longitudeAngle(longitude, Eigen::Vector3<Scalar>::UnitZ());
    Eigen::AngleAxis<Scalar> latitudeAngle(-M_PI_2 - latitude, Eigen::Vector3<Scalar>::UnitY());
 
    return (longitudeAngle * latitudeAngle).normalized();
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_e(const Scalar& latitude, const Scalar& longitude)
{
    return e_Quat_n(latitude, longitude).conjugate();
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_n(const Scalar& roll, const Scalar& pitch, const Scalar& yaw)
{
    // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
    // Eigen uses here a different sign convention as the physical system.
    Eigen::AngleAxis<Scalar> rollAngle(-roll, Eigen::Vector3<Scalar>::UnitX());
    Eigen::AngleAxis<Scalar> pitchAngle(-pitch, Eigen::Vector3<Scalar>::UnitY());
    Eigen::AngleAxis<Scalar> yawAngle(-yaw, Eigen::Vector3<Scalar>::UnitZ());
 
    return (rollAngle * pitchAngle * yawAngle).normalized();
}

template<typename Derived>
[[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> b_Quat_n(const Eigen::MatrixBase<Derived>& rollPitchYaw)
{
    return b_Quat_n(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2));
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> n_Quat_b(Scalar roll, Scalar pitch, Scalar yaw)
{
    return b_Quat_n(roll, pitch, yaw).conjugate();
}

template<typename Derived>
[[nodiscard]] Eigen::Quaternion<typename Derived::Scalar> n_Quat_b(const Eigen::MatrixBase<Derived>& rollPitchYaw)
{
    return n_Quat_b(rollPitchYaw(0), rollPitchYaw(1), rollPitchYaw(2));
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> b_Quat_p(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
{
    // Initialize angle-axis rotation from an angle in radian and an axis which must be normalized.
    Eigen::AngleAxis<Scalar> xAngle(-mountingAngleX, Eigen::Vector3<Scalar>::UnitX());
    Eigen::AngleAxis<Scalar> yAngle(-mountingAngleY, Eigen::Vector3<Scalar>::UnitY());
    Eigen::AngleAxis<Scalar> zAngle(-mountingAngleZ, Eigen::Vector3<Scalar>::UnitZ());
 
    return (xAngle * yAngle * zAngle).normalized();
}

template<typename Scalar>
[[nodiscard]] Eigen::Quaternion<Scalar> p_Quat_b(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
{
    return b_Quat_p(mountingAngleX, mountingAngleY, mountingAngleZ).conjugate();
}

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_WGS84(const Eigen::MatrixBase<Derived>& lla_position)
{
    return internal::lla2ecef(lla_position, InsConst::WGS84::a, InsConst::WGS84::e_squared);
}

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> lla2ecef_GRS80(const Eigen::MatrixBase<Derived>& lla_position)
{
    return internal::lla2ecef(lla_position, InsConst::GRS80::a, InsConst::GRS80::e_squared);
}

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_WGS84(const Eigen::MatrixBase<Derived>& e_position)
{
    return internal::ecef2lla(e_position,
                              InsConst::WGS84::a,
                              InsConst::WGS84::b,
                              InsConst::WGS84::e_squared);
}

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> ecef2lla_GRS80(const Eigen::MatrixBase<Derived>& e_position)
{
    return internal::ecef2lla(e_position,
                              InsConst::GRS80::a,
                              InsConst::GRS80::b,
                              InsConst::GRS80::e_squared);
}

template<typename DerivedA, typename DerivedB>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ecef2ned(const Eigen::MatrixBase<DerivedA>& e_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref)
{
    const auto& latitude_ref = lla_position_ref(0);  // 𝜙 Geodetic latitude
    const auto& longitude_ref = lla_position_ref(1); // λ Geodetic longitude
 
    auto e_position_ref = lla2ecef_WGS84(lla_position_ref);
 
    Eigen::Matrix3<typename DerivedA::Scalar> R_ne;
    // clang-format off
    R_ne << -std::sin(latitude_ref) * std::cos(longitude_ref), -std::sin(latitude_ref) * std::sin(longitude_ref),  std::cos(latitude_ref),
                     -std::sin(longitude_ref)                ,              std::cos(longitude_ref)             ,            0           ,
            -std::cos(latitude_ref) * std::cos(longitude_ref), -std::cos(latitude_ref) * std::sin(longitude_ref), -std::sin(latitude_ref);
    // clang-format on
 
    return R_ne * (e_position - e_position_ref);
}

template<typename DerivedA, typename DerivedB>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> ned2ecef(const Eigen::MatrixBase<DerivedA>& n_position, const Eigen::MatrixBase<DerivedB>& lla_position_ref)
{
    const auto& latitude_ref = lla_position_ref(0);  // 𝜙 Geodetic latitude
    const auto& longitude_ref = lla_position_ref(1); // λ Geodetic longitude
 
    auto e_position_ref = lla2ecef_WGS84(lla_position_ref);
 
    Eigen::Matrix3<typename DerivedA::Scalar> R_en;
    // clang-format off
    R_en << -std::sin(latitude_ref) * std::cos(longitude_ref), -std::sin(longitude_ref), -std::cos(latitude_ref) * std::cos(longitude_ref),
            -std::sin(latitude_ref) * std::sin(longitude_ref),  std::cos(longitude_ref), -std::cos(latitude_ref) * std::sin(longitude_ref),
                         std::cos(latitude_ref)              ,             0           ,                -std::sin(latitude_ref)           ;
    // clang-format on
 
    return e_position_ref + R_en * n_position;
}

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> pz90toWGS84_pos(const Eigen::MatrixBase<Derived>& pz90_pos)
{
    typename Derived::Scalar m = -0.008e-6;
    auto omega_x = static_cast<typename Derived::Scalar>(-2.3_mas);
    auto omega_y = static_cast<typename Derived::Scalar>(3.54_mas);
    auto omega_z = static_cast<typename Derived::Scalar>(-4.21_mas);
    Eigen::Vector3<typename Derived::Scalar> dX{ -0.013, 0.106, 0.022 };
 
    Eigen::Matrix3<typename Derived::Scalar> T;
    T << 1, -omega_z, omega_y,
        omega_z, 1, -omega_x,
        -omega_y, omega_x, 1;
 
    return 1.0 / (1.0 + m) * T * (pz90_pos - dX);
}

template<typename DerivedA, typename DerivedB>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> pz90toWGS84(const Eigen::MatrixBase<DerivedA>& pz90, const Eigen::MatrixBase<DerivedB>& pz90_pos)
{
    return pz90toWGS84_pos(pz90_pos + pz90) - pz90toWGS84_pos(pz90_pos);
}

template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> sph2ecef(const Eigen::MatrixBase<Derived>& position_s,
                                                                const typename Derived::Scalar& elevation,
                                                                const typename Derived::Scalar& azimuth)
{
    Eigen::Matrix3<typename Derived::Scalar> R_se;
    R_se << std::sin(elevation) * std::cos(azimuth), std::cos(elevation) * std::cos(azimuth), -std::sin(azimuth),
        std::sin(elevation) * std::sin(azimuth), std::cos(elevation) * std::sin(azimuth), std::cos(azimuth),
        std::cos(elevation), -std::sin(elevation), 0.0;
 
    return R_se * position_s;
}
 
} // namespace NAV::trafo