gps_pvt 0.2.1 → 0.3.3

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

@@ -339,10 +339,16 @@ struct GPS_Time {
     return t.operator<=(*this);
   }
   
+  /**
+   * Convert to std::tm struct
+   * @param leap_seconds If offset of GPS time relative to UTC is known,
+   * specify it by using this parameter.
+   * As of Jan. 1st, 2022, +18 seconds are specified.
+   */
   std::tm c_tm(const float_t &leap_seconds = 0) const {
     std::tm t;
     
-    GPS_Time mod_t((*this) + leap_seconds);
+    GPS_Time mod_t((*this) - leap_seconds);
     
     std::div_t min_sec(std::div((int)mod_t.seconds, 60));
     t.tm_sec = min_sec.rem;
@@ -526,6 +532,9 @@ class GPS_SpaceNode {
       return res;
     }
     static const float_t gamma_L1_L2;
+    static const float_t gamma_per_L1(const float_t &freq){
+      return std::pow(L1_Frequency / freq, 2);
+    }
 
   protected:
     static float_t rad2sc(const float_t &rad) {return rad / M_PI;}
@@ -2151,37 +2160,17 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
           usr.llh(), 
           t);
     }
-    
-    struct IonospericCorrection {
-      const GPS_SpaceNode<float_t> &space_node;
-      float_t operator()(
-          const enu_t &relative_pos,
-          const llh_t &usrllh,
-          const gps_time_t &t) const {
-        return space_node.iono_correction(relative_pos, usrllh, t);
-      }
-      float_t operator()(
-          const xyz_t &sat,
-          const xyz_t &usr,
-          const gps_time_t &t) const {
-        return space_node.iono_correction(sat, usr, t);
-      }
-    };
-    IonospericCorrection iono_correction() const {
-      IonospericCorrection res = {*this};
-      return res;
-    }
 
     /**
-     * Calculate correction value in accordance with tropospheric model
+     * Calculate correction value in accordance with tropospheric Hopfield model
      * 
      * @param relative_pos satellite position (relative position, NEU)
      * @param usrllh user position (absolute position, LLH)
      * @return correction in meters
      */
