gps_pvt 0.6.1 → 0.6.4

data/ext/ninja-scan-light/tool/algorithm/interpolate.h

@@ -34,6 +34,9 @@
 
 #include <cstddef>
 #include <vector>
+#include <map>
+#include <algorithm>
+#include <stdexcept>
 
 /*
  * perform Neville's interpolation (with derivative)
@@ -118,4 +121,107 @@ Ty interpolate_Neville(
   return interpolate_Neville(x_given, y_given, x, y_buf, n)[0];
 }
 
+template <class Tx, class Ty, class Tx_delta = Tx>
+struct InterpolatableSet {
+  struct condition_t {
+    std::size_t max_x_size; ///< maximum number of data used for interpolation
+    Tx_delta max_dx_range; ///< maximum acceptable absolute difference between x and x0
+  };
+
+  typedef typename std::vector<std::pair<Tx, Ty> > buffer_t;
+  buffer_t buf;
+  Tx x0, x_lower, x_upper;
+  std::vector<Tx_delta> dx;
+  bool ready;
+
+  InterpolatableSet() : x0(), dx(), ready(false) {}
+
+  /**
+   * update interpolation source
+   * @param force_update If true, update is forcibly performed irrespective of current state
+   * @param cnd condition for source data selection
+   */
+  InterpolatableSet &update(
+      const Tx &x, const std::map<Tx, Ty> &xy_table,
+      const condition_t &cnd,
+      const bool &force_update = false){
+
+    do{
+      if(force_update){break;}
+      if(dx.size() <= 2){break;}
+      if(x < x_lower){break;}
+      if(x > x_upper){break;}
+      return *this;
+    }while(false);
+
+    // If the 1st and 2nd nearest items are changed, then recalculate interpolation targets.
+    struct {
+      const Tx &x_base;
+      bool operator()(
+          const typename std::map<Tx, Ty>::value_type &rhs,
+          const typename std::map<Tx, Ty>::value_type &lhs) const {
+        return std::abs(rhs.first - x_base) < std::abs(lhs.first - x_base);
+      }
+    } cmp = {(x0 = x)};
+
+    buf.resize(cnd.max_x_size);
+    dx.clear();
+    // extract x where x0-dx_range <= x <= x0+dx_range, then sort by ascending order of |x-x0|
+    for(typename buffer_t::const_iterator
+          it(buf.begin()),
+          it_end(std::partial_sort_copy(
+            xy_table.lower_bound(x - cnd.max_dx_range),
+            xy_table.upper_bound(x + cnd.max_dx_range),
+            buf.begin(), buf.end(), cmp));
+        it != it_end; ++it){
+      dx.push_back(it->first - x0);
+    }
+    if(dx.size() >= 2){ // calculate a necessary condition to update samples for new x
+      // According to Nth nearest points, x range in which the order of extracted samples
+      // is retained can be calculated.
+      bool ascending(buf[0].first <= buf[1].first);
+      Tx &xa(ascending ? x_lower : x_upper), &xb(ascending ? x_upper : x_lower);
+      xa = x0;
+      for(std::size_t i(2); i < buf.size(); ++i){
+        if(ascending ? (dx[1] <= dx[i]) : (dx[1] >= dx[i])){continue;}
+        xa = x0 + (dx[1] + dx[i]) / 2;
+        break;
+      }
+      xb = x0 + (dx[0] + dx[1]) / 2;
+    }
+    ready = (dx.size() >= cnd.max_x_size);
+
+    return *this;
+  }
+
+  template <class Ty2, class Ty2_Iterator>
+  Ty2 interpolate2(
+      const Tx &x, const Ty2_Iterator &y,
+      Ty2 *derivative = NULL) const {
+    int order(dx.size() - 1);
+    do{
+      if(order > 0){break;}
+      if((order == 0) && (!derivative)){return y[0];}
+      throw std::range_error("Insufficient records for interpolation");
+    }while(false);
+    std::vector<Ty2> y_buf(order), dy_buf(order);
+    interpolate_Neville(
+        dx, y, x - x0, y_buf, order,
+        &dy_buf, derivative ? 1 : 0);
+    if(derivative){*derivative = dy_buf[0];}
+    return y_buf[0];
+  }
+
+  Ty interpolate(const Tx &x, Ty *derivative = NULL) const {
+    struct second_iterator : public buffer_t::const_iterator {
+      second_iterator(const typename buffer_t::const_iterator &it)
+          : buffer_t::const_iterator(it) {}
+      const Ty &operator[](const int &n) const {
+        return buffer_t::const_iterator::operator[](n).second;
+      }
+    } y_it(buf.begin());
+    return interpolate2(x, y_it, derivative);
+  }
+};
+
 #endif /* __INTERPOLATE_H__ */

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

@@ -1113,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
@@ -1134,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));
 
@@ -1150,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));
@@ -1207,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)),
@@ -2013,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,29 +230,49 @@ 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() {
       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 &, const float_t &){
+        static float_t v(const void *, const gps_time_t &){
           return float_t(0);
         }
       };

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

@@ -1768,30 +1768,38 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
             return (delta_t < 0) || (delta_t > Timing::values[Timing::GEO_NAVIGATION_DATA].interval);
           }
 
-          constellation_t constellation(
-              const gps_time_t &t_rx, const float_t &pseudo_range = 0,
-              const bool &with_velocity = true) const {
+          float_t clock_error(const gps_time_t &t) const {
+            float_t t_G(-t.interval(WN, 0)); // t_0 is expected to be modified as time of week
+            return a_Gf0 + a_Gf1 * (t_G - t_0); // Eq.(A-45), delay is positive
+          }
 
-            float_t t_G(-t_rx.interval(WN, 0) - (pseudo_range / gps_space_node_t::light_speed));
+          float_t clock_error_dot(const gps_time_t &t) const {
+            return a_Gf1;
+          }
 
-            float_t t(t_G - (a_Gf0 + a_Gf1 * (t_G - t_0))); // Eq.(A-45)
-            float_t t_dot(1.0 - a_Gf1);
+          constellation_t constellation(
+              const gps_time_t &t_tx, const float_t &dt_transit = 0,
+              const bool &with_velocity = true) const {
 
-            float_t delta_t(t - t_0), delta_t2(delta_t * delta_t / 2);
+            float_t delta_t(-t_tx.interval(WN, t_0)), delta_t2(delta_t * delta_t / 2);
 
             constellation_t res = {
               xyz_t(
                   x + dx * delta_t + ddx * delta_t2,
                   y + dy * delta_t + ddy * delta_t2,
-                  z + dz * delta_t + ddz * delta_t2), // Eq. (A-44)
+                  z + dz * delta_t + ddz * delta_t2).after(dt_transit), // Eq. (A-44)
               xyz_t(
-                  (dx + ddx * delta_t) * t_dot,
-                  (dy + ddy * delta_t) * t_dot,
-                  (dz + ddz * delta_t) * t_dot),
+                  dx + ddx * delta_t,
+                  dy + ddy * delta_t,
+                  dz + ddz * delta_t).after(dt_transit),
             };
 
-            // Be careful, Sagnac correction must be performed before geometric distance calculation
-            // @see SBAS_SpaceNode::sagnac_correction
+            /* Sagnac correction, which is expected to perform before geometric distance calculation,
+             * can be skipped by using non-zero dt_transit.
+             * This is because the return values is corrected to be along with the coordinate
+             * at the reception time.
+             * @see SBAS_SpaceNode::sagnac_correction
+             */
 
             return res;
           }