gps_pvt 0.7.1 → 0.7.2

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: 14551de3c8e0eae8a584044249ff80a04e5c834929ec7968b39839aa611b3050
-  data.tar.gz: 0c82a31aec65f372d5221d5c6c7ef8e0fa72d6b5fa4d5d6f5d2174ad7a6cfa82
+  metadata.gz: b2eb20de6303a24b0e5b134c7c78cd1edb043ff38d823181e596b4626b6bc536
+  data.tar.gz: 183ae6e8cfd2577132113bb3684e6e225989ce444d6647820496b13ddd3a4903
 SHA512:
-  metadata.gz: fb245345b2d07710c7df3419eefee79720b63dc9820776612d0d5e9270d1f2186f54abd4d92f8e6967457b54f8b3fc5ea2fa3448232780847c6e57b66b42a116
-  data.tar.gz: a3f6edf6a85ef786305f15788a4e4b28d32287ec9727d2b3f2885ac3653524c264b10f82c6dcd009ee32c8cab956c6e1dcc8c14025495027a80b3d10100e89f0
+  metadata.gz: a35228704032066f682e15f5abf6f8a2238d90424e6b50182e07b8fc342a1f4150ba6747ca88a9fcaef163e0ce8149a68edf087e6c0f5525a2c7a09553beb504
+  data.tar.gz: 10ace25d753123375bf8ad46cbd4d7490b13219522b39ee5774a1df6d3ba1fd3fd9f318364a19253981f24a3b83998e5df9bb82700b37d6d7f17bd1ad27ad14d

data/README.md

@@ -113,7 +113,7 @@ receiver.solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_
   # 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
+  weight = 1 # quick example: identical weight for each visible satellite
   # 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

data/exe/gps_pvt

@@ -11,7 +11,7 @@ As GPS_file, rinex_nav(*.YYn, *.YYh, *.YYq, *.YYg), rinex_obs(*.YYo), ubx(*.ubx)
 (YY = last two digit of year)
 File format is automatically determined based on its extention described in above parentheses.
 If you want to specify its format manually, command options like --rinex_nav=file_name are available.
-Other than --rinex_nav, --rinex_obs, --ubx, --sp3 or --antex are supported. 
+Other than --rinex_nav, --rinex_obs, -rinex_clk, --ubx, --sp3 or --antex are supported. 
 Supported RINEX versions are 2 and 3.
 A file having additional ".gz" or ".Z" extension is recognized as a compressed file.
 Major URL such as http(s)://... or ftp://... is acceptable as an input file name. 

data/ext/gps_pvt/GPS/GPS_wrap.cxx

@@ -3948,10 +3948,13 @@ SWIGINTERN double const &GPS_SolverOptions_Common_Sl_double_Sg__get_residual_mas
         static const int prop_items(sizeof(res.values) / sizeof(res.values[0]));
         VALUE hook(rb_hash_lookup(hooks, key));
         if(NIL_P(hook)){break;}
+        double weight((res.prop.range_sigma > 0) 
+            ? (1. / std::pow(res.prop.range_sigma, 2)) // weight=1/(sigma^2)
+            : res.prop.range_sigma);
         VALUE values[] = {
             SWIG_From_int  (prn), // prn
             rb_ary_new_from_args(prop_items, // relative_property
-              swig::from(res.prop.weight),
+              swig::from(weight),
               swig::from(res.prop.range_corrected),
               swig::from(res.prop.range_residual),
               swig::from(res.prop.rate_relative_neg),
@@ -3983,6 +3986,9 @@ SWIGINTERN double const &GPS_SolverOptions_Common_Sl_double_Sg__get_residual_mas
                   .append(" @ [").append(std::to_string(i)).append("]"));
           }
         }
+        if(res.values[0] > 0){
+          res.values[0] = std::pow(1. / res.values[0], 0.5); // sigma=(1/weight)^0.5
+        }
       }while(false);
 
       return res.prop;

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

@@ -814,11 +814,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
             static const float_t F_T_table[15]; ///< @see Table 4.4
 
             static const float_t F_T_value(const u8_t &F_T_){
-              if(F_T_ >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){
-                return -1; // not used
-              }else{
-                return F_T_table[F_T_];
-              }
+              return GPS_SpaceNode<float_t>::SatelliteProperties::Ephemeris::URA_meter(
+                  F_T_, F_T_table);
             }
 
             static const uint_t P1_value(const u8_t &P1_){
@@ -1315,12 +1312,8 @@ typename GLONASS_SpaceNode<FloatT>::u8_t GLONASS_SpaceNode<FloatT>::SatellitePro
   if(F_T <= 0){ // invalid value
     return sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]);
   }
