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 <cmath>
#include <Eigen/Core>
#include "Navigation/Constants.hpp"
#include "Navigation/GNSS/Core/Frequency.hpp"
 
namespace NAV
{
 
template<typename DerivedA, typename DerivedB>
[[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcLineOfSightUnitVector(const Eigen::MatrixBase<DerivedA>& e_posAnt,
                                                                                    const Eigen::MatrixBase<DerivedB>& e_posSat)
{
    return (e_posSat - e_posAnt) / (e_posSat - e_posAnt).norm();
}
 
template<typename Derived>
[[nodiscard]] typename Derived::Scalar calcSatElevation(const Eigen::MatrixBase<Derived>& n_lineOfSightUnitVector)
{
    return -asin(n_lineOfSightUnitVector(2));
}
 
template<typename Derived>
[[nodiscard]] typename Derived::Scalar calcSatAzimuth(const Eigen::MatrixBase<Derived>& n_lineOfSightUnitVector)
{
    return atan2(n_lineOfSightUnitVector(1), n_lineOfSightUnitVector(0));
}
 
template<typename DerivedA, typename DerivedB>
[[nodiscard]] typename DerivedA::Scalar calcSagnacCorrection(const Eigen::MatrixBase<DerivedA>& e_posAnt,
                                                             const Eigen::MatrixBase<DerivedB>& e_satPos)
{
    return 1.0 / InsConst::C * (e_posAnt - e_satPos).dot(InsConst::e_omega_ie.cross(e_posAnt));
}
 
template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
[[nodiscard]] typename DerivedA::Scalar calcSagnacRateCorrection(const Eigen::MatrixBase<DerivedA>& e_recvPos,
                                                                 const Eigen::MatrixBase<DerivedB>& e_satPos,
                                                                 const Eigen::MatrixBase<DerivedC>& e_recvVel,
                                                                 const Eigen::MatrixBase<DerivedD>& e_satVel)
{
    return InsConst::omega_ie / InsConst::C
           * (e_satVel.y() * e_recvPos.x() + e_satPos.y() * e_recvVel.x()
              - e_satVel.x() * e_recvPos.y() - e_satPos.x() * e_recvVel.y());
}
 
[[nodiscard]] double doppler2rangeRate(double doppler, Frequency freq, int8_t num);
 
[[nodiscard]] double rangeRate2doppler(double rangeRate, Frequency freq, int8_t num);
 
[[nodiscard]] double ratioFreqSquared(Frequency f1, Frequency f2, int8_t num1, int8_t num2);
 
[[nodiscard]] uint8_t galSisaVal2Idx(double val);
 
[[nodiscard]] double galSisaIdx2Val(uint8_t idx);
 
[[nodiscard]] uint8_t gpsUraVal2Idx(double val);
 
[[nodiscard]] double gpsUraIdx2Val(uint8_t idx);
 
} // namespace NAV