gps_pvt 0.6.1 → 0.6.4

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

@@ -114,19 +114,17 @@ class SBAS_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).ephemeris().constellation(t, pseudo_range, false).position;
+          static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).ephemeris().constellation(t_tx, dt_transit, false).position;
           }
-          static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).ephemeris().constellation(t, pseudo_range, true).velocity;
+          static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).ephemeris().constellation(t_tx, dt_transit, true).velocity;
           }
-          static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            // Clock correction is taken into account in position()
-            return 0;
+          static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().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 taken in account in velocity()
-            return 0;
+          static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().clock_error_dot(t_tx);
           }
         };
         satellite_t res = {
@@ -257,20 +255,25 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
       range -= receiver_error;
 
+      static const float_t &c(GPS_SpaceNode<float_t>::light_speed);
+
       // Clock error correction
       range += ((range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)
-          ? (sat.clock_error(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed)
+          ? (sat.clock_error(time_arrival - range / c) * c)
           : range_error.value[range_error_t::SATELLITE_CLOCK]);
 
       // TODO WAAS long term clock correction (2.1.1.4.11)
 
       // 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 with Sagnac correction (A.4.4.11)
+      // Calculate residual without Sagnac correction (A.4.4.11),
+      // because of the satellite position is calculated in the reception time ECEF.
       res.range_residual = range
-          + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
+          // + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
           - geometric_range;
 
       // Setup design matrix
@@ -312,12 +315,12 @@ class SBAS_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) * GPS_SpaceNode<float_t>::light_speed;
+          + sat.clock_error_dot(t_tx) * c;
 
       return res;
     }

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

@@ -41,7 +41,6 @@
 #include <cstring>
 #include <map>
 #include <set>
-#include <algorithm>
 #include <stdexcept>
 
 #include "util/text_helper.h"