-  u8_t res(0);
-  while(res < (sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]))){
-    if(F_T <= raw_t::F_T_table[res]){break;}
-    ++res;
-  }
-  return res;
+  return (u8_t)GPS_SpaceNode<float_t>::SatelliteProperties::Ephemeris::URA_index(
+      F_T, raw_t::F_T_table);
 }
 
 template <class FloatT>

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

@@ -131,11 +131,15 @@ class GLONASS_SinglePositioning : public SolverBaseT {
           static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
             return sat(ptr).clock_error_dot(t_tx);
           }
+          static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().F_T;
+          }
         };
         satellite_t res = {
-            &(it_sat->second), &(it_sat->second),
+            &(it_sat->second), &(it_sat->second), // position, time
             impl_t::position, impl_t::velocity,
-            impl_t::clock_error, impl_t::clock_error_dot};
+            impl_t::clock_error, impl_t::clock_error_dot,
+            &(it_sat->second), impl_t::range_sigma, NULL}; // error model
         return res;
       }
       satellites_t(const space_node_t &sn)
@@ -292,21 +296,29 @@ class GLONASS_SinglePositioning : public SolverBaseT {
         res.range_residual += range_error.value[range_error_t::IONOSPHERIC];
       }
 
-      // Setup weight
-      if(std::abs(res.range_residual) > _options.residual_mask){
-        // If residual is too big, gently exclude it by decreasing its weight.
-        res.weight = 1E-8;
-      }else{
+      // Setup range standard deviation, whose reciprocal square is used as weight
+      res.range_sigma = 1E+4; // sufficiently big value, 1E4 [m]
+      do{
+        // If residual is too big, gently exclude it.
+        if(std::abs(res.range_residual) > _options.residual_mask){break;}
 
         float_t elv(relative_pos.elevation());
         if(elv < _options.elevation_mask){
-          res.weight = 0; // exclude it when elevation is less than threshold
-        }else{
-          // elevation weight based on "GPS���p�v���O���~���O" @see GPS_Solver.h
-          res.weight = std::pow(sin(elv)/0.8, 2);
-          if(res.weight < 1E-3){res.weight = 1E-3;}
+          res.range_sigma = 0; // exclude it when elevation is less than threshold
+          break;
         }
-      }
+
+        res.range_sigma = sat.range_sigma(t_tx);
+
+        /* elevation weight based on "GPS���p�v���O���~���O"
+         * elevation[deg] :   90    53    45    30    15    10    5
+         * sf_sigma(k)    :   0.80  1.00  1.13  1.60  3.09  4.61  9.18
+         * weight(k^-2)   :   1.56  1.00  0.78  0.39  0.10  0.05  0.01
+         */
+        static const float_t max_sf(10);
+        static const float_t elv_limit(std::asin(0.8/max_sf)); // limit
+        res.range_sigma *= (elv > elv_limit) ? (0.8 / sin(elv)) : max_sf;
+      }while(false);
 
       res.range_corrected = range;
 

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

@@ -990,7 +990,7 @@ static s ## bits ## _t name(const InputT *buf){ \
           
           // Subframe.1
           uint_t WN;          ///< Week number
-          int_t URA;          ///< User range accuracy (index)
+          float_t URA;          ///< User range accuracy (m)
           uint_t SV_health;   ///< Health status
           int_t iodc;         ///< Issue of clock data
           float_t t_GD;       ///< Group delay (s)
@@ -1061,22 +1061,29 @@ static s ## bits ## _t name(const InputT *buf){ \
             return !((delta_t >= 0) && (delta_t < transmission_interval));
           }
           
-          static const float_t URA_limits[];
-          static const int URA_MAX_INDEX;
+          static const float_t URA_limits[15];
 