-    float_t tropo_correction(
+    static float_t tropo_correction(
         const enu_t &relative_pos,
-        const llh_t &usrllh) const {
+        const llh_t &usrllh){
       
       // Elevation (rad)
       float_t el(relative_pos.elevation());
@@ -2291,31 +2280,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
      * @param usr user position (absolute position, XYZ)
      * @return correction in meters
      */
-    float_t tropo_correction(
+    static float_t tropo_correction(
         const xyz_t &sat,
-        const xyz_t &usr) const {
+        const xyz_t &usr) {
       return tropo_correction(
           enu_t::relative(sat, usr),
           usr.llh());
     }
-
-    struct TropospericCorrection {
-      const GPS_SpaceNode<float_t> &space_node;
-      float_t operator()(
-          const enu_t &relative_pos,
-          const llh_t &usrllh) const {
-        return space_node.tropo_correction(relative_pos, usrllh);
-      }
-      float_t operator()(
-          const xyz_t &sat,
-          const xyz_t &usr) const {
-        return space_node.tropo_correction(sat, usr);
-      }
-    };
-    TropospericCorrection tropo_correction() const {
-      TropospericCorrection res = {*this};
-      return res;
-    }
 };
 
 template <class FloatT>

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

@@ -49,7 +49,6 @@
 
 #include "GPS.h"
 #include "GPS_Solver_Base.h"
-#include "SBAS.h"
 #include "NTCM.h"
 
 template <class FloatT>
@@ -57,66 +56,10 @@ struct GPS_Solver_GeneralOptions {
   FloatT elevation_mask;
   FloatT residual_mask;
 
-  enum ionospheric_model_t {
-    IONOSPHERIC_KLOBUCHAR,
-    IONOSPHERIC_SBAS,
-    IONOSPHERIC_NTCM_GL,
-    IONOSPHERIC_NONE, // which allows no correction
-    IONOSPHERIC_MODELS,
-    IONOSPHERIC_SKIP = IONOSPHERIC_MODELS, // which means delegating the next slot
-  };
-  ionospheric_model_t ionospheric_models[IONOSPHERIC_MODELS]; // lower index means having higher priority
-
-  int count_ionospheric_models() const {
-    int res(0);
-    for(std::size_t i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
-      if(ionospheric_models[i] != IONOSPHERIC_SKIP){res++;}
-    }
-    return res;
-  }
-
-  FloatT f_10_7;
-
   GPS_Solver_GeneralOptions()
       : elevation_mask(0), // elevation mask default is 0 [deg]
-      residual_mask(30), // range residual mask is 30 [m]
-      f_10_7(-1) {
-    for(std::size_t i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
-      ionospheric_models[i] = IONOSPHERIC_SKIP;
-    }
-    // default: broadcasted Klobuchar parameters are at least required for solution.
-    ionospheric_models[0] = IONOSPHERIC_KLOBUCHAR;
-  }
-
-  bool insert_ionospheric_model(const ionospheric_model_t &model, const int &index = 0) {
-    if((index < 0)
-        || (index >= sizeof(ionospheric_models) / sizeof(ionospheric_models[0]))){
-      return false;
-    }
+      residual_mask(30) { // range residual mask is 30 [m]
 
-    // shift, then replace
-    for(int i(index), j(i + 1); j < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i, ++j){
-      ionospheric_models[j] = ionospheric_models[i];
-    }
-    ionospheric_models[index] = model;
-
-    return true;
-  }
-  bool remove_ionospheric_model(const ionospheric_model_t &model) {
-    // shift, then replace
-    int j(0);
-    for(int i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
-      if(ionospheric_models[i] == model){continue;}
-      ionospheric_models[j++] = ionospheric_models[i];
-    }
-    if(j == (sizeof(ionospheric_models) / sizeof(ionospheric_models[0]))){
-      return false;
-    }
-    do{
-      ionospheric_models[j++] = IONOSPHERIC_SKIP;
-    }while(j < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]));
-
-    return true;
   }
 };
 
@@ -158,24 +101,23 @@ class GPS_SinglePositioning : public SolverBaseT {
     inheritate_type(llh_t);
     inheritate_type(enu_t);
 
-    typedef SBAS_SpaceNode<float_t> sbas_space_node_t;
-
     inheritate_type(pos_t);
 
     inheritate_type(prn_obs_t);
     typedef typename base_t::measurement_t measurement_t;
     inheritate_type(measurement_items_t);
     typedef typename base_t::range_error_t range_error_t;
+    typedef typename base_t::range_corrector_t range_corrector_t;
+    typedef typename base_t::range_correction_t range_correction_t;
 
     inheritate_type(relative_property_t);
     inheritate_type(geometric_matrices_t);
     inheritate_type(user_pvt_t);
 #undef inheritate_type
 
-    const sbas_space_node_t *space_node_sbas; ///< optional
-
     typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
         GPS_SinglePositioning_Options<float_t>, base_t> options_t;
+
   protected:
     const space_node_t &_space_node;
     GPS_SinglePositioning_Options<float_t> _options;
@@ -183,43 +125,50 @@ class GPS_SinglePositioning : public SolverBaseT {
   public:
     const space_node_t &space_node() const {return _space_node;}
 
-    /**
-     * Check availability of ionospheric models
-     * If a model is completely unavailable, it will be replaced to IONOSPHERIC_SKIP.
-     * It implies that when a model has conditional applicability (such as SBAS), it will be retained.
-     *
-     * @return (int) number of (possibly) available models
-     */
-    int filter_ionospheric_models(GPS_SinglePositioning_Options<float_t> &opt) const {
-
-      int available_models(0);
-
-      for(std::size_t i(0);
-          i < sizeof(opt.ionospheric_models) / sizeof(opt.ionospheric_models[0]); ++i){
-        bool usable(false);
-        switch(opt.ionospheric_models[i]){
-          case options_t::IONOSPHERIC_KLOBUCHAR:
-            // check whether Klobuchar parameters alpha and beta have been already received
-            if(_space_node.is_valid_iono()){usable = true;}
-            break;
-          case options_t::IONOSPHERIC_SBAS:
-            if(space_node_sbas){usable = true;}
-            break;
-          case options_t::IONOSPHERIC_NTCM_GL:
-            if(opt.f_10_7 >= 0){usable = true;}
-            // check F10.7 has been already configured
-            break;
-          default: break;
-        }
-        if(usable){
-          available_models++;
-        }else{
-          opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
-        }
+    struct klobuchar_t : public range_corrector_t {
+      const space_node_t &space_node;
+      klobuchar_t(const space_node_t &sn) : range_corrector_t(), space_node(sn) {}
+      bool is_available(const gps_time_t &t) const {
+        return space_node.is_valid_iono();
+      }
+      float_t *calculate(
+          const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
+          float_t &buf) const {
+        return &(buf = space_node.iono_correction(sat_rel_pos, usr_pos.llh, t));
       }
+    } ionospheric_klobuchar;
 
-      return available_models;
-    }
+    struct ntcm_gl_t : public range_corrector_t {
+      float_t f_10_7;
+      ntcm_gl_t() : range_corrector_t(), f_10_7(-1) {}
+      bool is_available(const gps_time_t &t) const {
+        return f_10_7 >= 0;
+      }
+      float_t *calculate(
+          const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
+          float_t &buf) const {
+        if(!is_available(t)){return NULL;}
+        typename space_node_t::pierce_point_res_t pp(
+            space_node_t::pierce_point(sat_rel_pos, usr_pos.llh));
+        return &(buf = -space_node_t::tec2delay(space_node_t::slant_factor(sat_rel_pos)
+            * NTCM_GL_Generic<float_t>::tec_vert(
+              pp.latitude, pp.longitude, t.year(), f_10_7)));
+      }
+    } ionospheric_ntcm_gl;
+
+    struct tropospheric_simplified_t : public range_corrector_t {
+      tropospheric_simplified_t() : range_corrector_t() {}
+      bool is_available(const gps_time_t &t) const {
+        return true;
+      }
+      float_t *calculate(
+          const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
+          float_t &buf) const {
+        return &(buf = space_node_t::tropo_correction(sat_rel_pos, usr_pos.llh));
+      }
+    } tropospheric_simplified;
+
+    range_correction_t ionospheric_correction, tropospheric_correction;
 
     options_t available_options() const {
       return options_t(base_t::available_options(), _options);
@@ -227,20 +176,26 @@ class GPS_SinglePositioning : public SolverBaseT {
 
     options_t available_options(const options_t &opt_wish) const {
       GPS_SinglePositioning_Options<float_t> opt(opt_wish);
-      filter_ionospheric_models(opt);
       return options_t(base_t::available_options(opt_wish), opt);
     }
 
     options_t update_options(const options_t &opt_wish){
-      filter_ionospheric_models(_options = opt_wish);
+      _options = opt_wish;
       return options_t(base_t::update_options(opt_wish), _options);
     }
 
     GPS_SinglePositioning(const space_node_t &sn)
-        : base_t(), 
-        _space_node(sn), space_node_sbas(NULL),
-        _options() {
-      filter_ionospheric_models(_options);
+        : base_t(), _space_node(sn), _options(available_options(options_t())),
+        ionospheric_klobuchar(sn), ionospheric_ntcm_gl(),
+        tropospheric_simplified(),
+        ionospheric_correction(), tropospheric_correction() {
+
+      // default ionospheric correction:
+      // Broadcasted Klobuchar parameters are at least required for solution.
+      ionospheric_correction.push_front(&ionospheric_klobuchar);
+
+      // default troposheric correction: simplified
+      tropospheric_correction.push_front(&tropospheric_simplified);
     }
 
     ~GPS_SinglePositioning(){}
@@ -294,55 +249,14 @@ class GPS_SinglePositioning : public SolverBaseT {
       enu_t relative_pos(enu_t::relative(sat_pos, usr_pos.xyz));
 
       if(error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
-        // Ionospheric model selection, the fall back is no correction
-        bool iono_model_hit(false);
-        for(std::size_t i(0);
-            i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]);
-            ++i){
-          switch(_options.ionospheric_models[i]){
-            case options_t::IONOSPHERIC_KLOBUCHAR:
-              residual.residual += _space_node.iono_correction(relative_pos, usr_pos.llh, time_arrival);
-              break;
-            case options_t::IONOSPHERIC_SBAS: {
-              // placeholder of checking availability and performing correction
-              typedef typename SBAS_SpaceNode<float_t>::available_satellites_t sats_t;
-              sats_t sats(space_node_sbas->available_satellites(usr_pos.llh.longitude()));
-
-              typename SBAS_SpaceNode<float_t>::IonosphericGridPoints::PointProperty prop;
-              for(typename sats_t::const_iterator it(sats.begin()); it != sats.end(); ++it){
-                prop = it->second
-                    ->ionospheric_grid_points().iono_correction(relative_pos, usr_pos.llh);
-                break; // TODO The nearest satellite is only checked
-              }
-              if(!prop.is_available()){continue;}
-              residual.residual += prop.delay;
-              break;
-            }
-            case options_t::IONOSPHERIC_NTCM_GL: {
-              // TODO f_10_7 setup, optimization (mag_model etc.)
-              typename space_node_t::pierce_point_res_t pp(_space_node.pierce_point(relative_pos, usr_pos.llh));
-              residual.residual -= space_node_t::tec2delay(
-                  _space_node.slant_factor(relative_pos)
-                  * NTCM_GL_Generic<float_t>::tec_vert(
-                      pp.latitude, pp.longitude,
-                      time_arrival.year(), _options.f_10_7));
-              break;
-            }
-            case options_t::IONOSPHERIC_NONE:
-              break;
-            default:
-              continue;
-          }
-          iono_model_hit = true;
-          break;
-        }
+        residual.residual += ionospheric_correction(time_arrival, usr_pos, relative_pos);
       }else{
         residual.residual += error.value[range_error_t::IONOSPHERIC];
       }
 
       // Tropospheric
       residual.residual += (error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
-          ? _space_node.tropo_correction(relative_pos, usr_pos.llh)
+          ? tropospheric_correction(time_arrival, usr_pos, relative_pos)
           : error.value[range_error_t::TROPOSPHERIC];
 
       // Setup weight
@@ -488,7 +402,7 @@ class GPS_SinglePositioning : public SolverBaseT {
 
       res.receiver_time = receiver_time;
 
-      if(_options.count_ionospheric_models() == 0){
+      if(!ionospheric_correction.select(receiver_time)){
         res.error_code = user_pvt_t::ERROR_INVALID_IONO_MODEL;
         return;
       }

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

@@ -38,6 +38,8 @@
 #include <vector>
 #include <map>
 #include <utility>
+#include <deque>
+#include <algorithm>
 
 #include <cmath>
 #include <cstring>
@@ -101,6 +103,7 @@ struct GPS_Solver_Base {
       L1_RANGE_RATE_SIGMA,
       L1_SIGNAL_STRENGTH_dBHz,
       L1_LOCK_SEC,
+      L1_FREQUENCY,
       MEASUREMENT_ITEMS_PREDEFINED,
     };
   };
@@ -223,6 +226,53 @@ struct GPS_Solver_Base {
     return res;
   }
 
+  struct range_corrector_t {
+    virtual ~range_corrector_t() {}
+    virtual bool is_available(const gps_time_t &t) const {
+      return false;
+    }
+    virtual float_t *calculate(
+        const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
+        float_t &buf) const = 0;
+  };
+  static const struct no_correction_t : public range_corrector_t {
+    bool is_available(const gps_time_t &t) const {
+      return true;
+    }
+    float_t *calculate(
+        const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
+        float_t &buf) const {
+      return &(buf = 0);
+    }
+  } no_correction;
+  struct range_correction_t : public std::deque<const range_corrector_t *> {
+    typedef std::deque<const range_corrector_t *> super_t;
+    range_correction_t() : super_t() {}
+    const range_corrector_t *select(const gps_time_t &t) const {
+      for(typename super_t::const_iterator it(super_t::begin()), it_end(super_t::end());
+          it != it_end; ++it){
+        if((*it)->is_available(t)){return *it;}
+      }
+      return NULL;
+    }
+    float_t operator()(
+        const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos) const {
+      float_t res;
+      for(typename super_t::const_iterator it(super_t::begin()), it_end(super_t::end());
+          it != it_end; ++it){
+        if((*it)->calculate(t, usr_pos, sat_rel_pos, res)){return res;}
+      }
+      return 0;
+    }
+    void remove(const typename super_t::value_type &v){
+      std::remove(super_t::begin(), super_t::end(), v);
+    }
+    void add(const typename super_t::value_type &v){
+      remove(v);
+      super_t::push_front(v);
+    }
+  };
+
   /**
    * Select appropriate solver, this is provision for GNSS extension
    * @param prn satellite number
@@ -602,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 {
@@ -1043,6 +1093,10 @@ const typename GPS_Solver_Base<FloatT>::range_error_t
       {0},
     };
 
+template <class FloatT>
+const typename GPS_Solver_Base<FloatT>::no_correction_t
+    GPS_Solver_Base<FloatT>::no_correction;
+
 template <class FloatT, class PVT_BaseT = typename GPS_Solver_Base<FloatT>::user_pvt_t>
 struct GPS_PVT_Debug : public PVT_BaseT {
   typename GPS_Solver_Base<FloatT>::matrix_t G, W, delta_r;

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

@@ -1536,10 +1536,10 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
      * @see A.4.2.4
      * @return correction in meters
      */
-    float_t tropo_correction(
+    static float_t tropo_correction(
         const float_t &year_utc,
         const enu_t &relative_pos,
-        const llh_t &usrllh) const {
+        const llh_t &usrllh){
 
       if(usrllh.height() > 10E3){ // same as RTKlib; troposphere ranges from 0 to approximately 11km
         return 0;