@@ -61,22 +60,15 @@ struct SP3_Product {
     typedef std::map<GPS_Time<FloatT>, prop_t> history_t;
     history_t pos_history;
     history_t vel_history;
-
-    static const struct interpolate_cnd_t {
-      std::size_t max_epochs; ///< maximum number of epochs used for interpolation
-      FloatT max_delta_t; ///< maximum acceptable absolute time difference between t and epoch
-    } interpolate_cnd_default;
+    
+    typedef InterpolatableSet<GPS_Time<FloatT>, prop_t, FloatT> interpolator_t;
+    typedef typename interpolator_t::condition_t interpolate_cnd_t;
+    static const interpolate_cnd_t interpolate_cnd_default;
 
     mutable struct {
-      struct target_t {
-
-        typedef typename std::vector<std::pair<GPS_Time<FloatT>, prop_t> > buf_t;
-        buf_t buf;
-        GPS_Time<FloatT> t0;
-        std::vector<FloatT> dt;
-        bool updated_full_cnd;
+      struct target_t : public interpolator_t {
 
-        target_t() : t0(0, 0), updated_full_cnd(false) {}
+        target_t() : interpolator_t() {}
 
         /**
          * update interpolation source
@@ -88,58 +80,13 @@ struct SP3_Product {
             const bool &force_update = false,
             const interpolate_cnd_t &cnd = interpolate_cnd_default){
 
-          FloatT t_diff(t0 - t);
-          if((!force_update)
-              && ((std::abs(t_diff) <= 10)
-                || ((dt.size() >= 2)
-                  && (std::abs(t_diff + dt[0]) <= std::abs(t_diff + dt[1])))) ){
-            return *this;
-          }
-
-          // If the 1st and 2nd nearest epochs are changed, then recalculate interpolation targets.
-          struct {
-            const GPS_Time<FloatT> &t_base;
-            bool operator()(
-                const typename history_t::value_type &rhs,
-                const typename history_t::value_type &lhs) const {
-              return std::abs(rhs.first - t_base) < std::abs(lhs.first - t_base);
-            }
-          } cmp = {(t0 = t)};
-
-          buf.resize(cnd.max_epochs);
-          dt.clear();
-          // extract t where t0-dt <= t <= t0+dt, then sort by ascending order of |t-t0|
-          for(typename buf_t::const_iterator
-                it(buf.begin()),
-                it_end(std::partial_sort_copy(
-                  history.lower_bound(t - cnd.max_delta_t),
-                  history.upper_bound(t + cnd.max_delta_t),
-                  buf.begin(), buf.end(), cmp));
-              it != it_end; ++it){
-            dt.push_back(it->first - t0);
-          }
-          updated_full_cnd = (dt.size() >= cnd.max_epochs);
-
+          if((!force_update) && (std::abs(t - interpolator_t::x0) <= 10)){return *this;}
+          interpolator_t::update(t, history, cnd, force_update);
           return *this;
         }
 
-        template <class Ty, class Ty_Array>
-        Ty interpolate(
-            const GPS_Time<FloatT> &t, const Ty_Array &y,
-            Ty *derivative = NULL) const {
-          int order(dt.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<Ty> y_buf(order), dy_buf(order);
-          interpolate_Neville(
-              dt, y, t - t0, y_buf, order,
-              &dy_buf, derivative ? 1 : 0);
-          if(derivative){*derivative = dy_buf[0];}
-          return y_buf[0];
-        }
+        typedef typename interpolator_t::buffer_t buf_t;
+
         Vector3<FloatT> interpolate_xyz(
             const GPS_Time<FloatT> &t,
             Vector3<FloatT> *derivative = NULL) const {
@@ -149,8 +96,8 @@ struct SP3_Product {
             const Vector3<FloatT> &operator[](const int &n) const {
               return buf_t::const_iterator::operator[](n).second.xyz;
             }
-          } xyz(buf.begin());
-          return interpolate(t, xyz, derivative);
+          } xyz(interpolator_t::buf.begin());
+          return interpolator_t::interpolate2(t, xyz, derivative);
         }
         FloatT interpolate_clk(
             const GPS_Time<FloatT> &t,
@@ -161,8 +108,8 @@ struct SP3_Product {
             const FloatT &operator[](const int &n) const {
               return buf_t::const_iterator::operator[](n).second.clk;
             }
-          } clk(buf.begin());
-          return interpolate(t, clk, derivative);
+          } clk(interpolator_t::buf.begin());
+          return interpolator_t::interpolate2(t, clk, derivative);
         }
       } pos_clk, vel_rate;
     } subset;
@@ -175,7 +122,7 @@ struct SP3_Product {
     bool precheck(const GPS_Time<FloatT> &t) const {
       // Only position/clock is checked, because velocity/rate can be calculated based on pos/clk.
       subset.pos_clk.update(t, pos_history);
-      return subset.pos_clk.updated_full_cnd;
+      return subset.pos_clk.ready;
     }
 
     typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t
@@ -224,22 +171,17 @@ struct SP3_Product {
         static inline const per_satellite_t &sat(const void *ptr) {
           return *reinterpret_cast<const per_satellite_t *>(ptr);
         }
-        static inline float_t pr2sec(const float_t &pr){
-          return pr / GPS_SpaceNode<FloatT>::light_speed;
-        }
-        static xyz_t position(const void *ptr, const gt_t &t, const float_t &pr) {
-          float_t delta_t(pr2sec(pr));
-          return sat(ptr).position(t - delta_t).after(delta_t);
+        static xyz_t position(const void *ptr, const gt_t &t_tx, const float_t &dt_transit) {
+          return sat(ptr).position(t_tx).after(dt_transit);
         }
-        static xyz_t velocity(const void *ptr, const gt_t &t, const float_t &pr) {
-          float_t delta_t(pr2sec(pr));
-          return sat(ptr).velocity(t - delta_t).after(delta_t);
+        static xyz_t velocity(const void *ptr, const gt_t &t_tx, const float_t &dt_transit) {
+          return sat(ptr).velocity(t_tx).after(dt_transit);
         }
-        static float_t clock_error(const void *ptr, const gt_t &t, const float_t &pr) {
-          return sat(ptr).clock_error(t - pr2sec(pr));
+        static float_t clock_error(const void *ptr, const gt_t &t_tx) {
+          return sat(ptr).clock_error(t_tx);
         }
-        static float_t clock_error_dot(const void *ptr, const gt_t &t, const float_t &pr) {
-          return sat(ptr).clock_error_dot(t - pr2sec(pr));
+        static float_t clock_error_dot(const void *ptr, const gt_t &t_tx) {
+          return sat(ptr).clock_error_dot(t_tx);
         }
       };
       typename GPS_Solver_Base<FloatT>::satellite_t res = {
@@ -386,8 +328,11 @@ static typename GPS_Solver_Base<FloatT>::satellite_t select_ ## sys( \
 template <class FloatT>
 const typename SP3_Product<FloatT>::per_satellite_t::interpolate_cnd_t
     SP3_Product<FloatT>::per_satellite_t::interpolate_cnd_default = {
-  9, // max_epochs
-  60 * 60 * 2, // max_delta_t, default is 2 hr = 15 min interval records; (2 hr * 2 / (9 - 1) = 15 min)
+  9, // maximum number of epochs used for interpolation
+  /* maximum acceptable absolute time difference between t and epoch,
+   * default is 2 hr = 15 min interval records; (2 hr * 2 / (9 - 1) = 15 min)
+   */
+  60 * 60 * 2,
 };
 
 template <class FloatT>

data/ext/ninja-scan-light/tool/swig/GPS.i

@@ -302,6 +302,10 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
   GPS_Ephemeris(const typename GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris &eph) 
       : GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris(eph),
       iode_subframe3(eph.iode) {}
+  struct constellation_res_t {
+    System_XYZ<FloatT, WGS84> position, velocity;
+    FloatT clock_error, clock_error_dot;
+  };
 };
 %}
 %extend GPS_Ephemeris {
@@ -389,20 +393,21 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
         break;
     }
   }