-          static float_t URA_meter(const int_t &index){
+          template <std::size_t N>
+          static float_t URA_meter(const int_t &index, const float_t (&table)[N]){
             if(index < 0){return -1;}
-            return (index < URA_MAX_INDEX)
-                ? URA_limits[index]
-                : URA_limits[URA_MAX_INDEX - 1] * 2;
+            return (index < N)
+                ? table[index]
+                : table[N - 1] * 2;
+          }
+          inline static float_t URA_meter(const int_t &index){
+            return URA_meter(index, URA_limits);
           }
 
-          static int_t URA_index(const float_t &meter){
+          template <std::size_t N>
+          static int_t URA_index(const float_t &meter, const float_t (&table)[N]){
             if(meter < 0){return -1;}
-            for(int i(0); i < URA_MAX_INDEX; ++i){
-              if(meter <= URA_limits[i]){return i;}
+            for(std::size_t i(0); i < N; ++i){
+              if(meter <= table[i]){return i;}
             }
-            return URA_MAX_INDEX;
+            return N;
+          }
+          inline static int_t URA_index(const float_t &meter){
+            return URA_index(meter, URA_limits);
           }
 
           float_t eccentric_anomaly(const float_t &period_from_toe) const {
@@ -1399,7 +1406,7 @@ static s ## bits ## _t name(const InputT *buf){ \
               converted.svid = svid;
               
               converted.WN = WN;
-              converted.URA = URA;
+              converted.URA = URA_meter(URA);
               converted.SV_health = SV_health;
               converted.iodc = iodc;
               CONVERT(t_GD);
@@ -1438,7 +1445,7 @@ static s ## bits ## _t name(const InputT *buf){ \
               svid = eph.svid;
 
               WN = eph.WN;
-              URA = eph.URA;
+              URA = URA_index(eph.URA);
               SV_health = eph.SV_health;
               iodc = eph.iodc;
               CONVERT(t_GD);
@@ -1475,7 +1482,7 @@ static s ## bits ## _t name(const InputT *buf){ \
           bool is_equivalent(const Ephemeris &eph) const {
             do{
               if(WN != eph.WN){break;}
-              if(URA != eph.URA){break;}
+              if(URA_index(URA) != URA_index(eph.URA)){break;}
               if(SV_health != eph.SV_health){break;}
 
 #define CHECK(TARGET) \
@@ -2478,10 +2485,6 @@ const typename GPS_SpaceNode<FloatT>::float_t GPS_SpaceNode<FloatT>::SatellitePr
   6144.00,
 };
 
-template <class FloatT>
-const int GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::URA_MAX_INDEX
-    = sizeof(URA_limits) / sizeof(URA_limits[0]);
-
 #ifdef POW2_ALREADY_DEFINED
 #undef POW2_ALREADY_DEFINED
 #else

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

@@ -155,11 +155,15 @@ class GPS_SinglePositioning : public SolverBaseT {
           static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
             return sat(ptr).clock_error_dot(t_tx);
           }
+          static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().URA;
+          }
         };
         satellite_t res = {
-            &(it_sat->second), &(it_sat->second),
+            &(it_sat->second), &(it_sat->second), // position, clock
             impl_t::position, impl_t::velocity,
-            impl_t::clock_error, impl_t::clock_error_dot};
+            impl_t::clock_error, impl_t::clock_error_dot,
+            &(it_sat->second), impl_t::range_sigma, NULL}; // error model
         return res;
       }
       satellites_t(const space_node_t &sn)
@@ -248,7 +252,7 @@ class GPS_SinglePositioning : public SolverBaseT {
       float_t &los_neg_x;
       float_t &los_neg_y;
       float_t &los_neg_z;
-      float_t &weight;
+      float_t &range_sigma;
     };
 
     /**
@@ -258,8 +262,8 @@ class GPS_SinglePositioning : public SolverBaseT {
      * @param range "corrected" pseudo range subtracted by (temporal solution of) receiver clock error in meter
      * @param time_arrival time when signal arrive at receiver
      * @param usr_pos (temporal solution of) user position
-     * @param residual calculated residual with line of site vector, and weight;
-     * When weight is equal to or less than zero, the calculated results should not be used.
+     * @param residual calculated residual with line of site vector, and pseudorange standard deviation (sigma);
+     * When sigma is equal to or less than zero, the calculated results should not be used.
      * @param error Some correction can be overwritten. If its unknown_flag is zero,
      * corrections will be skipped as possible. @see range_errors_t
      * @return (float_t) corrected range just including delay, and excluding receiver/satellite error.
@@ -281,7 +285,8 @@ class GPS_SinglePositioning : public SolverBaseT {
 
       // Calculate satellite position
       float_t dt_transit(range / c);
-      xyz_t sat_pos(sat.position(time_arrival - dt_transit, dt_transit));
+      gps_time_t time_depature(time_arrival - dt_transit);
+      xyz_t sat_pos(sat.position(time_depature, dt_transit));
       float_t geometric_range(usr_pos.xyz.dist(sat_pos));
 
       // Calculate residual
@@ -305,25 +310,30 @@ class GPS_SinglePositioning : public SolverBaseT {
           ? tropospheric_correction(time_arrival, usr_pos, relative_pos)
           : error.value[range_error_t::TROPOSPHERIC];
 
-      // Setup weight
-      if(std::abs(residual.residual) > _options.residual_mask){
-        // If residual is too big, gently exclude it by decreasing its weight.
-        residual.weight = 1E-8;
-      }else{
+      // Setup range standard deviation, whose reciprocal square is used as weight
+      residual.range_sigma = 1E+4; // sufficiently big value, 1E4 [m]
+      do{
+        // If residual is too big, gently exclude it.
+        if(std::abs(residual.residual) > _options.residual_mask){break;}
 
         float_t elv(relative_pos.elevation());
         if(elv < _options.elevation_mask){
-          residual.weight = 0; // exclude it when elevation is less than threshold
-        }else{
-          /* elevation weight based on "GPS���p�v���O���~���O"
-           * elevation[deg] :   90    53    45    30    15    10    5
-           * sigma(s)       :   0.80  1.00  1.13  1.60  3.09  4.61  9.18
-           * weight(s^-2)   :   1.56  1.00  0.78  0.39  0.10  0.05  0.01
-           */
-          residual.weight = std::pow(sin(elv)/0.8, 2); // weight=1 @ elv=53[deg]
-          if(residual.weight < 1E-3){residual.weight = 1E-3;}
+          residual.range_sigma = 0; // exclude it when elevation is less than threshold
+          break;
         }
