gps_pvt 0.5.1 → 0.6.2

data/ext/ninja-scan-light/tool/navigation/GLONASS.h

@@ -998,17 +998,18 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
           bool is_equivalent(const Ephemeris_with_Time &eph) const {
             return Ephemeris::is_equivalent(eph) && TimeProperties::is_equivalent(eph);
           }
-          float_t calculate_clock_error(float_t delta_t, const float_t &pseudo_range = 0) const {
-            delta_t -= pseudo_range / light_speed;
+          float_t calculate_clock_error(const float_t &delta_t) const {
             return -TimeProperties::tau_c - Ephemeris::tau_n + Ephemeris::gamma_n * delta_t;
           }
           /**
-           * @param t_arrival_onboard signal arrival time in onboard time scale,
+           * @param t_depature_onboard signal depature time in onboard time scale,
            * which is along to glonass time scale (UTC + 3 hr) but is shifted in gradually changing length.
            */
-          float_t clock_error(
-              const float_t &t_arrival_onboard, const float_t &pseudo_range = 0) const {
-            return calculate_clock_error(t_arrival_onboard - Ephemeris::t_b, pseudo_range);
+          float_t clock_error(const float_t &t_depature_onboard) const {
+            return calculate_clock_error(t_depature_onboard - Ephemeris::t_b);
+          }
+          const float_t &clock_error_dot() const {
+            return Ephemeris::gamma_n;
           }
           /**
            * Calculate absolute constellation(t) based on constellation(t_0).
@@ -1064,23 +1065,22 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
            * @param pseudo_range measured pusedo_range to correct delta_t, default is 0.
            */
           constellation_t calculate_constellation(
-              const float_t &delta_t, const float_t &pseudo_range = 0) const {
+              const float_t &delta_t, const float_t &dt_transit = 0) const {
 
-            float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed);
             constellation_t res(
-                constellation_abs(delta_t_to_transmit, xa_t_b, float_t(0)));
+                constellation_abs(delta_t, xa_t_b, float_t(0)));
 
             return res.rel_corrdinate(
-                sidereal_t_b_rad + (constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02
+                sidereal_t_b_rad + (constellation_t::omega_E * (delta_t + dt_transit))); // transform from abs to PZ-90.02
           }
           /**
-           * @param t_arrival_glonass signal arrival time in glonass time scale,
+           * @param t_depature_glonass signal depature time in glonass time scale,
            * which is the corrected onboard time by removing clock error.
            */
           constellation_t constellation(
-              const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const {
+              const float_t &t_depature_glonass, const float_t &dt_transit = 0) const {
             return calculate_constellation(
-                t_arrival_glonass - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale
+                t_depature_glonass - Ephemeris::t_b, dt_transit); // measure in UTC + 3hr scale
           }
         };
 
@@ -1107,14 +1107,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
             return std::abs(t_b_gps.interval(t)) <= 60 * 60; // 1 hour
           }
           using Ephemeris_with_Time::clock_error;
-          float_t clock_error(
-              const GPS_Time<float_t> &t_arrival_onboard, const float_t &pseudo_range = 0) const {
-            return Ephemeris_with_Time::calculate_clock_error(t_arrival_onboard - t_b_gps, pseudo_range);
+          float_t clock_error(const GPS_Time<float_t> &t_depature_onboard) const {
+            return Ephemeris_with_Time::calculate_clock_error(t_depature_onboard - t_b_gps);
           }
           using Ephemeris_with_Time::constellation;
           typename Ephemeris_with_Time::constellation_t constellation(
-              const GPS_Time<float_t> &t_arrival_gps, const float_t &pseudo_range = 0) const {
-            return this->calculate_constellation(t_arrival_gps - t_b_gps, pseudo_range);
+              const GPS_Time<float_t> &t_depature_gps, const float_t &dt_transit = 0) const {
+            return this->calculate_constellation(t_depature_gps - t_b_gps, dt_transit);
           }
         };
     };