-  %typemap(in,numinputs=0) System_XYZ<FloatT, WGS84> & (System_XYZ<FloatT, WGS84> temp) %{
-    $1 = &temp;
-  %}
-  %typemap(argout) System_XYZ<FloatT, WGS84> & {
-    %append_output(SWIG_NewPointerObj((new $*1_ltype(*$1)), $1_descriptor, SWIG_POINTER_OWN));
-  }
-  void constellation(
-      System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
-      const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0,
-      const bool &with_velocity = true) const {
-    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
-        self->constellation(t, pseudo_range, with_velocity));
-    position = res.position;
-    velocity = res.velocity;
+  %typemap(out) constellation_res_t {
+    %append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.position)), 
+        $descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
+    %append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.velocity)), 
+        $descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
+    %append_output(swig::from($1.clock_error));
+    %append_output(swig::from($1.clock_error_dot));
+  }
+  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
+      const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
+    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
+        self->constellation(t_tx, dt_transit, true));
+    typename GPS_Ephemeris<FloatT>::constellation_res_t res = {
+        pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
+    return res;
   }
 #if defined(SWIGRUBY)
   %rename("consistent?") is_consistent;
@@ -461,15 +466,14 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
   MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
   MAKE_ACCESSOR(a_Gf0, FloatT);
   MAKE_ACCESSOR(a_Gf1, FloatT);
-  %apply GPS_Ephemeris::System_XYZ<FloatT, WGS84> & { System_XYZ<FloatT, WGS84> & }; // TODO ineffective?
-  void constellation(
-      System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
-      const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0,
+  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
+      const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0,
       const bool &with_velocity = true) const {
-    typename SBAS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
-        self->constellation(t, pseudo_range, with_velocity));
-    position = res.position;
-    velocity = res.velocity;
+    typename SBAS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
+        self->constellation(t_tx, dt_transit, with_velocity));
+    typename GPS_Ephemeris<FloatT>::constellation_res_t res = {
+        pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
+    return res;
   }
 }
 
@@ -612,23 +616,13 @@ struct GLONASS_Ephemeris
     self->has_string = has_string;
     return updated;
   }