-      }
+
+        residual.range_sigma = sat.range_sigma(time_depature);
+
+        /* elevation weight based on "GPS���p�v���O���~���O"
+         * elevation[deg] :   90    53    45    30    15    10    5
+         * sf_sigma(k)    :   0.80  1.00  1.13  1.60  3.09  4.61  9.18
+         * weight(k^-2)   :   1.56  1.00  0.78  0.39  0.10  0.05  0.01
+         */
+        static const float_t max_sf(10);
+        static const float_t elv_limit(std::asin(0.8/max_sf)); // limit
+        residual.range_sigma *= (elv > elv_limit) ? (0.8 / sin(elv)) : max_sf;
+      }while(false);
+
 
       return range;
     }
@@ -396,16 +406,16 @@ class GPS_SinglePositioning : public SolverBaseT {
       float_t range;
       range_error_t range_error;
       if(!this->range(measurement, range, &range_error)){
-        return res; // If no range entry, return with weight = 0
+        return res; // If no range entry, return with range_sigma = 0
       }
 
       satellite_t sat(select_satellite(prn, time_arrival));
-      if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0
+      if(!sat.is_available()){return res;} // If satellite is unavailable, return with range_sigma = 0
 
       residual_t residual = {
         res.range_residual,
         res.los_neg[0], res.los_neg[1], res.los_neg[2],
-        res.weight,
+        res.range_sigma,
       };
 
       res.range_corrected = range_corrected(
@@ -414,8 +424,15 @@ class GPS_SinglePositioning : public SolverBaseT {
       res.rate_relative_neg = rate_relative_neg(sat, res.range_corrected, time_arrival, usr_vel,
           res.los_neg[0], res.los_neg[1], res.los_neg[2]);
 
-      return res;
+#if 0
+      // TODO consider case when standard deviation of pseudorange measurement is provided by receiver
+      if(!this->range_sigma(measurement, res.range_sigma)){
+        // If receiver's range variance is not provided
+        res.range_sigma = 1E0; // TODO range error variance [m]
+      }
+#endif
 
+      return res;
     }
 
     /**

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

@@ -232,6 +232,9 @@ struct GPS_Solver_Base {
     xyz_t (*impl_velocity)(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 &);
+    const void *impl_error; ///< pointer to actual implementation of error model
+    float_t (*impl_range_sigma)(const void *, const gps_time_t &);
+    float_t (*impl_rate_sigma)(const void *, const gps_time_t &);
     inline bool is_available() const {
       return (impl_xyz != NULL) && (impl_t != NULL);
     }
@@ -267,6 +270,26 @@ struct GPS_Solver_Base {
     inline float_t clock_error_dot(const gps_time_t &t_tx) const {
       return impl_clock_error_dot(impl_t, t_tx);
     }
+    /**
+     * Return expected user range accuracy (URA) in standard deviation (1-sigma)
+     * at the transmission time.
+     * @param t_tx transmission time
+     */
+    inline float_t range_sigma(const gps_time_t &t_tx) const {
+      return impl_range_sigma
+          ? impl_range_sigma(impl_error, t_tx)
+          : 3.5; // 7.0 [m] of 95% (2-sigma) URE in Sec. 3.4.1 of April 2020 GPS SPS PS;
+    }
+    /**
+     * Return expected user range rate accuracy (URRA) in standard deviation (1-sigma)
+     * at the transmission time.
+     * @param t_tx transmission time
+     */
+    inline float_t rate_sigma(const gps_time_t &t_tx) const {
+      return impl_rate_sigma
+          ? impl_rate_sigma(impl_error, t_tx)
+          : 0.003; // 0.006 [m/s] of 95% (2-sigma) URRE in Sec. 3.4.2 of April 2020 GPS SPS PS
+    }
     static const satellite_t &unavailable() {
       struct impl_t {
         static xyz_t v3(const void *, const gps_time_t &, const float_t &){
@@ -276,7 +299,9 @@ struct GPS_Solver_Base {
           return float_t(0);
         }
       };
-      static const satellite_t res = {NULL, NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v};
+      static const satellite_t res = {
+          NULL, NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v,
+          NULL, NULL, NULL};
       return res;
     }
   };
@@ -359,7 +384,7 @@ struct GPS_Solver_Base {
   }
 
   struct relative_property_t {
-    float_t weight; ///< How useful this information is. square value is required; thus, only positive value activates the other values.
+    float_t range_sigma; ///< Standard deviation of pseudorange; If zero or negative, other values are invalid.
     float_t range_corrected; ///< corrected range just including delay, and excluding receiver/satellite error
     float_t range_residual; ///< residual range
     float_t rate_relative_neg; /// relative rate
@@ -514,6 +539,9 @@ protected:
      * @param G_ original design matrix
      * @param rotation_matrix 3 by 3 rotation matrix
      * @return transformed design matrix G'
+     * @see Eq.(3) and (10) in https://gssc.esa.int/navipedia/index.php/Positioning_Error,
+     * however, be careful that their R is used as both range error covariance in Eq.(1)
+     * and rotation matrix in Eq.(3).
      */
     static matrix_t rotate_G(const MatrixT &G_, const matrix_t &rotation_matrix){
       unsigned int r(G_.rows()), c(G_.columns()); // Normally c=4
@@ -529,17 +557,15 @@ protected:
     }
 
     /**
-     * Calculate C matrix, which is required to obtain DOP
-     * C = G^t * W * G
-     *
+     * Calculate C matrix, where C = G^t * G to be used for DOP calculation
      * @return C matrix
      */
     matrix_t C() const {
-      return (G.transpose() * W * G).inverse();
+      return (G.transpose() * G).inverse();
     }
     /**
-     * Transform coordinate of matrix C, which will be used to calculate HDOP/VDOP
-     * C' = (G * R^t)^t W * (G * R^t) = R * G^t * W * G * R^t = R * C * R^t,
+     * Transform coordinate of matrix C
+     * C' = (G * R^t)^t * (G * R^t) = R * G^t * G * R^t = R * C * R^t,
      * where R is a rotation matrix, for example, ECEF to ENU.
      *
      * @param rotation_matrix 3 by 3 rotation matrix
@@ -804,14 +830,14 @@ protected:
             res.receiver_error, time_arrival,
             res.user_position, zero));
 
-        if(prop.weight <= 0){
+        if(prop.range_sigma <= 0){
           continue; // intentionally excluded satellite
         }else{
           res.used_satellite_mask.set(it->prn);
         }
 
         if(coarse_estimation){
-          prop.weight = 1;
+          prop.range_sigma = 1;
         }else{
           idx_rate_rel.push_back(std::make_pair(i_measurement, prop.rate_relative_neg));
         }
@@ -820,7 +846,7 @@ protected:
         geomat.G(i_row, 0) = prop.los_neg[0];
         geomat.G(i_row, 1) = prop.los_neg[1];
         geomat.G(i_row, 2) = prop.los_neg[2];
-        geomat.W(i_row, i_row) = prop.weight;
+        geomat.W(i_row, i_row) = 1. / std::pow(prop.range_sigma, 2);
 
         ++i_row;
       }

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

@@ -353,7 +353,6 @@ struct RINEX_NAV {
     int t_oc_year4, t_oc_year2, t_oc_mon12;
     FloatT t_oc_sec;
     FloatT t_oe_WN;
-    FloatT ura_meter;
     FloatT t_ot;  ///< Transmitting time [s]
     FloatT fit_interval_hr;
     FloatT dummy;
@@ -367,7 +366,6 @@ struct RINEX_NAV {
         t_oc_mon12(t_oc_tm.tm_mon + 1),
         t_oc_sec(std::fmod(eph.t_oc, 60)),
         t_oe_WN(eph.WN),
-        ura_meter(ephemeris_t::URA_meter(eph.URA)),
         t_ot(0), // TODO
         fit_interval_hr(eph.fit_interval / (60 * 60)),
         dummy(0) {
@@ -378,8 +376,6 @@ struct RINEX_NAV {
       eph.WN = t_oc.week;
       eph.t_oc = t_oc.seconds;
 
-      eph.URA = ephemeris_t::URA_index(ura_meter); // meter to index
-
       /* @see ftp://igs.org/pub/data/format/rinex210.txt
        * 6.7 Satellite Health
        * RINEX Value:   0    Health OK
@@ -1434,14 +1430,14 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v2[] = {
-  GEN_E ( 3, 19, 12, message_t, ura_meter),
+  GEN_E ( 3, 19, 12, message_t, eph.URA),
   GEN_E2(22, 19, 12, message_t, eph.SV_health, unsigned int),
   GEN_E (41, 19, 12, message_t, eph.t_GD),
   GEN_E2(60, 19, 12, message_t, eph.iodc, int),
 };
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v3[] = {
-  GEN_E ( 4, 19, 12, message_t, ura_meter),
+  GEN_E ( 4, 19, 12, message_t, eph.URA),
   GEN_E2(23, 19, 12, message_t, eph.SV_health, unsigned int),
   GEN_E (42, 19, 12, message_t, eph.t_GD),
   GEN_E2(61, 19, 12, message_t, eph.iodc, int),

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

@@ -1711,7 +1711,7 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
           uint_t WN;              ///< Week number
 
           float_t t_0;            ///< Time of applicability (s) <= time of a week
-          int_t URA;              ///< User range accuracy (index)
+          float_t URA;            ///< User range accuracy (m)
           float_t x, y, z;        ///< ECEF position (m)
           float_t dx, dy, dz;     ///< ECEF velocity (m/s)
           float_t ddx, ddy, ddz;  ///< ECEF acceleration (m/s^2)
@@ -1804,6 +1804,15 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
             return res;
           }
 
+          static const float_t URA_table[15];
+
+          inline static float_t URA_meter(const int_t &index){
+            return gps_space_node_t::SatelliteProperties::Ephemeris::URA_meter(index, URA_table);
+          }
+          inline static int_t URA_index(const float_t &meter){
+            return gps_space_node_t::SatelliteProperties::Ephemeris::URA_index(meter, URA_table);
+          }
+
           struct raw_t {
             u8_t  svid;           ///< Satellite number
 
@@ -1850,7 +1859,7 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
               converted.svid = svid;
               converted.WN = 0; // Week number (must be configured later) @see adjust_time
 
-              converted.URA = URA;
+              converted.URA = URA_meter(URA);
               CONVERT(t_0);     // Time of a day => time of a week (must be configured later) @see adjust_time
               CONVERT2(x, xy);      CONVERT2(y, xy);      CONVERT(z);
               CONVERT2(dx, dxy);    CONVERT2(dy, dxy);    CONVERT(dz);
@@ -1869,7 +1878,7 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
 #define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
               svid = eph.svid;
 
-              URA = eph.URA;
+              URA = URA_index(eph.URA);
               CONVERT3(t_0, std::fmod(t_0, gps_time_t::seconds_day), t_0);
               CONVERT2(x, xy);      CONVERT2(y, xy);      CONVERT(z);
               CONVERT2(dx, dxy);    CONVERT2(dy, dxy);    CONVERT(dz);
@@ -2350,4 +2359,23 @@ const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::UTC_Param
 
 #undef POWER_2
 
+template <class FloatT>
+const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::URA_table[] = {
+  2,
+  2.8,
+  4,
+  5.7,
+  8,
+  11.3,
+  16,
+  32,
+  64,
+  128,
+  256,
+  512,
+  1024,
+  2048,
+  4096,
+};
+
 #endif /* __SBAS_H__ */

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

@@ -126,11 +126,15 @@ class SBAS_SinglePositioning : public SolverBaseT {
           static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
             return sat(ptr).ephemeris().clock_error_dot(t_tx);
           }
+          static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().URA;
+          }
         };
         satellite_t res = {
-            &(it_sat->second), &(it_sat->second),
+            &(it_sat->second), &(it_sat->second), // position, time
             impl_t::position, impl_t::velocity,
-            impl_t::clock_error, impl_t::clock_error_dot};
+            impl_t::clock_error, impl_t::clock_error_dot,
+            &(it_sat->second), impl_t::range_sigma, NULL}; // error model
         return res;
       }
       satellites_t(const space_node_t &sn)