@@ -1130,25 +1129,25 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
           eph_cached_t(const eph_t &eph) : eph_t(eph), xa_t_0(eph.xa_t_b), t_0_from_t_b(0) {}
           using eph_t::constellation;
           typename eph_t::constellation_t constellation(
-              const GPS_Time<float_t> &t_arrival_gps, const float_t &pseudo_range = 0) const {
-            float_t delta_t(t_arrival_gps - eph_t::t_b_gps);
-            float_t delta_t_transmit_from_t_0(delta_t - pseudo_range / light_speed - t_0_from_t_b);
+              const GPS_Time<float_t> &t_depature_gps, const float_t &dt_transit = 0) const {
+            float_t delta_t(t_depature_gps - eph_t::t_b_gps);
+            float_t delta_t_depature_from_t_0(delta_t - t_0_from_t_b);
 
-            float_t t_step_max(delta_t_transmit_from_t_0 >= 0
+            float_t t_step_max(delta_t_depature_from_t_0 >= 0
                 ? eph_t::time_step_max_per_one_integration
                 : -eph_t::time_step_max_per_one_integration);
 
-            int big_steps(std::floor(delta_t_transmit_from_t_0 / t_step_max));
+            int big_steps(std::floor(delta_t_depature_from_t_0 / t_step_max));
             if(big_steps > 0){ // perform integration and update cache
               xa_t_0 = eph_t::constellation_abs(t_step_max, big_steps, xa_t_0, t_0_from_t_b);
               float_t delta_t_updated(t_step_max * big_steps);
               t_0_from_t_b += delta_t_updated;
-              delta_t_transmit_from_t_0 -= delta_t_updated;
+              delta_t_depature_from_t_0 -= delta_t_updated;
             }
 
-            return eph_t::constellation_abs(delta_t_transmit_from_t_0, 1, xa_t_0, t_0_from_t_b)
-                .rel_corrdinate(
-                  eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02
+            return eph_t::constellation_abs(delta_t_depature_from_t_0, 1, xa_t_0, t_0_from_t_b)
+                .rel_corrdinate( // transform from abs to PZ-90.02
+                  eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * (delta_t + dt_transit)));
           }
         };
 
@@ -1190,22 +1189,25 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
           return eph_history.select(target_time, &eph_t::is_valid);
         }
 
-        float_t clock_error(const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const{
-          return ephemeris().clock_error(t, pseudo_range);
+        float_t clock_error(const GPS_Time<float_t> &t_tx) const{
+          return ephemeris().clock_error(t_tx);
+        }
+        float_t clock_error_dot(const GPS_Time<float_t> &t_tx) const{
+          return ephemeris().clock_error_dot();
         }
 
         typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t constellation(
-            const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const {
+            const GPS_Time<float_t> &t_tx, const float_t &dt_transit = 0) const {
           return (typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t)(
-              eph_history.current().constellation(t, pseudo_range));
+              eph_history.current().constellation(t_tx, dt_transit));
         }
 
-        typename GPS_SpaceNode<float_t>::xyz_t position(const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const {
-          return constellation(t, pseudo_range).position;
+        typename GPS_SpaceNode<float_t>::xyz_t position(const GPS_Time<float_t> &t_tx, const float_t &dt_transit= 0) const {
+          return constellation(t_tx, dt_transit).position;
         }
 
-        typename GPS_SpaceNode<float_t>::xyz_t velocity(const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const {
-          return constellation(t, pseudo_range).velocity;
+        typename GPS_SpaceNode<float_t>::xyz_t velocity(const GPS_Time<float_t> &t_tx, const float_t &dt_transit = 0) const {
+          return constellation(t_tx, dt_transit).velocity;
         }
     };
   public:

data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h

@@ -119,18 +119,17 @@ class GLONASS_SinglePositioning : public SolverBaseT {
           static inline const typename space_node_t::Satellite &sat(const void *ptr) {
             return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
           }
-          static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).constellation(t, pseudo_range).position;
+          static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).constellation(t_tx, dt_transit).position;
           }
-          static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).constellation(t, pseudo_range).velocity;
+          static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).constellation(t_tx, dt_transit).velocity;
           }
-          static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).clock_error(t, pseudo_range);
+          static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).clock_error(t_tx);
           }
