INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/GNSS/Satellite/Ephemeris/GPSEphemeris.cpp
Date:	2025-07-19 10:51:51

	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) {}
    
      1204
      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,
    
      1204
                                 const double& fitInterval)
    
          : SatNavData(SatNavData::GPSEphemeris, toc),
    
      1204
            toc(toc),
    
      1204
            toe(toe),
    
      1204
            IODE(IODE),
    
      1204
            IODC(IODC),
    
      1204
            a(a),
    
      1204
            sqrt_A(sqrt_A),
    
      1204
            e(e),
    
      1204
            i_0(i_0),
    
      1204
            Omega_0(Omega_0),
    
      1204
            omega(omega),
    
      1204
            M_0(M_0),
    
      1204
            delta_n(delta_n),
    
      1204
            Omega_dot(Omega_dot),
    
      1204
            i_dot(i_dot),
    
      1204
            Cus(Cus),
    
      1204
            Cuc(Cuc),
    
      1204
            Cis(Cis),
    
      1204
            Cic(Cic),
    
      1204
            Crs(Crs),
    
      1204
            Crc(Crc),
    
      1204
            svAccuracy(svAccuracy),
    
      1204
            svHealth(svHealth),
    
      1204
            L2ChannelCodes(L2ChannelCodes),
    
      1204
            L2DataFlagPCode(L2DataFlagPCode),
    
      1204
            T_GD(T_GD),
    
      1204
            fitInterval(fitInterval) {}
    
      #ifdef TESTING
    
      8851
      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,
    
      8851
                                 double /* TransmissionTimeOfMessage */, double fitInterval, double /* spare1 */, double /* spare2 */)
    
        2/4✓ Branch 1 taken 8851 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8851 times.
✗ Branch 5 not taken.

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

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

      17702
            fitInterval(fitInterval)
    
      8851
      {}
    
      #endif
    
      175975
      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)
    
      175975
          const auto mu = InsConst::GPS::MU;
    
          // Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
    
      175975
          const auto F = InsConst::GPS::F;
    
          LOG_DATA("    toe {} [{}] (Time of ephemeris)", toe.toGPSweekTow(), toe.toYMDHMS(GPST));
    
      175975
          const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
    
          LOG_DATA("    A {} [m] (Semi-major axis)", A);
    
      175975
          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);
    
      175975
          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 175975 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 175975 times.
✗ Branch 6 not taken.

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

      527924
          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 351949 times.
✗ Branch 2 not taken.

      351949
              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 351948 times.
✗ Branch 2 not taken.

      351949
              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]
    
      351948
              auto M_k = M_0 + n * t_k;
    
              LOG_DATA("      M_k {} [s] (Mean anomaly)", M_k);
    
              // Eccentric anomaly [rad]
    
      351948
              double E_k = M_k;
    
      351948
              double E_k_old = 0.0;
    
        5/6✓ Branch 1 taken 1979109 times.
✓ Branch 2 taken 351942 times.
✓ Branch 3 taken 1979109 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1979108 times.
✓ Branch 6 taken 351943 times.

      2331056
              for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
    
              {
    
      1979108
                  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:
    
      1979108
                  E_k = M_k + e * sin(E_k);
    
              }
    
              // Relativistic correction term [s]
    
      351943
              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]
    
      351943
              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 235722 times.
✓ Branch 3 taken 116228 times.
✓ Branch 6 taken 116228 times.
✗ Branch 7 not taken.

      351948
              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
    
      351950
              clkDrift = a[1] + a[2] / 2.0 * t_minus_toc;
    
              // Correct transmit time for the satellite clock bias
    
        2/4✓ Branch 2 taken 351949 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 351949 times.
