gps_pvt 0.2.3 → 0.3.0

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: b6820c87f08dcf6c64e70d304c634b32c1bea265d6f360fc79c8c9405822569a
-  data.tar.gz: 75b9db05a1c23827fc9e1163b476f4e3293a7d09efbf4a9a1cd9636dda7387bd
+  metadata.gz: 3ceeb392e9d4a3df9a3b6af1ed6b19a92fec4fae110c9c89118602f3213d6905
+  data.tar.gz: c91c94043d2e39dde8ea0909dbd862f0194b15a4f2baa3030b48d760f933a147
 SHA512:
-  metadata.gz: f22eb60b17dc0bbd6a8c249deedd39e0fa5ec179bf28abb5c36cdf4191d79219af0dd456af93e8432f93173d07e42debdef11cdcdf6c9327395c38535c7d8de5
-  data.tar.gz: a7643df74fd9947d07383353d4cdf860b45494b13e2c332ccb2176a771112acd3887a9a8dc95e62ad8f97841506cceaccb79d8ba990dc6148aeb66efc07f51a3
+  metadata.gz: bcf702f83fd0df488470b385df370c3639e953b064a6acd9cdc4418a98b03ffab9c15dc9af0ad79185177bc3feb03e74b0ee0c9dba667bf73af1e1afabdca7be
+  data.tar.gz: eac046233bfd52db17a3fb94d2c74fcf01c8cd3200d0d5dce2546a669bb9313cccda4bdb74e2b991f1397d27325ec87d79d8c0e87be36b05fa6856daf0342ad0

data/README.md

@@ -41,7 +41,7 @@ From version 0.2.0, SBAS and QZSS are supported in addition to GPS. QZSS ranging
 
     $ gps_pvt --with=137 RINEX_or_UBX_file(s)
 