-          static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            // Clock rate error is already taken into account in constellation()
-            return 0;
+          static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).clock_error_dot(t_tx);
           }
         };
         satellite_t res = {
@@ -250,16 +249,20 @@ class GLONASS_SinglePositioning : public SolverBaseT {
 
       range -= receiver_error;
 
+      static const float_t &c(space_node_t::light_speed);
+
       // Clock correction, which will be considered in the next constellation()
       // as extra transmission time by using extra psuedo range.
       if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){
-        range += (sat.clock_error(time_arrival, range) * space_node_t::light_speed);
+        range += (sat.clock_error(time_arrival - range / c) * c);
       }else{
         range += range_error.value[range_error_t::SATELLITE_CLOCK];
       }
 
       // Calculate satellite position
-      xyz_t sat_pos(sat.position(time_arrival, range));
+      float_t dt_transit(range / c);
+      gps_time_t t_tx(time_arrival - dt_transit);
+      xyz_t sat_pos(sat.position(t_tx, dt_transit));
       float_t geometric_range(usr_pos.xyz.dist(sat_pos));
 
       // Calculate residual
@@ -307,12 +310,12 @@ class GLONASS_SinglePositioning : public SolverBaseT {
 
       res.range_corrected = range;
 
-      xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
+      xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
 
       res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
           + res.los_neg[1] * rel_vel.y()
           + res.los_neg[2] * rel_vel.z()
-          + sat.clock_error_dot(time_arrival, range) * space_node_t::light_speed;
+          + sat.clock_error_dot(t_tx) * c;
 
       return res;
     }

data/ext/ninja-scan-light/tool/navigation/GPS.h

@@ -452,7 +452,7 @@ struct GPS_Time {
     struct {
       int week;
       float_t seconds;
-    } uncorrected; // to work around of "incomplete type" error within g++
+    } uncorrected, corrected; // to work around of "incomplete type" error within g++
     leap_second_event_t(
         const int &year, const int &month, const int &day,
         const int &leap)
@@ -462,9 +462,11 @@ struct GPS_Time {
       t.tm_year = tm_year;
       t.tm_mon = tm_mon;
       t.tm_mday = tm_mday;
-      GPS_Time t_gps(t);
-      uncorrected.week = t_gps.week;
-      uncorrected.seconds = t_gps.seconds;
+      GPS_Time t_gps_uc(t), t_gps(t_gps_uc + leap);
+      uncorrected.week = t_gps_uc.week;
+      uncorrected.seconds = t_gps_uc.seconds;
+      corrected.week = t_gps.week;
+      corrected.seconds = t_gps.seconds;
     }
   };
   static const leap_second_event_t leap_second_events[];
@@ -484,6 +486,92 @@ struct GPS_Time {
     }
     return 0;
   }
