INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/GNSS/Satellite/Ephemeris/GPSEphemeris.cpp
Date:	2025-06-02 15:19:59

	Exec	Total	Coverage
Lines:	146	146	100.0%
Functions:	7	7	100.0%
Branches:	48	78	61.5%

  
      Line
      Branch
      Exec
      Source
    
      // 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/.
    
      #include "GPSEphemeris.hpp"
    
      #include "Navigation/Constants.hpp"
    
      #include "Navigation/GNSS/Functions.hpp"
    
      #include "util/Logger.hpp"
    
      namespace NAV
    
      {
    
      19
      GPSEphemeris::GPSEphemeris(const InsTime& toc)
    
      19
          : SatNavData(SatNavData::GPSEphemeris, toc) {}
    
      1173
      GPSEphemeris::GPSEphemeris(const InsTime& toc, const InsTime& toe,
    
                                 const size_t& IODE, const size_t& IODC,
    
                                 const std::array<double, 3>& a,
    
                                 const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0,
    
                                 const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc,
    
                                 const double& Cis, const double& Cic, const double& Crs, const double& Crc,
    
                                 const double& svAccuracy, uint8_t svHealth,
    
                                 uint8_t L2ChannelCodes, bool L2DataFlagPCode,
    
                                 const double& T_GD,
    
      1173
                                 const double& fitInterval)
    
          : SatNavData(SatNavData::GPSEphemeris, toc),
    
      1173
            toc(toc),
    
      1173
            toe(toe),
    
      1173
            IODE(IODE),
    
      1173
            IODC(IODC),
    
      1173
            a(a),
    
      1173
            sqrt_A(sqrt_A),
    
      1173
            e(e),
    
      1173
            i_0(i_0),
    
      1173
            Omega_0(Omega_0),
    
      1173
            omega(omega),
    
      1173
            M_0(M_0),
    
      1173
            delta_n(delta_n),
    
      1173
            Omega_dot(Omega_dot),
    
      1173
            i_dot(i_dot),
    
      1173
            Cus(Cus),
    
      1173
            Cuc(Cuc),
    
      1173
            Cis(Cis),
    
      1173
            Cic(Cic),
    
      1173
            Crs(Crs),
    
      1173
            Crc(Crc),
    
      1173
            svAccuracy(svAccuracy),
    
      1173
            svHealth(svHealth),
    
      1173
            L2ChannelCodes(L2ChannelCodes),
    
      1173
            L2DataFlagPCode(L2DataFlagPCode),
    
      1173
            T_GD(T_GD),
    
      1173
            fitInterval(fitInterval) {}
    
      #ifdef TESTING
    
      8251
      GPSEphemeris::GPSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate,
    
                                 double IODE, double Crs, double delta_n, double M_0,
    
                                 double Cuc, double e, double Cus, double sqrt_A,
    
                                 double Toe, double Cic, double Omega_0, double Cis,
    
                                 double i_0, double Crc, double omega, double Omega_dot,
    
                                 double i_dot, double L2ChannelCodes, double GPSWeek, double L2DataFlagPCode,
    
                                 double svAccuracy, double svHealth, double T_GD, double IODC,
    
      8251
                                 double /* TransmissionTimeOfMessage */, double fitInterval, double /* spare1 */, double /* spare2 */)
    
        2/4✓ Branch 1 taken 8251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8251 times.
✗ Branch 5 not taken.

      8251
          : SatNavData(SatNavData::GPSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(GPS).getTimeSystem())),
    
      8251
            toc(refTime),
    
        2/4✓ Branch 1 taken 8251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8251 times.
✗ Branch 5 not taken.

      8251
            toe(InsTime(0, static_cast<int32_t>(GPSWeek), Toe, SatelliteSystem(GPS).getTimeSystem())),
    
      8251
            IODE(static_cast<size_t>(IODE)),
    
      8251
            IODC(static_cast<size_t>(IODC)),
    
      8251
            a({ svClockBias, svClockDrift, svClockDriftRate }),
    
      8251
            sqrt_A(sqrt_A),
    
      8251
            e(e),
    
      8251
            i_0(i_0),
    
      8251
            Omega_0(Omega_0),
    
      8251
            omega(omega),
    
      8251
            M_0(M_0),
    
      8251
            delta_n(delta_n),
    
      8251
            Omega_dot(Omega_dot),
    
      8251
            i_dot(i_dot),
    
      8251
            Cus(Cus),
    
      8251
            Cuc(Cuc),
    
      8251
            Cis(Cis),
    
      8251
            Cic(Cic),
    
      8251
            Crs(Crs),
    
      8251
            Crc(Crc),
    
      8251
            svAccuracy(svAccuracy),
    
      8251
            svHealth(static_cast<uint8_t>(svHealth)),
    
      8251
            L2ChannelCodes(static_cast<uint8_t>(L2ChannelCodes)),
    
      8251
            L2DataFlagPCode(static_cast<bool>(L2DataFlagPCode)),
    
      8251
            T_GD(T_GD),
    
        1/2✓ Branch 2 taken 8251 times.