-  FloatT clock_error(
-      const GPS_Time<FloatT> &t_arrival, const FloatT &pseudo_range = 0) const {
-    return self->clock_error(t_arrival, pseudo_range);
-  }
-  %typemap(in,numinputs=0) System_XYZ<FloatT, WGS84> & (System_XYZ<FloatT, WGS84> temp) %{
-    $1 = &temp;
-  %}
-  %typemap(argout) System_XYZ<FloatT, WGS84> & {
-    %append_output(SWIG_NewPointerObj((new $*1_ltype(*$1)), $1_descriptor, SWIG_POINTER_OWN));
-  }
-  void constellation(
-      System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
-      const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0) const {
-    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
-        self->constellation(t, pseudo_range));
-    position = res.position;
-    velocity = res.velocity;
+  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
+      const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
+    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
+        self->constellation(t_tx, dt_transit));
+    typename GPS_Ephemeris<FloatT>::constellation_res_t res = {
+        pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot()};
+    return res;
   }
 #if defined(SWIGRUBY)
   %rename("consistent?") is_consistent;

data/gps_pvt.gemspec

@@ -53,6 +53,9 @@ Gem::Specification.new do |spec|
     }.flatten
   }.call
 
+  spec.rdoc_options << '--exclude=ext/ninja-scan-light'
+  spec.extra_rdoc_files = []
+
   # Uncomment to register a new dependency of your gem
   # spec.add_dependency "example-gem", "~> 1.0"
   spec.add_development_dependency "rake"

data/lib/gps_pvt/receiver.rb

@@ -410,7 +410,7 @@ class Receiver
               @solver.sbas_space_node.ionospheric_grid_points(prn)].each{|str|
             $stderr.puts str
           } if @debug[:SBAS_IGP] 
-        end
+        end if t_meas
       when :GLONASS
         next unless eph = eph_glonass_list[prn]
         leap_sec = @solver.gps_space_node.is_valid_utc ? 
@@ -508,10 +508,10 @@ class Receiver
           {
             :L1_PSEUDORANGE => [16, 8, "E", proc{|v| (trk_stat & 0x1 == 0x1) ? v : nil}],
             :L1_PSEUDORANGE_SIGMA => [43, 1, nil, proc{|v|
-              (trk_stat & 0x1 == 0x1) ? (1E-2 * (v[0] & 0xF)) : nil
+              (trk_stat & 0x1 == 0x1) ? (1E-2 * (1 << (v[0] & 0xF))) : nil
             }],
             :L1_DOPPLER => [32, 4, "e"],
-            :L1_DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (v[0] & 0xF)}],
+            :L1_DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (1 << (v[0] & 0xF))}],
             :L1_CARRIER_PHASE => [24, 8, "E", proc{|v| (trk_stat & 0x2 == 0x2) ? v : nil}],
             :L1_CARRIER_PHASE_SIGMA => [44, 1, nil, proc{|v|
               (trk_stat & 0x2 == 0x2) ? (0.004 * (v[0] & 0xF)) : nil

data/lib/gps_pvt/version.rb

@@ -1,5 +1,5 @@
-# frozen_string_literal: true
-
-module GPS_PVT
-  VERSION = "0.6.1"
-end
+# frozen_string_literal: true
+
+module GPS_PVT
+  VERSION = "0.6.4"
+end

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.6.1
+  version: 0.6.4
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
 autorequire: 
 bindir: exe
 cert_chain: []
-date: 2022-07-21 00:00:00.000000000 Z
+date: 2022-08-19 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rake
@@ -44,6 +44,7 @@ email:
 - fenrir.naru@gmail.com
 executables:
 - gps_pvt
+- to_ubx
 extensions:
 - ext/gps_pvt/extconf.rb
 extra_rdoc_files: []
@@ -57,6 +58,7 @@ files:
 - bin/console
 - bin/setup
 - exe/gps_pvt
+- exe/to_ubx
 - ext/gps_pvt/Coordinate/Coordinate_wrap.cxx
 - ext/gps_pvt/GPS/GPS_wrap.cxx
 - ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx
@@ -109,7 +111,8 @@ metadata:
   homepage_uri: https://github.com/fenrir-naru/gps_pvt
   source_code_uri: https://github.com/fenrir-naru/gps_pvt
 post_install_message: 
-rdoc_options: []
+rdoc_options:
+- "--exclude=ext/ninja-scan-light"
 require_paths:
 - lib
 required_ruby_version: !ruby/object:Gem::Requirement