+
+  int leap_seconds() const {
+    // Treat *this as (normal) GPS time, to which leap seconds are added when it is converted from std::tm
+    for(const leap_second_event_t *i(&leap_second_events[0]); i->leap_seconds > 0; ++i){
+      if(*this >= GPS_Time(i->corrected.week, i->corrected.seconds)){return i->leap_seconds;}
+    }
+    return 0;
+  }
+  float_t julian_date() const {
+    struct conv_t {
+      static int ymd2jd(const int &year, const int &month, const int &day){
+        // @see https://en.wikipedia.org/wiki/Julian_day#Converting_Gregorian_calendar_date_to_Julian_Day_Number
+        return std::div(1461 * (year + 4800 + std::div(month - 14, 12).quot), 4).quot
+            + std::div(367 * (month - 2 - 12 * std::div((month - 14), 12).quot), 12).quot
+            - std::div(3 * std::div((year + 4900 + std::div(month - 14, 12).quot), 100).quot, 4).quot
+            + day - 32075;
+      }
+    };
+    // origin of Julian day is "noon (not midnight)" BC4713/1/1
+    static const float_t t0(conv_t::ymd2jd(1980, 1, 6) - 0.5);
+    // GPS Time is advanced by leap seconds compared with UTC.
+    // The following calculation is incorrect for a day in which a leap second is inserted or truncated.
+    // @see https://en.wikipedia.org/wiki/Julian_day#Julian_date_calculation
+    return t0 + week * 7 + (seconds - leap_seconds()) / seconds_day;
+  }
+  float_t julian_date_2000() const {
+    static const std::tm tm_2000 = {0, 0, 12, 1, 0, 2000 - 1900};
+    static const float_t jd2000(GPS_Time(tm_2000, 13).julian_date());
+    return julian_date() - jd2000;
+  }
+  std::tm utc() const {return c_tm(leap_seconds());}
+
+  float_t greenwich_mean_sidereal_time_sec_ires1996(const float_t &delta_ut1 = float_t(0)) const {
+    float_t jd2000(julian_date_2000() + delta_ut1 / seconds_day);
+    float_t jd2000_day(float_t(0.5) + std::floor(jd2000 - 0.5)); // +/-0.5, +/-1.5, ...
+
+    // @see Chapter 2 of Orbits(978-3540785217) by Xu Guochang
+    // @see Chapter 5 of IERS Conventions (1996) https://www.iers.org/IERS/EN/Publications/TechnicalNotes/tn21.html
+    float_t jc(jd2000_day / 36525), // Julian century
+        jc2(std::pow(jc, 2)), jc3(std::pow(jc, 3));
+    float_t gmst0(24110.54841 // = 6h 41m 50.54841s
+        + jc * 8640184.812866
+        + jc2 * 0.093104
+        - jc3 * 6.2E-6);
+    // ratio of universal to sidereal time as given by Aoki et al. (1982)
+    float_t r(1.002737909350795 // 7.2921158553E-5 = 1.002737909350795 / 86400 * 2pi
+        + jc * 5.9006E-11 // 4.3E-15 = 5.9E-11 / 86400 * 2pi
+        - jc2 * 5.9E-15); // in seconds
+    return gmst0 + r * (jd2000 - jd2000_day) * seconds_day;
+  }
+
+  /**
+   * ERA (Earth rotation rate) defined in Sec. 5.5.3 in IERS 2010(Technical Note No.36)
+   */
+  float_t earth_rotation_angle(const float_t &delta_ut1 = float_t(0),
+      const float_t &scale_factor = float_t(M_PI * 2)) const {
+    float_t jd2000(julian_date_2000() + delta_ut1 / seconds_day);
+    return (jd2000 * 1.00273781191135448 + 0.7790572732640) * scale_factor; // Eq.(5.14)
+  }
+
+  float_t greenwich_mean_sidereal_time_sec_ires2010(const float_t &delta_ut1 = float_t(0)) const {
+    float_t era(earth_rotation_angle(delta_ut1, seconds_day));
+
+    float_t t(julian_date_2000() / 36525);
+    // @see Eq.(5.32) Chapter 5 of IERS Conventions (2010)
+#define AS2SEC(as) (((float_t)seconds_day / (360 * 3600)) * (as))
+    return era
+        + AS2SEC(0.014506)
+        + t * AS2SEC(4612.156534)
+        + std::pow(t, 2) * AS2SEC(1.3915817)
+        - std::pow(t, 3) * AS2SEC(0.00000044)
+        - std::pow(t, 4) * AS2SEC(0.000029956)
+        - std::pow(t, 5) * AS2SEC(0.0000000368);
+#undef AS2SEC
+  }
+
+  /**
+   * Calculate Greenwich mean sidereal time (GMST) in seconds
+   * Internally, greenwich_mean_sidereal_time_sec_ired2010() is called.
+   * @param delta_ut1 time difference of UTC and UT1; UT1 = UTC + delta_UT1,
+   * @return GMST in seconds. 86400 seconds correspond to one rotation
+   * @see greenwich_mean_sidereal_time_sec_ires2010()
+   */
+  float_t greenwich_mean_sidereal_time_sec(const float_t &delta_ut1 = float_t(0)) const {
+    return greenwich_mean_sidereal_time_sec_ires2010(delta_ut1);
+  }
 };
 
 template <class FloatT>
@@ -1025,16 +1113,14 @@ static s ## bits ## _t name(const InputT *buf){ \
            * Calculate correction value in accordance with clock error model
            *
            * @param t current time
-           * @param pseudo_range pseudo range in meters
            * @param gamma factor for compensation of group delay
            * L1 = 1, L2 = (77/60)^2, see ICD 20.3.3.3.3.2 L1 - L2 Correction
            * @return in seconds
            */