✗ Branch 3 not taken.

      16502
            fitInterval(fitInterval)
    
      8251
      {}
    
      #endif
    
      43417
      Clock::Corrections GPSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const
    
      {
    
          LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow());
    
          // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
    
      43417
          const auto mu = InsConst::GPS::MU;
    
          // Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
    
      43417
          const auto F = InsConst::GPS::F;
    
          LOG_DATA("    toe {} [{}] (Time of ephemeris)", toe.toGPSweekTow(), toe.toYMDHMS(GPST));
    
      43417
          const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
    
          LOG_DATA("    A {} [m] (Semi-major axis)", A);
    
      43417
          auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
    
          LOG_DATA("    n_0 {} [rad/s] (Computed mean motion)", n_0);
    
      43417
          auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
    
          LOG_DATA("    n {} [rad/s] (Corrected mean motion)", n);
    
          // Time at transmission
    
        2/4✓ Branch 2 taken 43417 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 43417 times.
✗ Branch 6 not taken.

      43417
          InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C);
    
      43417
          InsTime transTime = transTime0;
    
          LOG_DATA("    Iterating Time at transmission");
    
      43417
          double dt_sv = 0.0;
    
      43417
          double clkDrift = 0.0;
    
        2/2✓ Branch 0 taken 86834 times.