✗ Branch 6 not taken.

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

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

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

      181552
          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)
    
      181552
          const auto mu = InsConst::GPS::MU;
    
          // Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
    
      181552
          const auto Omega_e_dot = InsConst::GPS::omega_ie;
    
          LOG_DATA("    toe {} (Time of ephemeris)", toe.toGPSweekTow());
    
      181552
          const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
    
          LOG_DATA("    A {} [m] (Semi-major axis)", A);
    
      181552
          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);
    
      181551
          auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
    
          LOG_DATA("    n {} [rad/s] (Corrected mean motion)", n);
    
          // Eccentric anomaly [rad]
    
      181551
          double E_k = 0.0;
    
          // Time difference from ephemeris reference epoch [s]
    
        1/2✓ Branch 1 taken 181552 times.
✗ Branch 2 not taken.

      181551
          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]
    
      181553
          auto M_k = M_0 + n * t_k;
    
          LOG_DATA("    M_k {} [s] (Mean anomaly)", M_k);
    
      181553
          E_k = M_k; // Initial Value [rad]
    
      181553
          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 544665 times.
✓ Branch 2 taken 181555 times.
✓ Branch 3 taken 544665 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 544668 times.
✓ Branch 6 taken 181552 times.

      726221
          for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
    
          {
    
      544668
              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:
    
      544668
              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)
    
      181552
          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);
    
      181552
          auto Phi_k = v_k + omega; // Argument of Latitude [rad]
    
          LOG_DATA("    Phi_k {} [rad] (Argument of Latitude)", Phi_k);
    
          // Second Harmonic Perturbations
    
      181552
          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);
    
      181552
          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);
    
      181552
          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);
    
      181552
          auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad]
    
          LOG_DATA("    u_k {} [rad] (Corrected Argument of Latitude)", u_k);
    
      181552
          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);
    
      181552
          auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad]
    
          LOG_DATA("    i_k {} [rad] (Corrected Inclination)", i_k);
    
      181552
          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);
    
      181552
          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 181555 times.
✗ Branch 3 not taken.

      181552
          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]
    
      181555
          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]
    
      181555
          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]
    
      181555
          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 181555 times.
✗ Branch 2 not taken.

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

      181554
          if (calc & Calc_Velocity || calc & Calc_Acceleration)
    
          {
    
              // Eccentric Anomaly Rate [rad/s]
    
      173675
              auto E_k_dot = n / (1 - e * std::cos(E_k));
    
              // True Anomaly Rate [rad/s]
    
      173675
              auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k));
    
              // Corrected Inclination Angle Rate [rad/s]
    
      173675
              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]
    
      173675
              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]
    
      173675
              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]
    
      173675
              auto Omega_k_dot = Omega_dot - Omega_e_dot;
    
              // In-plane x velocity [m/s]
    
      173675
              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]
    
      173675
              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]
    
      173675
              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)
    
      173675
                          - 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]
    
      173675
              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)
    
      173675
                          - 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]
    
      173675
              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 173676 times.
✗ Branch 2 not taken.

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

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

      173676
              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 181555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181555 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 181555 times.
✗ Branch 8 not taken.

      363110
                   .e_accel = e_accel };
    
      }
    
      175949
      bool GPSEphemeris::isHealthy() const
    
      {
    
      175949
          return svHealth == 0;
    
      }
    
      68587
      double GPSEphemeris::calcSatellitePositionVariance() const
    
      {
    
      68587
          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		1204	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		1204	const double& fitInterval)