-          float_t clock_error(const gps_time_t &t, const float_t &pseudo_range = 0,
+          float_t clock_error(const gps_time_t &t,
               const float_t &gamma = 1) const{
 
-            float_t transit_time(pseudo_range / light_speed);
-            float_t tk(period_from_time_of_clock(t) - transit_time);
+            float_t tk(period_from_time_of_clock(t));
             float_t Ek(eccentric_anomaly(tk));
 
             // Relativistic correction term
@@ -1046,10 +1132,9 @@ static s ## bits ## _t name(const InputT *buf){ \
             return dt_sv - (gamma * t_GD);
           }
 
-          float_t clock_error_dot(const gps_time_t &t, const float_t &pseudo_range = 0) const {
+          float_t clock_error_dot(const gps_time_t &t) const {
 
-            float_t transit_time(pseudo_range / light_speed);
-            float_t tk(period_from_time_of_clock(t) - transit_time);
+            float_t tk(period_from_time_of_clock(t));
             float_t Ek(eccentric_anomaly(tk));
             float_t Ek_dot(eccentric_anomaly_dot(Ek));
 
@@ -1062,17 +1147,24 @@ static s ## bits ## _t name(const InputT *buf){ \
             return dt_sv_dot;
           }
 
+          /**
+           * Return satellite position and velocity at the transmission time in EFEC.
+           * @param t_tx transmission time
+           * @param dt_transit Transit time. default is zero.
+           * If zero, the returned value is along with the ECEF at the transmission time.
+           * Otherwise (non-zero), they are along with the ECEF at the reception time,
+           * that is, the transmission time added by the transit time.
+           * @param with_velocity If true, velocity calculation is performed,
+           * otherwise invalid velocity is returned.
+           */
           constellation_t constellation(
-              const gps_time_t &t, const float_t &pseudo_range = 0,
+              const gps_time_t &t_tx, const float_t &dt_transit = 0,
               const bool &with_velocity = true) const {
 
             constellation_t res;
 
             // Time from ephemeris reference epoch (tk)
-            float_t tk0(period_from_time_of_ephemeris(t));
-
-            // Remove transit time
-            float_t tk(tk0 - pseudo_range / light_speed);
+            float_t tk(period_from_time_of_ephemeris(t_tx));
 
             // Eccentric Anomaly (Ek)
             float_t Ek(eccentric_anomaly(tk));
@@ -1119,12 +1211,12 @@ static s ## bits ## _t name(const InputT *buf){ \
             float_t Omegak(Omega0);
             if(false){ // __MISUNDERSTANDING_ABOUT_OMEGA0_CORRECTION__
               Omegak += (
-                  (dot_Omega0 - WGS84::Omega_Earth_IAU) * tk0
+                  (dot_Omega0 - WGS84::Omega_Earth_IAU) * tk
                   - WGS84::Omega_Earth_IAU * t_oe);
             }else{
               Omegak += (
-                  dot_Omega0 * tk                            // corrected with the time when the wave is transmitted
-                  - WGS84::Omega_Earth_IAU * (t_oe + tk0));  // corrected with the time when the wave is received
+                  dot_Omega0 * tk // correction (at the transmission)
+                  - WGS84::Omega_Earth_IAU * (t_oe + tk + dt_transit)); // correction and coordinate transformation
             }
 
             float_t Omegak_sin(sin(Omegak)),
@@ -1925,26 +2017,26 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
               &eph_t::period_from_time_of_clock) || is_valid;
         }
 
-        float_t clock_error(const gps_time_t &t, const float_t &pseudo_range = 0) const{
-          return ephemeris().clock_error(t, pseudo_range);
+        float_t clock_error(const gps_time_t &t_tx) const{
+          return ephemeris().clock_error(t_tx);
         }
 
-        float_t clock_error_dot(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-          return ephemeris().clock_error_dot(t, pseudo_range);
+        float_t clock_error_dot(const gps_time_t &t_tx) const {
+          return ephemeris().clock_error_dot(t_tx);
         }
 
         typename SatelliteProperties::constellation_t constellation(
-            const gps_time_t &t, const float_t &pseudo_range = 0,
+            const gps_time_t &t_tx, const float_t &dt_transit = 0,
             const bool &with_velocity = true) const {
-          return ephemeris().constellation(t, pseudo_range, with_velocity);
+          return ephemeris().constellation(t_tx, dt_transit, with_velocity);
         }
         
-        xyz_t position(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-          return constellation(t, pseudo_range, false).position;
+        xyz_t position(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
+          return constellation(t_tx, dt_transit, false).position;
         }
         
-        xyz_t velocity(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-          return constellation(t, pseudo_range, true).velocity;
+        xyz_t velocity(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
+          return constellation(t_tx, dt_transit, true).velocity;
         }
     };
   public:

data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h

@@ -143,17 +143,17 @@ class GPS_SinglePositioning : public SolverBaseT {
           static inline const typename space_node_t::Satellite &sat(const void *ptr) {
             return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
           }
-          static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).position(t, pseudo_range);
+          static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).position(t_tx, dt_transit);
           }
-          static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).velocity(t, pseudo_range);
+          static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).velocity(t_tx, dt_transit);
           }
-          static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).clock_error(t, pseudo_range);
+          static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).clock_error(t_tx);
           }