✓ Branch 1 taken 43417 times.

      130251
          for (size_t i = 0; i < 2; i++)
    
          {
    
              LOG_DATA("     [{}]", i);
    
              LOG_DATA("      transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST));
    
              // [s]
    
        1/2✓ Branch 1 taken 86834 times.
✗ Branch 2 not taken.

      86834
              auto t_minus_toc = static_cast<double>((transTime - toc).count());
    
              LOG_DATA("      transTime - toc {} [s]", t_minus_toc);
    
              // Time difference from ephemeris reference epoch [s]
    
        1/2✓ Branch 1 taken 86834 times.
✗ Branch 2 not taken.

      86834
              double t_k = static_cast<double>((transTime - toe).count());
    
              LOG_DATA("      transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k);
    
              // Mean anomaly [rad]
    
      86834
              auto M_k = M_0 + n * t_k;
    
              LOG_DATA("      M_k {} [s] (Mean anomaly)", M_k);
    
              // Eccentric anomaly [rad]
    
      86834
              double E_k = M_k;
    
      86834
              double E_k_old = 0.0;
    
        5/6✓ Branch 1 taken 510132 times.
✓ Branch 2 taken 86834 times.
✓ Branch 3 taken 510132 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 510132 times.
✓ Branch 6 taken 86834 times.

      596966
              for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
    
              {
    
      510132
                  E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
    
      510132
                  E_k = M_k + e * sin(E_k);
    
              }
    
              // Relativistic correction term [s]
    
      86834
              double dt_r = F * e * sqrt_A * std::sin(E_k);
    
              LOG_DATA("      dt_r {} [s] (Relativistic correction term)", dt_r);
    
              // SV PRN code phase time offset [s]
    
      86834
              dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r;
    
              // See IS-GPS-200M GPS ICD L1/L2, ch. 20.3.3.3.3.2, p.99
    
              // See IS-GPS-705J GPS ICD L5,    ch. 20.3.3.3.2.1, p.78
    
        3/4✓ Branch 2 taken 58978 times.
✓ Branch 3 taken 27856 times.
✓ Branch 6 taken 27856 times.
✗ Branch 7 not taken.

      86834
              dt_sv -= freq & (G01 | G05) ? T_GD : ratioFreqSquared(G01, freq, -128, -128) * T_GD;
    
              LOG_DATA("      dt_sv {} [s] (SV PRN code phase time offset)", dt_sv);
    
              // Groves ch. 9.3.1, eq. 9.78, p. 391
    
      86834
              clkDrift = a[1] + a[2] / 2.0 * t_minus_toc;
    
              // Correct transmit time for the satellite clock bias
    
        2/4✓ Branch 2 taken 86834 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 86834 times.
✗ Branch 6 not taken.

      86834
              transTime = transTime0 - std::chrono::duration<double>(dt_sv);
    
          }
    
          LOG_DATA("      transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST));
    
      43417
          return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift };
    
      }
    
      48997
      Orbit::PosVelAccel GPSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
    
      {
    
        2/4✓ Branch 1 taken 48997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48997 times.
✗ Branch 5 not taken.

      48997
          Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
    
        2/4✓ Branch 1 taken 48997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48997 times.
✗ Branch 5 not taken.

      48997
          Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
    
        2/4✓ Branch 1 taken 48997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48997 times.
✗ Branch 5 not taken.

      48997
          Eigen::Vector3d e_accel = Eigen::Vector3d::Zero();
    
          LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow());
    
          // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
    
      48997
          const auto mu = InsConst::GPS::MU;
    
          // Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
    
      48997
          const auto Omega_e_dot = InsConst::GPS::omega_ie;
    
          LOG_DATA("    toe {} (Time of ephemeris)", toe.toGPSweekTow());
    
      48997
          const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
    
          LOG_DATA("    A {} [m] (Semi-major axis)", A);
    
      48997
          auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
    
          LOG_DATA("    n_0 {} [rad/s] (Computed mean motion)", n_0);
    
      48997
          auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
    
          LOG_DATA("    n {} [rad/s] (Corrected mean motion)", n);
    
          // Eccentric anomaly [rad]
    
      48997
          double E_k = 0.0;
    
          // Time difference from ephemeris reference epoch [s]
    
        1/2✓ Branch 1 taken 48997 times.
✗ Branch 2 not taken.

      48997
          double t_k = static_cast<double>((transTime - toe).count());
    
          LOG_DATA("    t_k {} [s] (Time difference from ephemeris reference epoch)", t_k);
    
          // Mean anomaly [rad]
    
      48997
          auto M_k = M_0 + n * t_k;
    
          LOG_DATA("    M_k {} [s] (Mean anomaly)", M_k);
    
      48997
          E_k = M_k; // Initial Value [rad]
    
      48997
          double E_k_old = 0.0;
    
          LOG_DATA("    Iterating E_k");
    
          LOG_DATA("      E_k {} [rad] (Eccentric anomaly)", E_k);
    
        5/6✓ Branch 1 taken 149485 times.
✓ Branch 2 taken 48997 times.
✓ Branch 3 taken 149485 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 149485 times.
✓ Branch 6 taken 48997 times.

      198482
          for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
    
          {
    
      149485
              E_k_old = E_k;                                                         // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
    
      149485
              E_k = E_k + (M_k - E_k + e * std::sin(E_k)) / (1 - e * std::cos(E_k)); // - Refined Value, minimum of three iterations, (j=1,2,3)
    
              LOG_DATA("      E_k {} [rad] (Eccentric anomaly)", E_k);               // - Final Value (radians)
    
          }
    
          // auto v_k = 2.0 * std::atan(std::sqrt((1.0 + e) / (1.0 - e)) * std::tan(E_k / 2.0)); // True Anomaly (unambiguous quadrant) [rad] (GPS ICD algorithm)
    
          // auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k) / (1 - e * std::cos(E_k)), (std::cos(E_k) - e) / (1 - e * std::cos(E_k))); // True Anomaly [rad] (GALILEO ICD algorithm)
    
      48997
          auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k), (std::cos(E_k) - e)); // True Anomaly [rad] // simplified, since the denominators cancel out
    
          LOG_DATA("    v_k {} [rad] (True Anomaly (unambiguous quadrant))", v_k);
    
      48997
          auto Phi_k = v_k + omega; // Argument of Latitude [rad]
    
          LOG_DATA("    Phi_k {} [rad] (Argument of Latitude)", Phi_k);
    
          // Second Harmonic Perturbations
    
      48997
          auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad]
    
          LOG_DATA("    delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k);
    
      48997
          auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m]
    
          LOG_DATA("    delta_r_k {} [m] (Radius Correction)", delta_r_k);
    
      48997
          auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad]
    
          LOG_DATA("    delta_i_k {} [rad] (Inclination Correction)", delta_i_k);
    
      48997
          auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad]
    
          LOG_DATA("    u_k {} [rad] (Corrected Argument of Latitude)", u_k);
    
      48997
          auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m]
    
          LOG_DATA("    r_k {} [m] (Corrected Radius)", r_k);
    
      48997
          auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad]
    
          LOG_DATA("    i_k {} [rad] (Corrected Inclination)", i_k);
    
      48997
          auto x_k_op = r_k * std::cos(u_k); // Position in orbital plane [m]
    
          LOG_DATA("    x_k_op {} [m] (Position in orbital plane)", x_k_op);
    
      48997
          auto y_k_op = r_k * std::sin(u_k); // Position in orbital plane [m]
    
          LOG_DATA("    y_k_op {} [m] (Position in orbital plane)", y_k_op);
    
          // Corrected longitude of ascending node [rad]
    
        1/2✓ Branch 2 taken 48997 times.
