INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/GNSS/Satellite/Ephemeris/GPSEphemeris.cpp
Date:	2025-02-07 16:54:41

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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