Gravity.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 <Eigen/Dense>
#include <Eigen/Core>
 
#include "Navigation/Transformations/CoordinateFrames.hpp"
#include "util/Logger.hpp"
#include "Navigation/Constants.hpp"
#include "internal/AssociatedLegendre.hpp"
#include "internal/egm96Coeffs.hpp"
 
namespace NAV
{
enum class GravitationModel : int
{
    None,         
    WGS84,        
    WGS84_Skydel, 
    Somigliana,   
    EGM96,        
    COUNT,        
};
 
const char* to_string(GravitationModel gravitationModel);
 
bool ComboGravitationModel(const char* label, GravitationModel& gravitationModel);
 
template<typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
[[nodiscard]] Eigen::Vector3<Scalar> n_calcGravitation_SomiglianaAltitude(const Scalar& latitude, const Scalar& altitude)
{
    // eq 6.16 has a fault in the denominator, it should be a sin^2(latitude)
    auto g_0 = 9.7803253359 * (1.0 + 1.931853e-3 * std::pow(std::sin(latitude), 2))
               / std::sqrt(1.0 - InsConst<Scalar>::WGS84::e_squared * std::pow(std::sin(latitude), 2));
 
    // Altitude compensation (Matlab example from Chapter 6_GNSS_INS_1 - glocal.m)
    auto k = 1
             - (2 * altitude / InsConst<Scalar>::WGS84::a)
                   * (1 + InsConst<Scalar>::WGS84::f
                      + (std::pow(InsConst<Scalar>::omega_ie * InsConst<Scalar>::WGS84::a, 2))
                            * (InsConst<Scalar>::WGS84::b / InsConst<Scalar>::WGS84::MU))
             + 3 * std::pow(altitude / InsConst<Scalar>::WGS84::a, 2);
 
    return { 0.0, 0.0, k * g_0 };
}
 
template<typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
[[nodiscard]] Eigen::Vector3<Scalar> n_calcGravitation_WGS84_Skydel(const Scalar& latitude, const Scalar& altitude)
{
    // geocentric latitude determination from geographic latitude
    auto latitudeGeocentric = std::atan((std::pow(InsConst<Scalar>::WGS84::b, 2.0) / std::pow(InsConst<Scalar>::WGS84::a, 2.0)) * std::tan(latitude));
    // effective radius determination, i.e. earth radius on WGS84 spheroid plus local altitude --> possible error!! altitude in lla should be added rather than subtracted!
    auto radiusSpheroid = InsConst<Scalar>::WGS84::a * (1.0 - InsConst<Scalar>::WGS84::f * std::pow(std::sin(latitudeGeocentric), 2.0)) - altitude;
 
    // Derivation of gravity, i.e. gravitational potential derived after effective radius
    auto gravitationMagnitude = InsConst<Scalar>::WGS84::MU * std::pow(radiusSpheroid, -2.0)
                                - 3 * InsConst<Scalar>::WGS84::MU * InsConst<Scalar>::WGS84::J2 * std::pow(InsConst<Scalar>::WGS84::a, 2.0) * 0.5 * std::pow(radiusSpheroid, -4.0) * (3 * std::pow(std::sin(latitudeGeocentric), 2.0) - 1)
                                - std::pow(InsConst<Scalar>::omega_ie_Skydel, 2.0) * radiusSpheroid * std::pow(std::cos(latitudeGeocentric), 2.0);
 
    return { 0, 0, gravitationMagnitude };
}
 
template<typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
[[nodiscard]] Eigen::Vector3<Scalar> n_calcGravitation_WGS84(const Scalar& latitude, const Scalar& altitude)
{
    // Geocentric latitude determination from geographic latitude
    auto latitudeGeocentric = std::atan((std::pow(InsConst<Scalar>::WGS84::b, 2.0) / std::pow(InsConst<Scalar>::WGS84::a, 2.0)) * std::tan(latitude));
    // Radius of spheroid determination
    auto radiusSpheroid = InsConst<Scalar>::WGS84::a * (1.0 - InsConst<Scalar>::WGS84::f * std::pow(std::sin(latitudeGeocentric), 2.0)) + altitude;
 
    // Magnitude of the gravity, i.e. without orientation
    auto gravitationMagnitude = InsConst<Scalar>::WGS84::MU * std::pow(radiusSpheroid, -2.0)
                                - 3 * InsConst<Scalar>::WGS84::MU * InsConst<Scalar>::WGS84::J2 * std::pow(InsConst<Scalar>::WGS84::a, 2.0) * 0.5 * std::pow(radiusSpheroid, -4.0) * (3 * std::pow(std::sin(latitudeGeocentric), 2.0) - 1)
                                - std::pow(InsConst<Scalar>::omega_ie, 2.0) * radiusSpheroid * std::pow(std::cos(latitudeGeocentric), 2.0);
 
    return { 0, 0, gravitationMagnitude };
}
 
template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> n_calcGravitation_EGM96(const Eigen::MatrixBase<Derived>& lla_position, size_t ndegree = 10)
{
    using internal::egm96Coeffs;
    using internal::associatedLegendre;
 
    auto e_position = trafo::lla2ecef_WGS84(lla_position);
 
    // Geocentric latitude determination from Groves (2013) - eq. (2.114)
    auto latitudeGeocentric = std::atan(e_position(2) / std::sqrt(e_position(0) * e_position(0) + e_position(1) * e_position(1)));
 
    // Spherical coordinates
    auto radius = std::sqrt(e_position(0) * e_position(0) + e_position(1) * e_position(1) + e_position(2) * e_position(2));
    auto elevation = M_PI_2 - latitudeGeocentric; // [rad]
    auto azimuth = lla_position(1);               // [rad]
 
    // Gravitation vector in local-navigation frame coordinates in [m/s^2]
    Eigen::Vector3<typename Derived::Scalar> n_gravitation = Eigen::Vector3<typename Derived::Scalar>::Zero();
 
    typename Derived::Scalar Pnm = 0;
    typename Derived::Scalar Pnmd = 0;
 
    auto coeffsRows = egm96Coeffs.size();
 
    // Associated Legendre Polynomial Coefficients 'P' and their derivatives 'Pd'
    auto [P, Pd] = associatedLegendre(static_cast<double>(elevation), ndegree);
 
    for (size_t i = 0; i < coeffsRows; i++) // NOLINT(clang-analyzer-core.UndefinedBinaryOperatorResult) // FIXME: Wrong error message about Eigen (error: The left operand of '*' is a garbage value)
    {
        // Retrieving EGM96 coefficients
        auto n = static_cast<int>(egm96Coeffs.at(i).at(0)); // Degree of the Associated Legendre Polynomial
        auto m = static_cast<int>(egm96Coeffs.at(i).at(1)); // Order of the Associated Legendre Polynomial
        auto C = egm96Coeffs.at(i).at(2);
        auto S = egm96Coeffs.at(i).at(3);
 
        if (static_cast<size_t>(n) == ndegree + 1)
        {
            // Ending of the for-loop once the iterated 'n' becomes larger than the user-defined 'ndegree'
            i = coeffsRows;
        }
        else
        {
            // Retrieving the parameters of the associated Legendre Polynomials
            Pnm = P(n, m);
            Pnmd = Pd(n, m);
 
            auto nd = static_cast<double>(n);
            auto md = static_cast<double>(m);
 
            // Gravity vector from differentiation of the gravity potential in spherical coordinates (see 'GUT User Guide' eq. 7.4.2)
            n_gravitation(0) += std::pow((InsConst<typename Derived::Scalar>::WGS84::a / radius), nd) * (C * std::cos(md * azimuth) + S * std::sin(md * azimuth)) * Pnmd;
            n_gravitation(1) += std::pow((InsConst<typename Derived::Scalar>::WGS84::a / radius), nd) * md * (C * std::sin(md * azimuth) - S * std::cos(md * azimuth)) * Pnm;
            n_gravitation(2) += (nd + 1.0) * std::pow((InsConst<typename Derived::Scalar>::WGS84::a / radius), nd) * (C * std::cos(md * azimuth) + S * std::sin(md * azimuth)) * Pnm;
        }
    }
 
    return { -InsConst<typename Derived::Scalar>::WGS84::MU / (radius * radius) * n_gravitation(0),
             (1.0 / std::sin(elevation)) * (-InsConst<typename Derived::Scalar>::WGS84::MU / (radius * radius)) * n_gravitation(1),
             InsConst<typename Derived::Scalar>::WGS84::MU / (radius * radius) * (1.0 + n_gravitation(2)) };
}
 
template<typename Derived>
[[nodiscard]] Eigen::Vector3<typename Derived::Scalar> n_calcGravitation(const Eigen::MatrixBase<Derived>& lla_position,
                                                                         GravitationModel gravitationModel = GravitationModel::EGM96)
{
    const typename Derived::Scalar& latitude = lla_position(0);
    const typename Derived::Scalar& altitude = lla_position(2);
 
    if (gravitationModel == GravitationModel::WGS84)
    {
        return n_calcGravitation_WGS84(latitude, altitude);
    }
    if (gravitationModel == GravitationModel::WGS84_Skydel) // TODO: This function becomes obsolete, once the ImuStream is deactivated due to the 'InstinctDataStream'
    {
        return n_calcGravitation_WGS84_Skydel(latitude, altitude);
    }
    if (gravitationModel == GravitationModel::Somigliana)
    {
        return n_calcGravitation_SomiglianaAltitude(latitude, altitude);
    }
    if (gravitationModel == GravitationModel::EGM96)
    {
        return n_calcGravitation_EGM96(lla_position);
    }
    return Eigen::Vector3<typename Derived::Scalar>::Zero();
}
 
} // namespace NAV