✗ Branch 3 not taken.

      48997
          auto Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow().tow);
    
          LOG_DATA("    Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k);
    
          // Earth-fixed x coordinates [m]
    
      48997
          auto x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k);
    
          LOG_DATA("    x_k {} [m] (Earth-fixed x coordinates)", x_k);
    
          // Earth-fixed y coordinates [m]
    
      48997
          auto y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k);
    
          LOG_DATA("    y_k {} [m] (Earth-fixed y coordinates)", y_k);
    
          // Earth-fixed z coordinates [m]
    
      48997
          auto z_k = y_k_op * std::sin(i_k);
    
          LOG_DATA("    z_k {} [m] (Earth-fixed z coordinates)", z_k);
    
        1/2✓ Branch 1 taken 48997 times.
✗ Branch 2 not taken.

      48997
          e_pos = Eigen::Vector3d{ x_k, y_k, z_k };
    
        5/6✓ Branch 1 taken 7879 times.
✓ Branch 2 taken 41118 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7879 times.
✓ Branch 6 taken 41118 times.
✓ Branch 7 taken 7879 times.

      48997
          if (calc & Calc_Velocity || calc & Calc_Acceleration)
    
          {
    
              // Eccentric Anomaly Rate [rad/s]
    
      41118
              auto E_k_dot = n / (1 - e * std::cos(E_k));
    
              // True Anomaly Rate [rad/s]
    
      41118
              auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k));
    
              // Corrected Inclination Angle Rate [rad/s]
    
      41118
              auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k));
    
              // Corrected Argument of Latitude Rate [rad/s]
    
      41118
              auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k));
    
              // Corrected Radius Rate [m/s]
    
      41118
              auto r_k_dot = e * A * E_k_dot * std::sin(E_k) + 2 * v_k_dot * (Crs * std::cos(2 * Phi_k) - Crc * std::sin(2 * Phi_k));
    
              // Longitude of Ascending Node Rate [rad/s]
    
      41118
              auto Omega_k_dot = Omega_dot - Omega_e_dot;
    
              // In-plane x velocity [m/s]
    
      41118
              auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k);
    
              // In-plane y velocity [m/s]
    
      41118
              auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k);
    
              // Earth-Fixed x velocity [m/s]
    
      41118
              auto vx_k = -x_k_op * Omega_k_dot * std::sin(Omega_k) + vx_k_op * std::cos(Omega_k) - vy_k_op * std::sin(Omega_k) * std::cos(i_k)
    
      41118
                          - y_k_op * (Omega_k_dot * std::cos(Omega_k) * std::cos(i_k) - i_k_dot * std::sin(Omega_k) * std::sin(i_k));
    
              // Earth-Fixed y velocity [m/s]
    
      41118
              auto vy_k = x_k_op * Omega_k_dot * std::cos(Omega_k) + vx_k_op * std::sin(Omega_k) + vy_k_op * std::cos(Omega_k) * std::cos(i_k)
    
      41118
                          - y_k_op * (Omega_k_dot * std::sin(Omega_k) * std::cos(i_k) + i_k_dot * std::cos(Omega_k) * std::sin(i_k));
    
              // Earth-Fixed z velocity [m/s]
    
      41118
              auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k);
    
        1/2✓ Branch 1 taken 41118 times.
✗ Branch 2 not taken.

      41118
              if (calc & Calc_Velocity)
    
              {
    
        1/2✓ Branch 1 taken 41118 times.
✗ Branch 2 not taken.

      41118
                  e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k };
    
              }
    
        2/2✓ Branch 1 taken 2790 times.
✓ Branch 2 taken 38328 times.

      41118
              if (calc & Calc_Acceleration)
    
              {
    
                  // Oblate Earth acceleration Factor [m/s^2]
    
      2790
                  auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2);
    
                  // Earth-Fixed x acceleration [m/s^2]
    
      2790
                  auto ax_k = -mu * (x_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (x_k / r_k))
    
      2790
                              + 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2);
    
                  // Earth-Fixed y acceleration [m/s^2]
    
      2790
                  auto ay_k = -mu * (y_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (y_k / r_k))
    
      2790
                              + 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2);
    
                  // Earth-Fixed z acceleration [m/s^2]
    
      2790
                  auto az_k = -mu * (z_k / std::pow(r_k, 3)) + F * ((3.0 - 5.0 * std::pow(z_k / r_k, 2)) * (z_k / r_k));
    
        1/2✓ Branch 1 taken 2790 times.