32			: SatNavData(SatNavData::GPSEphemeris, toc),
33		1204	toc(toc),
34		1204	toe(toe),
35		1204	IODE(IODE),
36		1204	IODC(IODC),
37		1204	a(a),
38		1204	sqrt_A(sqrt_A),
39		1204	e(e),
40		1204	i_0(i_0),
41		1204	Omega_0(Omega_0),
42		1204	omega(omega),
43		1204	M_0(M_0),
44		1204	delta_n(delta_n),
45		1204	Omega_dot(Omega_dot),
46		1204	i_dot(i_dot),
47		1204	Cus(Cus),
48		1204	Cuc(Cuc),
49		1204	Cis(Cis),
50		1204	Cic(Cic),
51		1204	Crs(Crs),
52		1204	Crc(Crc),
53		1204	svAccuracy(svAccuracy),
54		1204	svHealth(svHealth),
55		1204	L2ChannelCodes(L2ChannelCodes),
56		1204	L2DataFlagPCode(L2DataFlagPCode),
57		1204	T_GD(T_GD),
58		1204	fitInterval(fitInterval) {}
59
60			#ifdef TESTING
61
62		8851	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		8851	double /* TransmissionTimeOfMessage /, double fitInterval, double / spare1 /, double / spare2 */)
70	2/4 ✓ Branch 1 taken 8851 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 8851 times. ✗ Branch 5 not taken.	8851	: SatNavData(SatNavData::GPSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(GPS).getTimeSystem())),
71		8851	toc(refTime),
72	2/4 ✓ Branch 1 taken 8851 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 8851 times. ✗ Branch 5 not taken.	8851	toe(InsTime(0, static_cast<int32_t>(GPSWeek), Toe, SatelliteSystem(GPS).getTimeSystem())),
73		8851	IODE(static_cast<size_t>(IODE)),
74		8851	IODC(static_cast<size_t>(IODC)),
75		8851	a({ svClockBias, svClockDrift, svClockDriftRate }),
76		8851	sqrt_A(sqrt_A),
77		8851	e(e),
78		8851	i_0(i_0),
79		8851	Omega_0(Omega_0),
80		8851	omega(omega),
81		8851	M_0(M_0),
82		8851	delta_n(delta_n),
83		8851	Omega_dot(Omega_dot),
84		8851	i_dot(i_dot),
85		8851	Cus(Cus),
86		8851	Cuc(Cuc),
87		8851	Cis(Cis),
88		8851	Cic(Cic),
89		8851	Crs(Crs),
90		8851	Crc(Crc),
91		8851	svAccuracy(svAccuracy),
92		8851	svHealth(static_cast<uint8_t>(svHealth)),
93		8851	L2ChannelCodes(static_cast<uint8_t>(L2ChannelCodes)),
94		8851	L2DataFlagPCode(static_cast<bool>(L2DataFlagPCode)),
95		8851	T_GD(T_GD),
96	1/2 ✓ Branch 2 taken 8851 times. ✗ Branch 3 not taken.	17702	fitInterval(fitInterval)
97		8851	{}
98
99			#endif
100
101		175975	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		175975	const auto mu = InsConst::GPS::MU;
106			// Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
107		175975	const auto F = InsConst::GPS::F;
108
109			LOG_DATA(" toe {} [{}] (Time of ephemeris)", toe.toGPSweekTow(), toe.toYMDHMS(GPST));
110
111		175975	const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
112			LOG_DATA(" A {} [m] (Semi-major axis)", A);
113		175975	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		175975	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 175975 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 175975 times. ✗ Branch 6 not taken.	175975	InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C);
120
121		175975	InsTime transTime = transTime0;
122			LOG_DATA(" Iterating Time at transmission");
123		175975	double dt_sv = 0.0;
124		175975	double clkDrift = 0.0;
125
126	2/2 ✓ Branch 0 taken 351949 times. ✓ Branch 1 taken 175975 times.	527924	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 351949 times. ✗ Branch 2 not taken.	351949	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 351948 times. ✗ Branch 2 not taken.	351949	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		351948	auto M_k = M_0 + n * t_k;
141			LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
142
143			// Eccentric anomaly [rad]
144		351948	double E_k = M_k;
145		351948	double E_k_old = 0.0;
146
147	5/6 ✓ Branch 1 taken 1979109 times. ✓ Branch 2 taken 351942 times. ✓ Branch 3 taken 1979109 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 1979108 times. ✓ Branch 6 taken 351943 times.	2331056	for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
148			{
149		1979108	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		1979108	E_k = M_k + e * sin(E_k);
151			}
152
153			// Relativistic correction term [s]
154		351943	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		351943	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 235722 times. ✓ Branch 3 taken 116228 times. ✓ Branch 6 taken 116228 times. ✗ Branch 7 not taken.	351948	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		351950	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 351949 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 351949 times. ✗ Branch 6 not taken.	351950	transTime = transTime0 - std::chrono::duration<double>(dt_sv);
171			}
172			LOG_DATA(" transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST));
173
174		175975	return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift };
175			}
176
177		181554	Orbit::PosVelAccel GPSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
178			{
179	2/4 ✓ Branch 1 taken 181555 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 181554 times. ✗ Branch 5 not taken.	181554	Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
180	2/4 ✓ Branch 1 taken 181554 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 181552 times. ✗ Branch 5 not taken.	181554	Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
181	2/4 ✓ Branch 1 taken 181552 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 181552 times. ✗ Branch 5 not taken.	181552	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		181552	const auto mu = InsConst::GPS::MU;
186			// Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
187		181552	const auto Omega_e_dot = InsConst::GPS::omega_ie;
188
189			LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow());
190
191		181552	const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
192			LOG_DATA(" A {} [m] (Semi-major axis)", A);
193		181552	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		181551	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		181551	double E_k = 0.0;
200
201			// Time difference from ephemeris reference epoch [s]
202	1/2 ✓ Branch 1 taken 181552 times. ✗ Branch 2 not taken.	181551	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		181553	auto M_k = M_0 + n * t_k;
207			LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
208
209		181553	E_k = M_k; // Initial Value [rad]
210		181553	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 544665 times. ✓ Branch 2 taken 181555 times. ✓ Branch 3 taken 544665 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 544668 times. ✓ Branch 6 taken 181552 times.	726221	for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
214			{
215		544668	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		544668	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		181552	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		181552	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		181552	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		181552	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		181552	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		181552	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		181552	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		181552	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		181552	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		181552	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 181555 times. ✗ Branch 3 not taken.	181552	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		181555	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		181555	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		181555	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 181555 times. ✗ Branch 2 not taken.	181555	e_pos = Eigen::Vector3d{ x_k, y_k, z_k };
262
263	5/6 ✓ Branch 1 taken 7879 times. ✓ Branch 2 taken 173675 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 7879 times. ✓ Branch 6 taken 173675 times. ✓ Branch 7 taken 7879 times.	181554	if (calc & Calc_Velocity \|\| calc & Calc_Acceleration)
264			{
265			// Eccentric Anomaly Rate [rad/s]
266		173675	auto E_k_dot = n / (1 - e * std::cos(E_k));
267			// True Anomaly Rate [rad/s]
268		173675	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		173675	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		173675	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		173675	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		173675	auto Omega_k_dot = Omega_dot - Omega_e_dot;
277			// In-plane x velocity [m/s]
278		173675	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		173675	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		173675	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		173675	- 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		173675	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		173675	- 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		173675	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 173676 times. ✗ Branch 2 not taken.	173675	if (calc & Calc_Velocity)
291			{
292	1/2 ✓ Branch 1 taken 173676 times. ✗ Branch 2 not taken.	173676	e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k };
293			}
294
295	2/2 ✓ Branch 1 taken 2790 times. ✓ Branch 2 taken 170886 times.	173676	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 181555 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 181555 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 181555 times. ✗ Branch 8 not taken.	363110	.e_accel = e_accel };
315			}
316
317		175949	bool GPSEphemeris::isHealthy() const
318			{
319		175949	return svHealth == 0;
320			}
321
322		68587	double GPSEphemeris::calcSatellitePositionVariance() const
323			{
324		68587	return std::pow(svAccuracy, 2);
325			}
326
327			} // namespace NAV
328