-For developer, this library will be used in the following:
+For developer, this library will be used like:
 
 ```ruby
 require 'gps_pvt'
@@ -81,25 +81,41 @@ receiver.parse_rinex_obs(rinex_obs_file){|pvt, meas| # per epoch
   }
 }
 
-# Customize solution
+## Further customization
+# General options
 receiver.solver.gps_options.exclude(prn) # Exclude satellite; the default is to use every satellite if visible
 receiver.solver.gps_options.include(prn) # Discard previous setting of exclusion
 receiver.solver.gps_options.elevation_mask = Math::PI / 180 * 10 # example 10 [deg] elevation mask
+# receiver.solver.sbas_options is for SBAS.
+
+# Precise control of properties for each satellite and for each iteration
 receiver.solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel|
-  # control weight per satellite per iteration
   weight, range_c, range_r, rate_rel_neg, *los_neg = rel_prop # relative property
   # meas is measurement represented by pseudo range of the selected satellite.
   # rcv_e, t_arv, usr_pos, usr_vel are temporary solution of 
   # receiver clock error [m], time of arrival [s], user position and velocity in ECEF, respectively.
   
   weight = 1 # same as default; identical weight for each visible satellite
-  # or weight based on elevation
+  # or weight based on elevation, for example:
   # elv = GPS_PVT::Coordinate::ENU::relative_rel(GPS_PVT::Coordinate::XYZ::new(*los_neg), usr_pos).elevation
   # weight = (Math::sin(elv)/0.8)**2
   
   [weight, range_c, range_r, rate_rel_neg] + los_neg # must return relative property
 }
 
+# Range correction (since v0.3.0)
+receiver.solver.correction = { # provide by using a Hash
+  # ionospheric and transpheric models are changeable, and current configuration
+  # can be obtained by receiver.solver.correction without assigner.
+  :gps_ionospheric => proc{|t, usr_pos_xyz, sat_pos_enu|
+    # t, usr_pos_xyz, sat_pos_enu are temporary solution of 
+    # time of arrival [s], user position in ECEF, 
+    # and satellite position in ENU respectively.
+    0 # must return correction value, delaying is negative.
+  },
+  # combination of (gps or sbas) and (ionospheric or tropospheric) are available
+}
+
 # Dynamic customization of weight for each epoch
 (class << receiver; self; end).instance_eval{ # do before parse_XXX
   alias_method(:run_orig, :run)

data/ext/gps_pvt/GPS/GPS_wrap.cxx

@@ -2507,6 +2507,26 @@ struct SBAS_SolverOptions
 };
 
 
+template <class FloatT>
+struct GPS_RangeCorrector
+    : public GPS_Solver_Base<FloatT>::range_corrector_t {
+  VALUE callback;
+  GPS_RangeCorrector(const VALUE &callback_)
+      : GPS_Solver_Base<FloatT>::range_corrector_t(),
+      callback(callback_) {}
+  bool is_available(const typename GPS_Solver_Base<FloatT>::gps_time_t &t) const {
+    return false;
+  }
+  FloatT *calculate(
+      const typename GPS_Solver_Base<FloatT>::gps_time_t &t, 
+      const typename GPS_Solver_Base<FloatT>::pos_t &usr_pos, 
+      const typename GPS_Solver_Base<FloatT>::enu_t &sat_rel_pos,
+      FloatT &buf) const {
+    return NULL;
+  }
+};
+
+
 template <class FloatT>
 struct GPS_Solver 
     : public GPS_Solver_RAIM_LSR<FloatT, 
@@ -2527,14 +2547,22 @@ struct GPS_Solver
     sbas_t() : space_node(), options(), solver(space_node) {}
   } sbas;
   VALUE hooks;
+  typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
+  user_correctors_t user_correctors;
 
   static void mark(void *ptr){
     GPS_Solver<FloatT> *solver = (GPS_Solver<FloatT> *)ptr;
-    if(solver->hooks == Qnil){return;}
     rb_gc_mark(solver->hooks);
+    for(typename user_correctors_t::const_iterator 
+          it(solver->user_correctors.begin()), it_end(solver->user_correctors.end());
+        it != it_end; ++it){
+      rb_gc_mark(it->callback);
+    }
   }
 
-  GPS_Solver() : super_t(), gps(), sbas(), hooks() {
+  GPS_Solver() : super_t(),
+      gps(), sbas(),
+      hooks(), user_correctors() {
 
     hooks = rb_hash_new();
 
@@ -2606,6 +2634,19 @@ struct GPS_Solver
         if(!update){break;}
         typename range_correction_list_t::const_iterator it(list.find(i));
         if(it == list.end()){break;}
+
+        // Remove user defined unused correctors
+        for(typename base_t::range_correction_t::const_iterator
+              it2(root[i]->begin()), it2_end(root[i]->end());
+            it2 != it2_end; ++it2){
+          for(typename user_correctors_t::const_iterator
+                it3(user_correctors.begin()), it3_end(user_correctors.end());
+              it3 != it3_end; ++it3){
+            if(*it2 != &(*it3)){continue;}
+            user_correctors.erase(it3);
+          }
+        }
+
         root[i]->clear();
         for(typename range_correction_list_t::mapped_type::const_iterator
               it2(it->second.begin()), it2_end(it->second.end());
@@ -3794,6 +3835,38 @@ SWIGINTERN double const &GPS_SolverOptions_Common_Sl_double_Sg__get_residual_mas
     }
   
 
+    template <>
+    bool GPS_RangeCorrector<double>::is_available(
+        const typename GPS_Solver_Base<double>::gps_time_t &t) const {
+      VALUE values[] = {
+        SWIG_NewPointerObj(
+            const_cast< GPS_Time<double> * >(&t), SWIGTYPE_p_GPS_TimeT_double_t, 0),
+      };
+      VALUE res(proc_call_throw_if_error(
+          callback, sizeof(values) / sizeof(values[0]), values));
+      return RTEST(res) ? true : false;
+    }
+    template <>
+    double *GPS_RangeCorrector<double>::calculate(
+        const typename GPS_Solver_Base<double>::gps_time_t &t,
+        const typename GPS_Solver_Base<double>::pos_t &usr_pos, 
+        const typename GPS_Solver_Base<double>::enu_t &sat_rel_pos,
+        double &buf) const {
+      VALUE values[] = {
+        SWIG_NewPointerObj(
+            const_cast< GPS_Time<double> * >(&t),
+            SWIGTYPE_p_GPS_TimeT_double_t, 0),
+        SWIG_NewPointerObj(
+            (const_cast< System_XYZ<double,WGS84> * >(&usr_pos.xyz)),
+            SWIGTYPE_p_System_XYZT_double_WGS84_t, 0),
+        SWIG_NewPointerObj(
+            (const_cast< System_ENU<double,WGS84> * >(&sat_rel_pos)),
+            SWIGTYPE_p_System_ENUT_double_WGS84_t, 0),
+      };
+      VALUE res(proc_call_throw_if_error(
+          callback, sizeof(values) / sizeof(values[0]), values));
+      return SWIG_IsOK(swig::asval(res, &buf)) ? &buf : NULL;
+    }
     template<>
     VALUE GPS_Solver<double>::update_correction(
         const bool &update, const VALUE &hash){
@@ -3836,10 +3909,12 @@ SWIGINTERN double const &GPS_SolverOptions_Common_Sl_double_Sg__get_residual_mas
             for(; k < sizeof(item) / sizeof(item[0]); ++k){
               if(v == item[k].sym){break;}
             }
-            if(k >= sizeof(item) / sizeof(item[0])){
-              continue; // TODO other than symbol
+            if(k >= sizeof(item) / sizeof(item[0])){ // other than symbol
+              user_correctors.push_back(GPS_RangeCorrector<double>(v));
+              input[i].push_back(&user_correctors.back());
+            }else{
+              input[i].push_back(item[k].obj);
             }
-            input[i].push_back(item[k].obj);
           }
         }
         VALUE opt(rb_hash_lookup(hash, k_opt));
@@ -3866,10 +3941,12 @@ SWIGINTERN double const &GPS_SolverOptions_Common_Sl_double_Sg__get_residual_mas
           for(; i < sizeof(item) / sizeof(item[0]); ++i){
             if(*it2 == item[i].obj){break;}
           }
-          if(i >= sizeof(item) / sizeof(item[0])){
-            continue; // TODO other than built-in corrector
+          if(i >= sizeof(item) / sizeof(item[0])){ // other than built-in corrector
+            rb_ary_push(v, 
+                reinterpret_cast<const GPS_RangeCorrector<double> *>(*it2)->callback);
+          }else{
+            rb_ary_push(v, item[i].sym);
           }
-          rb_ary_push(v, item[i].sym);
         }
         rb_hash_aset(res, k, v);
       }

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

@@ -652,14 +652,14 @@ protected:
     // Least square
     matrix_t delta_x(geomat.partial(res.used_satellites).least_square());
 
-    xyz_t delta_user_position(delta_x.partial(3, 1, 0, 0));
+    xyz_t delta_user_position(delta_x.partial(3, 1));
     res.user_position.xyz += delta_user_position;
     res.user_position.llh = res.user_position.xyz.llh();
 
     const float_t &delta_receiver_error(delta_x(3, 0));
     res.receiver_error += delta_receiver_error;
 
-    return ((delta_x.transpose() * delta_x)(0, 0) <= 1E-6); // equivalent to abs(x) <= 1E-3 [m]
+    return (delta_x.partial(4, 1).norm2F() <= 1E-6); // equivalent to abs(x) <= 1E-3 [m]
   }
 
   struct user_pvt_opt_t {

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

@@ -878,6 +878,27 @@ struct SBAS_SolverOptions
 };
 %}
 
+%header {
+template <class FloatT>
+struct GPS_RangeCorrector
+    : public GPS_Solver_Base<FloatT>::range_corrector_t {
+  SWIG_Object callback;
+  GPS_RangeCorrector(const SWIG_Object &callback_)
+      : GPS_Solver_Base<FloatT>::range_corrector_t(),
+      callback(callback_) {}
+  bool is_available(const typename GPS_Solver_Base<FloatT>::gps_time_t &t) const {
+    return false;
+  }
+  FloatT *calculate(
+      const typename GPS_Solver_Base<FloatT>::gps_time_t &t, 
+      const typename GPS_Solver_Base<FloatT>::pos_t &usr_pos, 
+      const typename GPS_Solver_Base<FloatT>::enu_t &sat_rel_pos,
+      FloatT &buf) const {
+    return NULL;
+  }
+};
+}
+
 %extend GPS_Solver {
   %ignore super_t;
   %ignore base_t;
@@ -889,6 +910,8 @@ struct SBAS_SolverOptions
   %ignore relative_property;
   %ignore satellite_position;
   %ignore update_position_solution;
+  %ignore user_correctors_t;
+  %ignore user_correctors;
   %immutable hooks;
   %ignore mark;
   %fragment("hook"{GPS_Solver<FloatT>}, "header",
@@ -1033,6 +1056,38 @@ struct SBAS_SolverOptions
   %fragment("correction"{GPS_Solver<FloatT>}, "header",
       fragment=SWIG_From_frag(int),
       fragment=SWIG_Traits_frag(FloatT)){
+    template <>
+    bool GPS_RangeCorrector<FloatT>::is_available(
+        const typename GPS_Solver_Base<FloatT>::gps_time_t &t) const {
+      VALUE values[] = {
+        SWIG_NewPointerObj(
+            %const_cast(&t, GPS_Time<FloatT> *), $descriptor(GPS_Time<FloatT> *), 0),
+      };
+      VALUE res(proc_call_throw_if_error(
+          callback, sizeof(values) / sizeof(values[0]), values));
+      return RTEST(res) ? true : false;
+    }
+    template <>
+    FloatT *GPS_RangeCorrector<FloatT>::calculate(
+        const typename GPS_Solver_Base<FloatT>::gps_time_t &t,
+        const typename GPS_Solver_Base<FloatT>::pos_t &usr_pos, 
+        const typename GPS_Solver_Base<FloatT>::enu_t &sat_rel_pos,
+        FloatT &buf) const {
+      VALUE values[] = {
+        SWIG_NewPointerObj(
+            %const_cast(&t, GPS_Time<FloatT> *),
+            $descriptor(GPS_Time<FloatT> *), 0),
+        SWIG_NewPointerObj(
+            (%const_cast(&usr_pos.xyz, System_XYZ<FloatT, WGS84> *)),
+            $descriptor(System_XYZ<FloatT, WGS84> *), 0),
+        SWIG_NewPointerObj(
+            (%const_cast(&sat_rel_pos, System_ENU<FloatT, WGS84> *)),
+            $descriptor(System_ENU<FloatT, WGS84> *), 0),
+      };
+      VALUE res(proc_call_throw_if_error(
+          callback, sizeof(values) / sizeof(values[0]), values));
+      return SWIG_IsOK(swig::asval(res, &buf)) ? &buf : NULL;
+    }
     template<>
     VALUE GPS_Solver<FloatT>::update_correction(
         const bool &update, const VALUE &hash){
@@ -1075,10 +1130,12 @@ struct SBAS_SolverOptions
             for(; k < sizeof(item) / sizeof(item[0]); ++k){
               if(v == item[k].sym){break;}
             }
-            if(k >= sizeof(item) / sizeof(item[0])){
-              continue; // TODO other than symbol
+            if(k >= sizeof(item) / sizeof(item[0])){ // other than symbol
+              user_correctors.push_back(GPS_RangeCorrector<FloatT>(v));
+              input[i].push_back(&user_correctors.back());
+            }else{
+              input[i].push_back(item[k].obj);
             }
-            input[i].push_back(item[k].obj);
           }
         }
         VALUE opt(rb_hash_lookup(hash, k_opt));
@@ -1105,10 +1162,12 @@ struct SBAS_SolverOptions
           for(; i < sizeof(item) / sizeof(item[0]); ++i){
             if(*it2 == item[i].obj){break;}
           }
-          if(i >= sizeof(item) / sizeof(item[0])){
-            continue; // TODO other than built-in corrector
+          if(i >= sizeof(item) / sizeof(item[0])){ // other than built-in corrector
+            rb_ary_push(v, 
+                reinterpret_cast<const GPS_RangeCorrector<FloatT> *>(*it2)->callback);
+          }else{
+            rb_ary_push(v, item[i].sym);
           }
-          rb_ary_push(v, item[i].sym);
         }
         rb_hash_aset(res, k, v);
       }
@@ -1160,14 +1219,22 @@ struct GPS_Solver
     sbas_t() : space_node(), options(), solver(space_node) {}
   } sbas;
   SWIG_Object hooks;
+  typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
+  user_correctors_t user_correctors;
 #ifdef SWIGRUBY
   static void mark(void *ptr){
     GPS_Solver<FloatT> *solver = (GPS_Solver<FloatT> *)ptr;
-    if(solver->hooks == Qnil){return;}
     rb_gc_mark(solver->hooks);
+    for(typename user_correctors_t::const_iterator 
+          it(solver->user_correctors.begin()), it_end(solver->user_correctors.end());
+        it != it_end; ++it){
+      rb_gc_mark(it->callback);
+    }
   }
 #endif
-  GPS_Solver() : super_t(), gps(), sbas(), hooks() {
+  GPS_Solver() : super_t(),
+      gps(), sbas(),
+      hooks(), user_correctors() {
 #ifdef SWIGRUBY
     hooks = rb_hash_new();
 #endif
@@ -1239,6 +1306,19 @@ struct GPS_Solver
         if(!update){break;}
         typename range_correction_list_t::const_iterator it(list.find(i));
         if(it == list.end()){break;}
+
+        // Remove user defined unused correctors
+        for(typename base_t::range_correction_t::const_iterator
+              it2(root[i]->begin()), it2_end(root[i]->end());
+            it2 != it2_end; ++it2){
+          for(typename user_correctors_t::const_iterator
+                it3(user_correctors.begin()), it3_end(user_correctors.end());
+              it3 != it3_end; ++it3){
+            if(*it2 != &(*it3)){continue;}
+            user_correctors.erase(it3);
+          }
+        }
+
         root[i]->clear();
         for(typename range_correction_list_t::mapped_type::const_iterator
               it2(it->second.begin()), it2_end(it->second.end());

data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb

@@ -343,7 +343,12 @@ __RINEX_OBS_TEXT__
       expect(solver.correction[:gps_tropospheric]).to include(:hopfield)
       expect{solver.correction = nil}.to raise_error(RuntimeError)
       expect{solver.correction = {
-        :gps_ionospheric => [:klobuchar, :no_correction],
+        :gps_ionospheric => [proc{|t, usr_pos, sat_pos|
+              expect(t).to be_a_kind_of(GPS::Time)
+              expect(usr_pos).to be_a_kind_of(Coordinate::XYZ) unless usr_pos
+              expect(sat_pos).to be_a_kind_of(Coordinate::ENU) unless sat_pos
+              false
+            }, :klobuchar, :no_correction],
         :options => {:f_10_7 => 10},
       }}.not_to raise_error
       expect(solver.correction[:gps_ionospheric]).to include(:no_correction)

data/lib/gps_pvt/receiver.rb

@@ -47,23 +47,31 @@ class Receiver
     ]] + [
       [:used_satellites, proc{|pvt| pvt.used_satellites}],
     ] + opt[:system].collect{|sys, range|
-      bit_flip = if range.kind_of?(Array) then
-        proc{|res, i|
+      range = range.kind_of?(Array) ? proc{
+        # check whether inputs can be converted to Range
+        next nil if range.empty?
+        a, b = range.minmax
+        ((b - a) == (range.length - 1)) ? (a..b) : range
+      }.call : range
+      next nil unless range
+      bit_flip, label = case range
+      when Array
+        [proc{|res, i|
           res[i] = "1" if i = range.index(i)
           res
-        }
-      else # expect Range
+        }, range.collect{|pen| pen & 0xFF}.reverse.join('+')]
+      when Range
         base_prn = range.min
-        proc{|res, i|
+        [proc{|res, i|
           res[i - base_prn] = "1" if range.include?(i)
           res
-        }
+        }, [:max, :min].collect{|f| range.send(f) & 0xFF}.join('..')]
       end
-      ["#{sys}_PRN", proc{|pvt|
+      ["#{sys}_PRN(#{label})", proc{|pvt|
         pvt.used_satellite_list.inject("0" * range.size, &bit_flip) \
             .scan(/.{1,8}/).join('_').reverse
       }]
-    } + [[
+    }.compact + [[
       opt[:satellites].collect{|prn, label|
         [:range_residual, :weight, :azimuth, :elevation, :slopeH, :slopeV].collect{|str|
           "#{str}(#{label || prn})"
@@ -138,8 +146,11 @@ class Receiver
       rel_prop
     }
     @debug = {}
-    [:gps_options, :sbas_options].each{|target|
-      opt = @solver.send(target) # default solver options
+    solver_opts = [:gps_options, :sbas_options].collect{|target|
+      @solver.send(target)
+    }
+    solver_opts.each{|opt|
+      # default solver options
       opt.elevation_mask = 0.0 / 180 * Math::PI # 0 deg (use satellite over horizon)
       opt.residual_mask = 1E4 # 10 km (without residual filter, practically)
     }
@@ -168,6 +179,13 @@ class Receiver
         when :identical # same as default
           next true
         end
+      when :elevation_mask_deg
+        raise "Unknown elevation mask angle: #{v}" unless elv_deg = (Float(v) rescue nil)
+        $stderr.puts "Elevation mask: #{elv_deg} deg"
+        solver_opts.each{|opt|
+          opt.elevation_mask = elv_deg / 180 * Math::PI # 0 deg (use satellite over horizon)
+        }
+        next true
       when :base_station
         crd, sys = v.split(/ *, */).collect.with_index{|item, i|
           case item
@@ -219,12 +237,14 @@ class Receiver
               i = -1
               output_options[:system] << [sys_target, []]
             else
-              output_options[:system][i].reject!{|prn| prns.include?(prn)}
+              output_options[:system][i][1].reject!{|prn| prns.include?(prn)}
             end
             output_options[:satellites].reject!{|prn, label| prns.include?(prn)}
             if mode == :include then
               output_options[:system][i][1] += prns
+              output_options[:system][i][1].sort!
               output_options[:satellites] += (labels ? prns.zip(labels) : prns)
+              output_options[:satellites].sort!{|a, b| [a].flatten[0] <=> [b].flatten[0]}
             end
           }
           check_sys_svid = proc{|sys_target, range_in_sys, offset|
@@ -328,6 +348,11 @@ class Receiver
     [:azimuth, :elevation, :slopeH, :slopeV].each{|k|
       eval("define_method(:#{k}){@#{k} || self.post_solution(:@#{k})}")
     }
+    define_method(:other_state){
+      # If a design matrix G has columns larger than 4, 
+      # other states excluding position and time are estimated.
+      (self.G.rows <= 4) ? [] : (self.S * self.delta_r).transpose.to_a[0][4..-1]
+    }
   }
   
   proc{

data/lib/gps_pvt/version.rb

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

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.2.3
+  version: 0.3.0
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
-autorequire:
+autorequire: 
 bindir: exe
 cert_chain: []
-date: 2022-02-22 00:00:00.000000000 Z
+date: 2022-03-01 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rake
@@ -100,7 +100,7 @@ licenses: []
 metadata:
   homepage_uri: https://github.com/fenrir-naru/gps_pvt
   source_code_uri: https://github.com/fenrir-naru/gps_pvt
-post_install_message:
+post_install_message: 
 rdoc_options: []
 require_paths:
 - lib
@@ -115,8 +115,8 @@ required_rubygems_version: !ruby/object:Gem::Requirement
     - !ruby/object:Gem::Version
       version: '0'
 requirements: []
-rubygems_version: 3.3.7
-signing_key:
+rubygems_version: 3.1.2
+signing_key: 
 specification_version: 4
 summary: GPS position, velocity, and time (PVT) solver
 test_files: []