@@ -245,7 +249,7 @@ class SBAS_SinglePositioning : public SolverBaseT {
       float_t range;
       range_error_t range_error;
       if(!this->range(measurement, range, &range_error)){
-        return res; // If no range entry, return with weight = 0
+        return res; // If no range entry, return with sigma = 0
       }
 
       satellite_t sat(select_satellite(prn, time_arrival));
@@ -297,21 +301,29 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
       // TODO Fast corrections (2.1.1.4.12)
 
-      // TODO Setup weight
-      if(std::abs(res.range_residual) > _options.residual_mask){
-        // If residual is too big, gently exclude it by decreasing its weight.
-        res.weight = 1E-8;
-      }else{
+      // Setup weight
+      res.range_sigma = 1E+4; // sufficiently big value, 1E4 [m]
+      do{
+        // If residual is too big, gently exclude it.
+        if(std::abs(res.range_residual) > _options.residual_mask){break;}
 
         float_t elv(relative_pos.elevation());
         if(elv < _options.elevation_mask){
-          res.weight = 0; // exclude it when elevation is less than threshold
-        }else{
-          // elevation weight based on "GPS���p�v���O���~���O" @see GPS_Solver.h
-          res.weight = std::pow(sin(elv)/0.8, 2);
-          if(res.weight < 1E-3){res.weight = 1E-3;}
+          res.range_sigma = 0; // exclude it when elevation is less than threshold
+          break;
         }
-      }
+
+        res.range_sigma = sat.range_sigma(t_tx);
+
+        /* elevation weight based on "GPS���p�v���O���~���O"
+         * elevation[deg] :   90    53    45    30    15    10    5
+         * sf_sigma(k)    :   0.80  1.00  1.13  1.60  3.09  4.61  9.18
+         * weight(k^-2)   :   1.56  1.00  0.78  0.39  0.10  0.05  0.01
+         */
+        static const float_t max_sf(10);
+        static const float_t elv_limit(std::asin(0.8/max_sf)); // limit
+        res.range_sigma *= (elv > elv_limit) ? (0.8 / sin(elv)) : max_sf;
+      }while(false);
 
       res.range_corrected = range;
 

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

@@ -185,9 +185,10 @@ struct SP3_Product {
         }
       };
       typename GPS_Solver_Base<FloatT>::satellite_t res = {
-        this, this,
+        this, this, // position, time
         impl_t::position, impl_t::velocity,
-        impl_t::clock_error, impl_t::clock_error_dot
+        impl_t::clock_error, impl_t::clock_error_dot,
+        NULL, NULL, NULL, // TODO error model
       };
       return res;
     }

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