✗ Branch 2 not taken.

      2790
                  e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k };
    
              }
    
          }
    
          return { .e_pos = e_pos,
    
                   .e_vel = e_vel,
    
        3/6✓ Branch 1 taken 48997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48997 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48997 times.
✗ Branch 8 not taken.

      97994
                   .e_accel = e_accel };
    
      }
    
      43392
      bool GPSEphemeris::isHealthy() const
    
      {
    
      43392
          return svHealth == 0;
    
      }
    
      34903
      double GPSEphemeris::calcSatellitePositionVariance() const
    
      {
    
      34903
          return std::pow(svAccuracy, 2);
    
      }
    
      } // namespace NAV

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			#include "GPSEphemeris.hpp"
10
11			#include "Navigation/Constants.hpp"
12			#include "Navigation/GNSS/Functions.hpp"
13
14			#include "util/Logger.hpp"
15
16			namespace NAV
17			{
18
19		19	GPSEphemeris::GPSEphemeris(const InsTime& toc)
20		19	: SatNavData(SatNavData::GPSEphemeris, toc) {}
21
22		1173	GPSEphemeris::GPSEphemeris(const InsTime& toc, const InsTime& toe,
23			const size_t& IODE, const size_t& IODC,
24			const std::array<double, 3>& a,
25			const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0,
26			const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc,
27			const double& Cis, const double& Cic, const double& Crs, const double& Crc,
28			const double& svAccuracy, uint8_t svHealth,
29			uint8_t L2ChannelCodes, bool L2DataFlagPCode,
30			const double& T_GD,
31		1173	const double& fitInterval)
32			: SatNavData(SatNavData::GPSEphemeris, toc),
33		1173	toc(toc),
34		1173	toe(toe),
35		1173	IODE(IODE),
36		1173	IODC(IODC),
37		1173	a(a),
38		1173	sqrt_A(sqrt_A),
39		1173	e(e),
40		1173	i_0(i_0),
41		1173	Omega_0(Omega_0),
42		1173	omega(omega),
43		1173	M_0(M_0),
44		1173	delta_n(delta_n),
45		1173	Omega_dot(Omega_dot),
46		1173	i_dot(i_dot),
47		1173	Cus(Cus),
48		1173	Cuc(Cuc),
49		1173	Cis(Cis),
50		1173	Cic(Cic),
51		1173	Crs(Crs),
52		1173	Crc(Crc),
53		1173	svAccuracy(svAccuracy),
54		1173	svHealth(svHealth),
55		1173	L2ChannelCodes(L2ChannelCodes),
56		1173	L2DataFlagPCode(L2DataFlagPCode),
57		1173	T_GD(T_GD),
58		1173	fitInterval(fitInterval) {}
59
60			#ifdef TESTING
61
62		8251	GPSEphemeris::GPSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate,
63			double IODE, double Crs, double delta_n, double M_0,
64			double Cuc, double e, double Cus, double sqrt_A,
65			double Toe, double Cic, double Omega_0, double Cis,
66			double i_0, double Crc, double omega, double Omega_dot,
67			double i_dot, double L2ChannelCodes, double GPSWeek, double L2DataFlagPCode,
68			double svAccuracy, double svHealth, double T_GD, double IODC,
69		8251	double /* TransmissionTimeOfMessage /, double fitInterval, double / spare1 /, double / spare2 */)
70	2/4 ✓ Branch 1 taken 8251 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 8251 times. ✗ Branch 5 not taken.	8251	: SatNavData(SatNavData::GPSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(GPS).getTimeSystem())),
71		8251	toc(refTime),
72	2/4 ✓ Branch 1 taken 8251 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 8251 times. ✗ Branch 5 not taken.	8251	toe(InsTime(0, static_cast<int32_t>(GPSWeek), Toe, SatelliteSystem(GPS).getTimeSystem())),
73		8251	IODE(static_cast<size_t>(IODE)),
74		8251	IODC(static_cast<size_t>(IODC)),
75		8251	a({ svClockBias, svClockDrift, svClockDriftRate }),
76		8251	sqrt_A(sqrt_A),
77		8251	e(e),
78		8251	i_0(i_0),
79		8251	Omega_0(Omega_0),
80		8251	omega(omega),
81		8251	M_0(M_0),
82		8251	delta_n(delta_n),
83		8251	Omega_dot(Omega_dot),
84		8251	i_dot(i_dot),
85		8251	Cus(Cus),
86		8251	Cuc(Cuc),
87		8251	Cis(Cis),
88		8251	Cic(Cic),
89		8251	Crs(Crs),
90		8251	Crc(Crc),
91		8251	svAccuracy(svAccuracy),
92		8251	svHealth(static_cast<uint8_t>(svHealth)),
93		8251	L2ChannelCodes(static_cast<uint8_t>(L2ChannelCodes)),
94		8251	L2DataFlagPCode(static_cast<bool>(L2DataFlagPCode)),
95		8251	T_GD(T_GD),
96	1/2 ✓ Branch 2 taken 8251 times. ✗ Branch 3 not taken.	16502	fitInterval(fitInterval)
97		8251	{}
98
99			#endif
100
101		43417	Clock::Corrections GPSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const
102			{
103			LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow());
104			// Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
105		43417	const auto mu = InsConst::GPS::MU;
106			// Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
107		43417	const auto F = InsConst::GPS::F;
108
109			LOG_DATA(" toe {} [{}] (Time of ephemeris)", toe.toGPSweekTow(), toe.toYMDHMS(GPST));
110
111		43417	const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
112			LOG_DATA(" A {} [m] (Semi-major axis)", A);
113		43417	auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
114			LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0);
115		43417	auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
116			LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n);
117
118			// Time at transmission
119	2/4 ✓ Branch 2 taken 43417 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 43417 times. ✗ Branch 6 not taken.	43417	InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C);
120
121		43417	InsTime transTime = transTime0;
122			LOG_DATA(" Iterating Time at transmission");
123		43417	double dt_sv = 0.0;
124		43417	double clkDrift = 0.0;
125
126	2/2 ✓ Branch 0 taken 86834 times. ✓ Branch 1 taken 43417 times.	130251	for (size_t i = 0; i < 2; i++)
127			{
128			LOG_DATA(" [{}]", i);
129			LOG_DATA(" transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST));
130
131			// [s]
132	1/2 ✓ Branch 1 taken 86834 times. ✗ Branch 2 not taken.	86834	auto t_minus_toc = static_cast<double>((transTime - toc).count());
133			LOG_DATA(" transTime - toc {} [s]", t_minus_toc);
134
135			// Time difference from ephemeris reference epoch [s]
136	1/2 ✓ Branch 1 taken 86834 times. ✗ Branch 2 not taken.	86834	double t_k = static_cast<double>((transTime - toe).count());
137			LOG_DATA(" transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k);
138
139			// Mean anomaly [rad]
140		86834	auto M_k = M_0 + n * t_k;
141			LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
142
143			// Eccentric anomaly [rad]
144		86834	double E_k = M_k;
145		86834	double E_k_old = 0.0;
146
147	5/6 ✓ Branch 1 taken 510132 times. ✓ Branch 2 taken 86834 times. ✓ Branch 3 taken 510132 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 510132 times. ✓ Branch 6 taken 86834 times.	596966	for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
148			{
149		510132	E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
150		510132	E_k = M_k + e * sin(E_k);
151			}
152
153			// Relativistic correction term [s]
154		86834	double dt_r = F * e * sqrt_A * std::sin(E_k);
155			LOG_DATA(" dt_r {} [s] (Relativistic correction term)", dt_r);
156
157			// SV PRN code phase time offset [s]
158		86834	dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r;
159
160			// See IS-GPS-200M GPS ICD L1/L2, ch. 20.3.3.3.3.2, p.99
161			// See IS-GPS-705J GPS ICD L5, ch. 20.3.3.3.2.1, p.78
162	3/4 ✓ Branch 2 taken 58978 times. ✓ Branch 3 taken 27856 times. ✓ Branch 6 taken 27856 times. ✗ Branch 7 not taken.	86834	dt_sv -= freq & (G01 \| G05) ? T_GD : ratioFreqSquared(G01, freq, -128, -128) * T_GD;
163
164			LOG_DATA(" dt_sv {} [s] (SV PRN code phase time offset)", dt_sv);
165
166			// Groves ch. 9.3.1, eq. 9.78, p. 391
167		86834	clkDrift = a[1] + a[2] / 2.0 * t_minus_toc;
168
169			// Correct transmit time for the satellite clock bias
170	2/4 ✓ Branch 2 taken 86834 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 86834 times. ✗ Branch 6 not taken.	86834	transTime = transTime0 - std::chrono::duration<double>(dt_sv);
171			}
172			LOG_DATA(" transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST));
173
174		43417	return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift };
175			}
176
177		48997	Orbit::PosVelAccel GPSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
178			{
179	2/4 ✓ Branch 1 taken 48997 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 48997 times. ✗ Branch 5 not taken.	48997	Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
180	2/4 ✓ Branch 1 taken 48997 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 48997 times. ✗ Branch 5 not taken.	48997	Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
181	2/4 ✓ Branch 1 taken 48997 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 48997 times. ✗ Branch 5 not taken.	48997	Eigen::Vector3d e_accel = Eigen::Vector3d::Zero();
182
183			LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow());
184			// Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
185		48997	const auto mu = InsConst::GPS::MU;
186			// Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
187		48997	const auto Omega_e_dot = InsConst::GPS::omega_ie;
188
189			LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow());
190
191		48997	const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
192			LOG_DATA(" A {} [m] (Semi-major axis)", A);
193		48997	auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
194			LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0);
195		48997	auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
196			LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n);
197
198			// Eccentric anomaly [rad]
199		48997	double E_k = 0.0;
200
201			// Time difference from ephemeris reference epoch [s]
202	1/2 ✓ Branch 1 taken 48997 times. ✗ Branch 2 not taken.	48997	double t_k = static_cast<double>((transTime - toe).count());
203			LOG_DATA(" t_k {} [s] (Time difference from ephemeris reference epoch)", t_k);
204
205			// Mean anomaly [rad]
206		48997	auto M_k = M_0 + n * t_k;
207			LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
208
209		48997	E_k = M_k; // Initial Value [rad]
210		48997	double E_k_old = 0.0;
211			LOG_DATA(" Iterating E_k");
212			LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k);
213	5/6 ✓ Branch 1 taken 149485 times. ✓ Branch 2 taken 48997 times. ✓ Branch 3 taken 149485 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 149485 times. ✓ Branch 6 taken 48997 times.	198482	for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
214			{
215		149485	E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
216		149485	E_k = E_k + (M_k - E_k + e * std::sin(E_k)) / (1 - e * std::cos(E_k)); // - Refined Value, minimum of three iterations, (j=1,2,3)
217			LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); // - Final Value (radians)
218			}
219
220			// auto v_k = 2.0 * std::atan(std::sqrt((1.0 + e) / (1.0 - e)) * std::tan(E_k / 2.0)); // True Anomaly (unambiguous quadrant) [rad] (GPS ICD algorithm)
221			// auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k) / (1 - e * std::cos(E_k)), (std::cos(E_k) - e) / (1 - e * std::cos(E_k))); // True Anomaly [rad] (GALILEO ICD algorithm)
222		48997	auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k), (std::cos(E_k) - e)); // True Anomaly [rad] // simplified, since the denominators cancel out
223			LOG_DATA(" v_k {} [rad] (True Anomaly (unambiguous quadrant))", v_k);
224		48997	auto Phi_k = v_k + omega; // Argument of Latitude [rad]
225			LOG_DATA(" Phi_k {} [rad] (Argument of Latitude)", Phi_k);
226
227			// Second Harmonic Perturbations
228		48997	auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad]
229			LOG_DATA(" delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k);
230		48997	auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m]
231			LOG_DATA(" delta_r_k {} [m] (Radius Correction)", delta_r_k);
232		48997	auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad]
233			LOG_DATA(" delta_i_k {} [rad] (Inclination Correction)", delta_i_k);
234
235		48997	auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad]
236			LOG_DATA(" u_k {} [rad] (Corrected Argument of Latitude)", u_k);
237		48997	auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m]
238			LOG_DATA(" r_k {} [m] (Corrected Radius)", r_k);
239		48997	auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad]
240			LOG_DATA(" i_k {} [rad] (Corrected Inclination)", i_k);
241
242		48997	auto x_k_op = r_k * std::cos(u_k); // Position in orbital plane [m]
243			LOG_DATA(" x_k_op {} [m] (Position in orbital plane)", x_k_op);
244		48997	auto y_k_op = r_k * std::sin(u_k); // Position in orbital plane [m]
245			LOG_DATA(" y_k_op {} [m] (Position in orbital plane)", y_k_op);
246
247			// Corrected longitude of ascending node [rad]
248	1/2 ✓ Branch 2 taken 48997 times. ✗ Branch 3 not taken.	48997	auto Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow().tow);
249			LOG_DATA(" Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k);
250
251			// Earth-fixed x coordinates [m]
252		48997	auto x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k);
253			LOG_DATA(" x_k {} [m] (Earth-fixed x coordinates)", x_k);
254			// Earth-fixed y coordinates [m]
255		48997	auto y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k);
256			LOG_DATA(" y_k {} [m] (Earth-fixed y coordinates)", y_k);
257			// Earth-fixed z coordinates [m]
258		48997	auto z_k = y_k_op * std::sin(i_k);
259			LOG_DATA(" z_k {} [m] (Earth-fixed z coordinates)", z_k);
260
261	1/2 ✓ Branch 1 taken 48997 times. ✗ Branch 2 not taken.	48997	e_pos = Eigen::Vector3d{ x_k, y_k, z_k };
262
263	5/6 ✓ Branch 1 taken 7879 times. ✓ Branch 2 taken 41118 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 7879 times. ✓ Branch 6 taken 41118 times. ✓ Branch 7 taken 7879 times.	48997	if (calc & Calc_Velocity \|\| calc & Calc_Acceleration)
264			{
265			// Eccentric Anomaly Rate [rad/s]
266		41118	auto E_k_dot = n / (1 - e * std::cos(E_k));
267			// True Anomaly Rate [rad/s]
268		41118	auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k));
269			// Corrected Inclination Angle Rate [rad/s]
270		41118	auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k));
271			// Corrected Argument of Latitude Rate [rad/s]
272		41118	auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k));
273			// Corrected Radius Rate [m/s]
274		41118	auto r_k_dot = e * A * E_k_dot * std::sin(E_k) + 2 * v_k_dot * (Crs * std::cos(2 * Phi_k) - Crc * std::sin(2 * Phi_k));
275			// Longitude of Ascending Node Rate [rad/s]
276		41118	auto Omega_k_dot = Omega_dot - Omega_e_dot;
277			// In-plane x velocity [m/s]
278		41118	auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k);
279			// In-plane y velocity [m/s]
280		41118	auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k);
281			// Earth-Fixed x velocity [m/s]
282		41118	auto vx_k = -x_k_op * Omega_k_dot * std::sin(Omega_k) + vx_k_op * std::cos(Omega_k) - vy_k_op * std::sin(Omega_k) * std::cos(i_k)
283		41118	- y_k_op * (Omega_k_dot * std::cos(Omega_k) * std::cos(i_k) - i_k_dot * std::sin(Omega_k) * std::sin(i_k));
284			// Earth-Fixed y velocity [m/s]
285		41118	auto vy_k = x_k_op * Omega_k_dot * std::cos(Omega_k) + vx_k_op * std::sin(Omega_k) + vy_k_op * std::cos(Omega_k) * std::cos(i_k)
286		41118	- y_k_op * (Omega_k_dot * std::sin(Omega_k) * std::cos(i_k) + i_k_dot * std::cos(Omega_k) * std::sin(i_k));
287			// Earth-Fixed z velocity [m/s]
288		41118	auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k);
289
290	1/2 ✓ Branch 1 taken 41118 times. ✗ Branch 2 not taken.	41118	if (calc & Calc_Velocity)
291			{
292	1/2 ✓ Branch 1 taken 41118 times. ✗ Branch 2 not taken.	41118	e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k };
293			}
294
295	2/2 ✓ Branch 1 taken 2790 times. ✓ Branch 2 taken 38328 times.	41118	if (calc & Calc_Acceleration)
296			{
297			// Oblate Earth acceleration Factor [m/s^2]
298		2790	auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2);
299			// Earth-Fixed x acceleration [m/s^2]
300		2790	auto ax_k = -mu * (x_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (x_k / r_k))
301		2790	+ 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2);
302			// Earth-Fixed y acceleration [m/s^2]
303		2790	auto ay_k = -mu * (y_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (y_k / r_k))
304		2790	+ 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2);
305			// Earth-Fixed z acceleration [m/s^2]
306		2790	auto az_k = -mu * (z_k / std::pow(r_k, 3)) + F * ((3.0 - 5.0 * std::pow(z_k / r_k, 2)) * (z_k / r_k));
307
308	1/2 ✓ Branch 1 taken 2790 times. ✗ Branch 2 not taken.	2790	e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k };
309			}
310			}
311
312			return { .e_pos = e_pos,
313			.e_vel = e_vel,
314	3/6 ✓ Branch 1 taken 48997 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 48997 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 48997 times. ✗ Branch 8 not taken.	97994	.e_accel = e_accel };
315			}
316
317		43392	bool GPSEphemeris::isHealthy() const
318			{
319		43392	return svHealth == 0;
320			}
321
322		34903	double GPSEphemeris::calcSatellitePositionVariance() const
323			{
324		34903	return std::pow(svAccuracy, 2);
325			}
326
327			} // namespace NAV
328