Functions.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/Core>
 
#include "Navigation/Constants.hpp"
#include "Navigation/Math/Math.hpp"
 
namespace NAV
{
template<typename DerivedA, typename DerivedB>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> n_calcTransportRate(const Eigen::MatrixBase<DerivedA>& lla_position,
                                                                            const Eigen::MatrixBase<DerivedB>& n_velocity,
                                                                            const typename DerivedA::Scalar& R_N,
                                                                            const typename DerivedA::Scalar& R_E)
{
    // 𝜙 Latitude in [rad]
    const auto& latitude = lla_position(0);
    // h Altitude in [m]
    const auto& altitude = lla_position(2);
 
    // Velocity North in [m/s]
    const auto& v_N = n_velocity(0);
    // Velocity East in [m/s]
    const auto& v_E = n_velocity(1);
 
    Eigen::Vector3<typename DerivedA::Scalar> n_omega_en__t1;
    n_omega_en__t1(0) = v_E / (R_E + altitude);
    n_omega_en__t1(1) = -v_N / (R_N + altitude);
    n_omega_en__t1(2) = -n_omega_en__t1(0) * std::tan(latitude);
 
    return n_omega_en__t1;
}
 
template<typename DerivedA, typename DerivedB = DerivedA>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcCentrifugalAcceleration(const Eigen::MatrixBase<DerivedA>& e_position,
                                                                                      const Eigen::MatrixBase<DerivedB>& e_omega_ie = InsConst<typename DerivedB::Scalar>::e_omega_ie)
{
    // ω_ie_e ⨯ [ω_ie_e ⨯ x_e]
    return e_omega_ie.cross(e_omega_ie.cross(e_position));
}
 
template<typename DerivedA, typename DerivedB, typename DerivedC>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> n_calcCoriolisAcceleration(const Eigen::MatrixBase<DerivedA>& n_omega_ie,
                                                                                   const Eigen::MatrixBase<DerivedB>& n_omega_en,
                                                                                   const Eigen::MatrixBase<DerivedC>& n_velocity)
{
    return (2 * n_omega_ie + n_omega_en).cross(n_velocity);
}
 
template<typename DerivedA, typename DerivedB>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcCoriolisAcceleration(const Eigen::MatrixBase<DerivedA>& e_omega_ie, const Eigen::MatrixBase<DerivedB>& e_velocity)
{
    return (2 * e_omega_ie).cross(e_velocity);
}
 
template<typename Derived>
[[nodiscard]] typename Derived::Scalar calcRollFromStaticAcceleration(const Eigen::MatrixBase<Derived>& b_accel)
{
    // See E.-H. Shin (2005) - Estimation Techniques for Low-Cost Inertial Navigation (Chapter 2.6 - eq. 2.72a)
    return math::sgn(b_accel.z()) * std::asin(b_accel.y() / b_accel.norm());
 
    // Another possible calculation would be:
    // return std::atan2(b_accel.y(), b_accel.z());
}
 
template<typename Derived>
[[nodiscard]] typename Derived::Scalar calcPitchFromStaticAcceleration(const Eigen::MatrixBase<Derived>& b_accel)
{
    // See E.-H. Shin (2005) - Estimation Techniques for Low-Cost Inertial Navigation (Chapter 2.6 - eq. 2.72b)
    return -math::sgn(b_accel.z()) * std::asin(b_accel.x() / b_accel.norm());
 
    // Another possible calculation would be:
    // return std::atan2((-b_accel.x()), sqrt(std::pow(b_accel.y(), 2) + std::pow(b_accel.z(), 2)));
}
 
template<typename Derived>
[[nodiscard]] typename Derived::Scalar calcYawFromVelocity(const Eigen::MatrixBase<Derived>& n_velocity)
{
    return std::atan2(n_velocity(1), n_velocity(0));
}
 
template<typename Derived>
[[nodiscard]] typename Derived::Scalar calcPitchFromVelocity(const Eigen::MatrixBase<Derived>& n_velocity)
{
    return std::atan(-n_velocity(2) / std::sqrt(std::pow(n_velocity(0), 2) + std::pow(n_velocity(1), 2)));
}
 
} // namespace NAV