@@ -1124,10 +1124,13 @@ struct GPS_RangeCorrector
         static const int prop_items(sizeof(res.values) / sizeof(res.values[0]));
         VALUE hook(rb_hash_lookup(hooks, key));
         if(NIL_P(hook)){break;}
+        FloatT weight((res.prop.range_sigma > 0) 
+            ? (1. / std::pow(res.prop.range_sigma, 2)) // weight=1/(sigma^2)
+            : res.prop.range_sigma);
         VALUE values[] = {
             SWIG_From(int)(prn), // prn
             rb_ary_new_from_args(prop_items, // relative_property
-              swig::from(res.prop.weight),
+              swig::from(weight),
               swig::from(res.prop.range_corrected),
               swig::from(res.prop.range_residual),
               swig::from(res.prop.rate_relative_neg),
@@ -1159,6 +1162,9 @@ struct GPS_RangeCorrector
                   .append(" @ [").append(std::to_string(i)).append("]"));
           }
         }
+        if(res.values[0] > 0){
+          res.values[0] = std::pow(1. / res.values[0], 0.5); // sigma=(1/weight)^0.5
+        }
       }while(false);
 #endif
       return res.prop;

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

@@ -800,20 +800,20 @@ __RINEX_CLK_TEXT__
       expect(pvt.position_solved?).to be(true)
       expect(pvt.receiver_time.to_a).to eq([1849, 172413])
       expect(pvt.llh.to_a).to eq([:lat, :lng, :alt].collect{|k| pvt.llh.send(k)})