-          static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).clock_error_dot(t, pseudo_range);
+          static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).clock_error_dot(t_tx);
           }
         };
         satellite_t res = {
@@ -272,13 +272,16 @@ class GPS_SinglePositioning : public SolverBaseT {
         residual_t &residual,
         const range_error_t &error = range_error_t::not_corrected) const {
 
+      static const float_t &c(space_node_t::light_speed);
+
       // Clock error correction
       range += ((error.unknown_flag & range_error_t::SATELLITE_CLOCK)
-          ? (sat.clock_error(time_arrival, range) * space_node_t::light_speed)
+          ? (sat.clock_error(time_arrival - range / c) * c)
           : error.value[range_error_t::SATELLITE_CLOCK]);
 
       // Calculate satellite position
-      xyz_t sat_pos(sat.position(time_arrival, range));
+      float_t dt_transit(range / c);
+      xyz_t sat_pos(sat.position(time_arrival - dt_transit, dt_transit));
       float_t geometric_range(usr_pos.xyz.dist(sat_pos));
 
       // Calculate residual
@@ -344,11 +347,15 @@ class GPS_SinglePositioning : public SolverBaseT {
         const xyz_t &usr_vel,
         const float_t &los_neg_x, const float_t &los_neg_y, const float_t &los_neg_z) const {
 
-      xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
+      static const float_t &c(space_node_t::light_speed);
+
+      float_t dt_transit(range / c);
+      gps_time_t t_tx(time_arrival - dt_transit);
+      xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
       return los_neg_x * rel_vel.x()
           + los_neg_y * rel_vel.y()
           + los_neg_z * rel_vel.z()
-          + sat.clock_error_dot(time_arrival, range) * space_node_t::light_speed; // considering clock rate error
+          + sat.clock_error_dot(t_tx) * c; // considering clock error rate
     }
 
     /**

data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h

@@ -230,25 +230,53 @@ struct GPS_Solver_Base {
     const void *impl;
     xyz_t (*impl_position)(const void *, const gps_time_t &, const float_t &);
     xyz_t (*impl_velocity)(const void *, const gps_time_t &, const float_t &);
-    float_t (*impl_clock_error)(const void *, const gps_time_t &, const float_t &);
-    float_t (*impl_clock_error_dot)(const void *, const gps_time_t &, const float_t &);
+    float_t (*impl_clock_error)(const void *, const gps_time_t &);
+    float_t (*impl_clock_error_dot)(const void *, const gps_time_t &);
     inline bool is_available() const {
       return impl != NULL;
     }
-    inline xyz_t position(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-      return impl_position(impl, t, pseudo_range);
+    /**
+     * Return satellite position at the transmission time in EFEC.
+     * @param t_tx transmission time
+     * @param dt_transit Transit time. default is zero.
+     * If zero, the returned value is along with the ECEF at the transmission time.
+     * Otherwise (non-zero), they are along with the ECEF at the reception time,
+     * that is, the transmission time added by the transit time.
+     */
+    inline xyz_t position(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
+      return impl_position(impl, t_tx, dt_transit);
     }
-    inline xyz_t velocity(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-      return impl_velocity(impl, t, pseudo_range);
+    /**
+     * Return satellite velocity at the transmission time in EFEC.
+     * @see position
+     */
+    inline xyz_t velocity(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
+      return impl_velocity(impl, t_tx, dt_transit);
     }
-    inline float_t clock_error(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-      return impl_clock_error(impl, t, pseudo_range);
+    /**
+     * Return satellite clock error [s] at the transmission time.
+     * @param t_tx transmission time
+     */
+    inline float_t clock_error(const gps_time_t &t_tx) const {
+      return impl_clock_error(impl, t_tx);
     }
-    inline float_t clock_error_dot(const gps_time_t &t, const float_t &pseudo_range = 0) const {
-      return impl_clock_error_dot(impl, t, pseudo_range);
+    /**
+     * Return satellite clock error derivative [s/s] at the transmission time.
+     * @param t_tx transmission time
+     */
+    inline float_t clock_error_dot(const gps_time_t &t_tx) const {
+      return impl_clock_error_dot(impl, t_tx);
     }
     static const satellite_t &unavailable() {
-      static const satellite_t res = {NULL};
+      struct impl_t {
+        static xyz_t v3(const void *, const gps_time_t &, const float_t &){
+          return xyz_t(0, 0, 0);
+        }
+        static float_t v(const void *, const gps_time_t &){
+          return float_t(0);
+        }
+      };
+      static const satellite_t res = {NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v};
       return res;
     }
   };