-      expect(pvt.llh.lat / Math::PI * 180).to be_within(1E-9).of(35.6992591268) # latitude
-      expect(pvt.llh.lng / Math::PI * 180).to be_within(1E-9).of(139.541502292) # longitude
-      expect(pvt.llh.alt)                 .to be_within(1E-4).of(104.279402455) # altitude
-      expect(pvt.receiver_error).to be_within(1E-4).of(1259087.83603)
-      expect(pvt.gdop).to be_within(1E-10).of(3.83282723293)
-      expect(pvt.pdop).to be_within(1E-10).of(3.30873220653)
-      expect(pvt.hdop).to be_within(1E-10).of(2.05428293774)
-      expect(pvt.vdop).to be_within(1E-10).of(2.59376761222)
-      expect(pvt.tdop).to be_within(1E-10).of(1.9346461648)
+      expect(pvt.llh.lat / Math::PI * 180).to be_within(1E-3).of(35.7) # latitude
+      expect(pvt.llh.lng / Math::PI * 180).to be_within(1E-3).of(139.542) # longitude
+      expect(pvt.llh.alt)                 .to be_within(1E-1).of(104.3) # altitude
+      expect(pvt.receiver_error).to be_within(1E-1).of(1259087.8)
+      expect(pvt.gdop).to be_within(1E-2).of(2.42)
+      expect(pvt.pdop).to be_within(1E-2).of(2.17)
+      expect(pvt.hdop).to be_within(1E-2).of(1.11)
+      expect(pvt.vdop).to be_within(1E-2).of(1.87)
+      expect(pvt.tdop).to be_within(1E-2).of(1.08)
       expect(pvt.velocity.to_a).to eq([:e, :n, :u].collect{|k| pvt.velocity.send(k)})
-      expect(pvt.velocity.north).to be_within(1E-7).of(-0.839546227836) # north
-      expect(pvt.velocity.east) .to be_within(1E-7).of(-1.05805616381)  # east
-      expect(pvt.velocity.down) .to be_within(1E-7).of(-0.12355474006)  # down
-      expect(pvt.receiver_error_rate).to be_within(1E-7).of(-1061.92654151)
+      expect(pvt.velocity.north).to be_within(1E-2).of(-0.86) # north
+      expect(pvt.velocity.east) .to be_within(1E-2).of(-1.10) # east
+      expect(pvt.velocity.down) .to be_within(1E-2).of(-0.22) # down
+      expect(pvt.receiver_error_rate).to be_within(1E-2).of(-1061.86)
       expect(pvt.G.rows).to eq(6)
       expect(pvt.W.rows).to eq(6)
       expect(pvt.delta_r.rows).to eq(6)

data/lib/gps_pvt/receiver.rb

@@ -150,10 +150,6 @@ class Receiver
 
   def initialize(options = {})
     @solver = GPS::Solver::new
-    @solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel|
-      rel_prop[0] = 1 if rel_prop[0] > 0 # weight = 1
-      rel_prop
-    }
     @solver.options = {
       :skip_exclusion => true, # default is to skip fault exclusion calculation
     }
@@ -180,7 +176,7 @@ class Receiver
         next true
       when :weight
         case v.to_sym
-        when :elevation # (same as underneath C++ library)
+        when :elevation # (same as underneath C++ library except for ignoring broadcasted/calculated URA)
           @solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel|
             if rel_prop[0] > 0 then
               elv = Coordinate::ENU::relative_rel(
@@ -190,7 +186,11 @@ class Receiver
             rel_prop
           }
           next true
-        when :identical # same as default
+        when :identical # treat each satellite range having same accuracy
+          @solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel|
+            rel_prop[0] = 1 if rel_prop[0] > 0 # weight = 1
+            rel_prop
+          }
           next true
         end
       when :elevation_mask_deg

data/lib/gps_pvt/version.rb

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

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.7.1
+  version: 0.7.2
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
 autorequire: 
 bindir: exe
 cert_chain: []
-date: 2022-08-25 00:00:00.000000000 Z
+date: 2022-09-07 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rake