gps_pvt 0.6.4 → 0.7.2

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -3002,7 +3002,6 @@ struct MatrixUtil {
3002
3002
  }
3003
3003
  static VALUE read(
3004
3004
  const VALUE &v, const unsigned int &row = 0, const unsigned int &column = 0) {
3005
- int state;
3006
3005
  VALUE values[3] = {v, UINT2NUM(row), UINT2NUM(column)};
3007
3006
  return funcall_throw_if_error(run, reinterpret_cast<VALUE>(values));
3008
3007
  }
@@ -3355,7 +3354,7 @@ SWIGINTERN std::string Matrix_Frozen_Sl_double_Sc_Array2D_Dense_Sl_double_Sg__Sc
3355
3354
  static const ID with_index[] = {
3356
3355
  rb_intern("map_with_index"), rb_intern("map_with_index!"),
3357
3356
  rb_intern("collect_with_index"), rb_intern("collect_with_index!")};
3358
- for(int i(0); i < sizeof(with_index) / sizeof(with_index[0]); ++i){
3357
+ for(std::size_t i(0); i < sizeof(with_index) / sizeof(with_index[0]); ++i){
3359
3358
  if(id_callee == with_index[i]){
3360
3359
  return matrix_yield_get_with_index;
3361
3360
  }
@@ -3829,7 +3828,7 @@ SWIGINTERN std::string Matrix_Frozen_Sl_Complex_Sl_double_Sg__Sc_Array2D_Dense_S
3829
3828
  static const ID with_index[] = {
3830
3829
  rb_intern("map_with_index"), rb_intern("map_with_index!"),
3831
3830
  rb_intern("collect_with_index"), rb_intern("collect_with_index!")};
3832
- for(int i(0); i < sizeof(with_index) / sizeof(with_index[0]); ++i){
3831
+ for(std::size_t i(0); i < sizeof(with_index) / sizeof(with_index[0]); ++i){
3833
3832
  if(id_callee == with_index[i]){
3834
3833
  return matrix_yield_get_with_index;
3835
3834
  }
@@ -604,7 +604,7 @@ class ANTEX_Reader {
604
604
  typedef typename ANTEX_Product<FloatT>::antenna_t antenna_t;
605
605
  antenna_t *antenna;
606
606
  typedef typename ANTEX_Product<FloatT>::per_freq_t per_freq_t;
607
- per_freq_t *per_freq;
607
+ per_freq_t *per_freq(NULL); // NULL fot suppression of GCC uninitialized variable warning
608
608
 
609
609
  while(src.has_next()){
610
610
  parsed_t parsed(src.parse_line());
@@ -814,11 +814,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
814
814
  static const float_t F_T_table[15]; ///< @see Table 4.4
815
815
 
816
816
  static const float_t F_T_value(const u8_t &F_T_){
817
- if(F_T_ >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){
818
- return -1; // not used
819
- }else{
820
- return F_T_table[F_T_];
821
- }
817
+ return GPS_SpaceNode<float_t>::SatelliteProperties::Ephemeris::URA_meter(
818
+ F_T_, F_T_table);
822
819
  }
823
820
 
824
821
  static const uint_t P1_value(const u8_t &P1_){
@@ -1315,12 +1312,8 @@ typename GLONASS_SpaceNode<FloatT>::u8_t GLONASS_SpaceNode<FloatT>::SatellitePro
1315
1312
  if(F_T <= 0){ // invalid value
1316
1313
  return sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]);
1317
1314
  }
1318
- u8_t res(0);
1319
- while(res < (sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]))){
1320
- if(F_T <= raw_t::F_T_table[res]){break;}
1321
- ++res;
1322
- }
1323
- return res;
1315
+ return (u8_t)GPS_SpaceNode<float_t>::SatelliteProperties::Ephemeris::URA_index(
1316
+ F_T, raw_t::F_T_table);
1324
1317
  }
1325
1318
 
1326
1319
  template <class FloatT>
@@ -131,11 +131,15 @@ class GLONASS_SinglePositioning : public SolverBaseT {
131
131
  static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
132
132
  return sat(ptr).clock_error_dot(t_tx);
133
133
  }
134
+ static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
135
+ return sat(ptr).ephemeris().F_T;
136
+ }
134
137
  };
135
138
  satellite_t res = {
136
- &(it_sat->second),
139
+ &(it_sat->second), &(it_sat->second), // position, time
137
140
  impl_t::position, impl_t::velocity,
138
- impl_t::clock_error, impl_t::clock_error_dot};
141
+ impl_t::clock_error, impl_t::clock_error_dot,
142
+ &(it_sat->second), impl_t::range_sigma, NULL}; // error model
139
143
  return res;
140
144
  }
141
145
  satellites_t(const space_node_t &sn)
@@ -292,21 +296,29 @@ class GLONASS_SinglePositioning : public SolverBaseT {
292
296
  res.range_residual += range_error.value[range_error_t::IONOSPHERIC];
293
297
  }
294
298
 
295
- // Setup weight
296
- if(std::abs(res.range_residual) > _options.residual_mask){
297
- // If residual is too big, gently exclude it by decreasing its weight.
298
- res.weight = 1E-8;
299
- }else{
299
+ // Setup range standard deviation, whose reciprocal square is used as weight
300
+ res.range_sigma = 1E+4; // sufficiently big value, 1E4 [m]
301
+ do{
302
+ // If residual is too big, gently exclude it.
303
+ if(std::abs(res.range_residual) > _options.residual_mask){break;}
300
304
 
301
305
  float_t elv(relative_pos.elevation());
302
306
  if(elv < _options.elevation_mask){
303
- res.weight = 0; // exclude it when elevation is less than threshold
304
- }else{
305
- // elevation weight based on "GPS���p�v���O���~���O" @see GPS_Solver.h
306
- res.weight = std::pow(sin(elv)/0.8, 2);
307
- if(res.weight < 1E-3){res.weight = 1E-3;}
307
+ res.range_sigma = 0; // exclude it when elevation is less than threshold
308
+ break;
308
309
  }
309
- }
310
+
311
+ res.range_sigma = sat.range_sigma(t_tx);
312
+
313
+ /* elevation weight based on "GPS���p�v���O���~���O"
314
+ * elevation[deg] : 90 53 45 30 15 10 5
315
+ * sf_sigma(k) : 0.80 1.00 1.13 1.60 3.09 4.61 9.18
316
+ * weight(k^-2) : 1.56 1.00 0.78 0.39 0.10 0.05 0.01
317
+ */
318
+ static const float_t max_sf(10);
319
+ static const float_t elv_limit(std::asin(0.8/max_sf)); // limit
320
+ res.range_sigma *= (elv > elv_limit) ? (0.8 / sin(elv)) : max_sf;
321
+ }while(false);
310
322
 
311
323
  res.range_corrected = range;
312
324
 
@@ -990,7 +990,7 @@ static s ## bits ## _t name(const InputT *buf){ \
990
990
 
991
991
  // Subframe.1
992
992
  uint_t WN; ///< Week number
993
- int_t URA; ///< User range accuracy (index)
993
+ float_t URA; ///< User range accuracy (m)
994
994
  uint_t SV_health; ///< Health status
995
995
  int_t iodc; ///< Issue of clock data
996
996
  float_t t_GD; ///< Group delay (s)
@@ -1061,22 +1061,29 @@ static s ## bits ## _t name(const InputT *buf){ \
1061
1061
  return !((delta_t >= 0) && (delta_t < transmission_interval));
1062
1062
  }
1063
1063
 
1064
- static const float_t URA_limits[];
1065
- static const int URA_MAX_INDEX;
1064
+ static const float_t URA_limits[15];
1066
1065
 
1067
- static float_t URA_meter(const int_t &index){
1066
+ template <std::size_t N>
1067
+ static float_t URA_meter(const int_t &index, const float_t (&table)[N]){
1068
1068
  if(index < 0){return -1;}
1069
- return (index < URA_MAX_INDEX)
1070
- ? URA_limits[index]
1071
- : URA_limits[URA_MAX_INDEX - 1] * 2;
1069
+ return (index < N)
1070
+ ? table[index]
1071
+ : table[N - 1] * 2;
1072
+ }
1073
+ inline static float_t URA_meter(const int_t &index){
1074
+ return URA_meter(index, URA_limits);
1072
1075
  }
1073
1076
 
1074
- static int_t URA_index(const float_t &meter){
1077
+ template <std::size_t N>
1078
+ static int_t URA_index(const float_t &meter, const float_t (&table)[N]){
1075
1079
  if(meter < 0){return -1;}
1076
- for(int i(0); i < URA_MAX_INDEX; ++i){
1077
- if(meter <= URA_limits[i]){return i;}
1080
+ for(std::size_t i(0); i < N; ++i){
1081
+ if(meter <= table[i]){return i;}
1078
1082
  }
1079
- return URA_MAX_INDEX;
1083
+ return N;
1084
+ }
1085
+ inline static int_t URA_index(const float_t &meter){
1086
+ return URA_index(meter, URA_limits);
1080
1087
  }
1081
1088
 
1082
1089
  float_t eccentric_anomaly(const float_t &period_from_toe) const {
@@ -1399,7 +1406,7 @@ static s ## bits ## _t name(const InputT *buf){ \
1399
1406
  converted.svid = svid;
1400
1407
 
1401
1408
  converted.WN = WN;
1402
- converted.URA = URA;
1409
+ converted.URA = URA_meter(URA);
1403
1410
  converted.SV_health = SV_health;
1404
1411
  converted.iodc = iodc;
1405
1412
  CONVERT(t_GD);
@@ -1438,7 +1445,7 @@ static s ## bits ## _t name(const InputT *buf){ \
1438
1445
  svid = eph.svid;
1439
1446
 
1440
1447
  WN = eph.WN;
1441
- URA = eph.URA;
1448
+ URA = URA_index(eph.URA);
1442
1449
  SV_health = eph.SV_health;
1443
1450
  iodc = eph.iodc;
1444
1451
  CONVERT(t_GD);
@@ -1475,7 +1482,7 @@ static s ## bits ## _t name(const InputT *buf){ \
1475
1482
  bool is_equivalent(const Ephemeris &eph) const {
1476
1483
  do{
1477
1484
  if(WN != eph.WN){break;}
1478
- if(URA != eph.URA){break;}
1485
+ if(URA_index(URA) != URA_index(eph.URA)){break;}
1479
1486
  if(SV_health != eph.SV_health){break;}
1480
1487
 
1481
1488
  #define CHECK(TARGET) \
@@ -2478,10 +2485,6 @@ const typename GPS_SpaceNode<FloatT>::float_t GPS_SpaceNode<FloatT>::SatellitePr
2478
2485
  6144.00,
2479
2486
  };
2480
2487
 
2481
- template <class FloatT>
2482
- const int GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::URA_MAX_INDEX
2483
- = sizeof(URA_limits) / sizeof(URA_limits[0]);
2484
-
2485
2488
  #ifdef POW2_ALREADY_DEFINED
2486
2489
  #undef POW2_ALREADY_DEFINED
2487
2490
  #else
@@ -155,11 +155,15 @@ class GPS_SinglePositioning : public SolverBaseT {
155
155
  static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
156
156
  return sat(ptr).clock_error_dot(t_tx);
157
157
  }
158
+ static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
159
+ return sat(ptr).ephemeris().URA;
160
+ }
158
161
  };
159
162
  satellite_t res = {
160
- &(it_sat->second),
163
+ &(it_sat->second), &(it_sat->second), // position, clock
161
164
  impl_t::position, impl_t::velocity,
162
- impl_t::clock_error, impl_t::clock_error_dot};
165
+ impl_t::clock_error, impl_t::clock_error_dot,
166
+ &(it_sat->second), impl_t::range_sigma, NULL}; // error model
163
167
  return res;
164
168
  }
165
169
  satellites_t(const space_node_t &sn)
@@ -248,7 +252,7 @@ class GPS_SinglePositioning : public SolverBaseT {
248
252
  float_t &los_neg_x;
249
253
  float_t &los_neg_y;
250
254
  float_t &los_neg_z;
251
- float_t &weight;
255
+ float_t &range_sigma;
252
256
  };
253
257
 
254
258
  /**
@@ -258,8 +262,8 @@ class GPS_SinglePositioning : public SolverBaseT {
258
262
  * @param range "corrected" pseudo range subtracted by (temporal solution of) receiver clock error in meter
259
263
  * @param time_arrival time when signal arrive at receiver
260
264
  * @param usr_pos (temporal solution of) user position
261
- * @param residual calculated residual with line of site vector, and weight;
262
- * When weight is equal to or less than zero, the calculated results should not be used.
265
+ * @param residual calculated residual with line of site vector, and pseudorange standard deviation (sigma);
266
+ * When sigma is equal to or less than zero, the calculated results should not be used.
263
267
  * @param error Some correction can be overwritten. If its unknown_flag is zero,
264
268
  * corrections will be skipped as possible. @see range_errors_t
265
269
  * @return (float_t) corrected range just including delay, and excluding receiver/satellite error.
@@ -281,7 +285,8 @@ class GPS_SinglePositioning : public SolverBaseT {
281
285
 
282
286
  // Calculate satellite position
283
287
  float_t dt_transit(range / c);
284
- xyz_t sat_pos(sat.position(time_arrival - dt_transit, dt_transit));
288
+ gps_time_t time_depature(time_arrival - dt_transit);
289
+ xyz_t sat_pos(sat.position(time_depature, dt_transit));
285
290
  float_t geometric_range(usr_pos.xyz.dist(sat_pos));
286
291
 
287
292
  // Calculate residual
@@ -305,25 +310,30 @@ class GPS_SinglePositioning : public SolverBaseT {
305
310
  ? tropospheric_correction(time_arrival, usr_pos, relative_pos)
306
311
  : error.value[range_error_t::TROPOSPHERIC];
307
312
 
308
- // Setup weight
309
- if(std::abs(residual.residual) > _options.residual_mask){
310
- // If residual is too big, gently exclude it by decreasing its weight.
311
- residual.weight = 1E-8;
312
- }else{
313
+ // Setup range standard deviation, whose reciprocal square is used as weight
314
+ residual.range_sigma = 1E+4; // sufficiently big value, 1E4 [m]
315
+ do{
316
+ // If residual is too big, gently exclude it.
317
+ if(std::abs(residual.residual) > _options.residual_mask){break;}
313
318
 
314
319
  float_t elv(relative_pos.elevation());
315
320
  if(elv < _options.elevation_mask){
316
- residual.weight = 0; // exclude it when elevation is less than threshold
317
- }else{
318
- /* elevation weight based on "GPS���p�v���O���~���O"
319
- * elevation[deg] : 90 53 45 30 15 10 5
320
- * sigma(s) : 0.80 1.00 1.13 1.60 3.09 4.61 9.18
321
- * weight(s^-2) : 1.56 1.00 0.78 0.39 0.10 0.05 0.01
322
- */
323
- residual.weight = std::pow(sin(elv)/0.8, 2); // weight=1 @ elv=53[deg]
324
- if(residual.weight < 1E-3){residual.weight = 1E-3;}
321
+ residual.range_sigma = 0; // exclude it when elevation is less than threshold
322
+ break;
325
323
  }
326
- }
324
+
325
+ residual.range_sigma = sat.range_sigma(time_depature);
326
+
327
+ /* elevation weight based on "GPS���p�v���O���~���O"
328
+ * elevation[deg] : 90 53 45 30 15 10 5
329
+ * sf_sigma(k) : 0.80 1.00 1.13 1.60 3.09 4.61 9.18
330
+ * weight(k^-2) : 1.56 1.00 0.78 0.39 0.10 0.05 0.01
331
+ */
332
+ static const float_t max_sf(10);
333
+ static const float_t elv_limit(std::asin(0.8/max_sf)); // limit
334
+ residual.range_sigma *= (elv > elv_limit) ? (0.8 / sin(elv)) : max_sf;
335
+ }while(false);
336
+
327
337
 
328
338
  return range;
329
339
  }
@@ -396,16 +406,16 @@ class GPS_SinglePositioning : public SolverBaseT {
396
406
  float_t range;
397
407
  range_error_t range_error;
398
408
  if(!this->range(measurement, range, &range_error)){
399
- return res; // If no range entry, return with weight = 0
409
+ return res; // If no range entry, return with range_sigma = 0
400
410
  }
401
411
 
402
412
  satellite_t sat(select_satellite(prn, time_arrival));
403
- if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0
413
+ if(!sat.is_available()){return res;} // If satellite is unavailable, return with range_sigma = 0
404
414
 
405
415
  residual_t residual = {
406
416
  res.range_residual,
407
417
  res.los_neg[0], res.los_neg[1], res.los_neg[2],
408
- res.weight,
418
+ res.range_sigma,
409
419
  };
410
420
 
411
421
  res.range_corrected = range_corrected(
@@ -414,8 +424,15 @@ class GPS_SinglePositioning : public SolverBaseT {
414
424
  res.rate_relative_neg = rate_relative_neg(sat, res.range_corrected, time_arrival, usr_vel,
415
425
  res.los_neg[0], res.los_neg[1], res.los_neg[2]);
416
426
 
417
- return res;
427
+ #if 0
428
+ // TODO consider case when standard deviation of pseudorange measurement is provided by receiver
429
+ if(!this->range_sigma(measurement, res.range_sigma)){
430
+ // If receiver's range variance is not provided
431
+ res.range_sigma = 1E0; // TODO range error variance [m]
432
+ }
433
+ #endif
418
434
 
435
+ return res;
419
436
  }
420
437
 
421
438
  /**
@@ -227,13 +227,16 @@ struct GPS_Solver_Base {
227
227
  }
228
228
 
229
229
  struct satellite_t {
230
- const void *impl;
230
+ const void *impl_xyz, *impl_t; ///< pointer to actual implementation to return position and clock error
231
231
  xyz_t (*impl_position)(const void *, const gps_time_t &, const float_t &);
232
232
  xyz_t (*impl_velocity)(const void *, const gps_time_t &, const float_t &);
233
233
  float_t (*impl_clock_error)(const void *, const gps_time_t &);
234
234
  float_t (*impl_clock_error_dot)(const void *, const gps_time_t &);
235
+ const void *impl_error; ///< pointer to actual implementation of error model
236
+ float_t (*impl_range_sigma)(const void *, const gps_time_t &);
237
+ float_t (*impl_rate_sigma)(const void *, const gps_time_t &);
235
238
  inline bool is_available() const {
236
- return impl != NULL;
239
+ return (impl_xyz != NULL) && (impl_t != NULL);
237
240
  }
238
241
  /**
239
242
  * Return satellite position at the transmission time in EFEC.
@@ -244,28 +247,48 @@ struct GPS_Solver_Base {
244
247
  * that is, the transmission time added by the transit time.
245
248
  */
246
249
  inline xyz_t position(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
247
- return impl_position(impl, t_tx, dt_transit);
250
+ return impl_position(impl_xyz, t_tx, dt_transit);
248
251
  }
249
252
  /**
250
253
  * Return satellite velocity at the transmission time in EFEC.
251
254
  * @see position
252
255
  */
253
256
  inline xyz_t velocity(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
254
- return impl_velocity(impl, t_tx, dt_transit);
257
+ return impl_velocity(impl_xyz, t_tx, dt_transit);
255
258
  }
256
259
  /**
257
260
  * Return satellite clock error [s] at the transmission time.
258
261
  * @param t_tx transmission time
259
262
  */
260
263
  inline float_t clock_error(const gps_time_t &t_tx) const {
261
- return impl_clock_error(impl, t_tx);
264
+ return impl_clock_error(impl_t, t_tx);
262
265
  }
263
266
  /**
264
267
  * Return satellite clock error derivative [s/s] at the transmission time.
265
268
  * @param t_tx transmission time
266
269
  */
267
270
  inline float_t clock_error_dot(const gps_time_t &t_tx) const {
268
- return impl_clock_error_dot(impl, t_tx);
271
+ return impl_clock_error_dot(impl_t, t_tx);
272
+ }
273
+ /**
274
+ * Return expected user range accuracy (URA) in standard deviation (1-sigma)
275
+ * at the transmission time.
276
+ * @param t_tx transmission time
277
+ */
278
+ inline float_t range_sigma(const gps_time_t &t_tx) const {
279
+ return impl_range_sigma
280
+ ? impl_range_sigma(impl_error, t_tx)
281
+ : 3.5; // 7.0 [m] of 95% (2-sigma) URE in Sec. 3.4.1 of April 2020 GPS SPS PS;
282
+ }
283
+ /**
284
+ * Return expected user range rate accuracy (URRA) in standard deviation (1-sigma)
285
+ * at the transmission time.
286
+ * @param t_tx transmission time
287
+ */
288
+ inline float_t rate_sigma(const gps_time_t &t_tx) const {
289
+ return impl_rate_sigma
290
+ ? impl_rate_sigma(impl_error, t_tx)
291
+ : 0.003; // 0.006 [m/s] of 95% (2-sigma) URRE in Sec. 3.4.2 of April 2020 GPS SPS PS
269
292
  }
270
293
  static const satellite_t &unavailable() {
271
294
  struct impl_t {
@@ -276,7 +299,9 @@ struct GPS_Solver_Base {
276
299
  return float_t(0);
277
300
  }
278
301
  };
279
- static const satellite_t res = {NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v};
302
+ static const satellite_t res = {
303
+ NULL, NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v,
304
+ NULL, NULL, NULL};
280
305
  return res;
281
306
  }
282
307
  };
@@ -359,7 +384,7 @@ struct GPS_Solver_Base {
359
384
  }
360
385
 
361
386
  struct relative_property_t {
362
- float_t weight; ///< How useful this information is. square value is required; thus, only positive value activates the other values.
387
+ float_t range_sigma; ///< Standard deviation of pseudorange; If zero or negative, other values are invalid.
363
388
  float_t range_corrected; ///< corrected range just including delay, and excluding receiver/satellite error
364
389
  float_t range_residual; ///< residual range
365
390
  float_t rate_relative_neg; /// relative rate
@@ -474,12 +499,7 @@ struct GPS_Solver_Base {
474
499
  using super_t::reset;
475
500
  void reset(const int &prn) {set(prn, false);}
476
501
  std::vector<int> excluded() const {
477
- std::vector<int> res(super_t::indices_one());
478
- for(std::vector<int>::iterator it(res.begin()), it_end(res.end());
479
- it != it_end; ++it){
480
- *it += prn_begin;
481
- }
482
- return res;
502
+ return super_t::indices_one(prn_begin);
483
503
  }
484
504
  };
485
505
  };
@@ -519,6 +539,9 @@ protected:
519
539
  * @param G_ original design matrix
520
540
  * @param rotation_matrix 3 by 3 rotation matrix
521
541
  * @return transformed design matrix G'
542
+ * @see Eq.(3) and (10) in https://gssc.esa.int/navipedia/index.php/Positioning_Error,
543
+ * however, be careful that their R is used as both range error covariance in Eq.(1)
544
+ * and rotation matrix in Eq.(3).
522
545
  */
523
546
  static matrix_t rotate_G(const MatrixT &G_, const matrix_t &rotation_matrix){
524
547
  unsigned int r(G_.rows()), c(G_.columns()); // Normally c=4
@@ -534,17 +557,15 @@ protected:
534
557
  }
535
558
 
536
559
  /**
537
- * Calculate C matrix, which is required to obtain DOP
538
- * C = G^t * W * G
539
- *
560
+ * Calculate C matrix, where C = G^t * G to be used for DOP calculation
540
561
  * @return C matrix
541
562
  */
542
563
  matrix_t C() const {
543
- return (G.transpose() * W * G).inverse();
564
+ return (G.transpose() * G).inverse();
544
565
  }
545
566
  /**
546
- * Transform coordinate of matrix C, which will be used to calculate HDOP/VDOP
547
- * C' = (G * R^t)^t W * (G * R^t) = R * G^t * W * G * R^t = R * C * R^t,
567
+ * Transform coordinate of matrix C
568
+ * C' = (G * R^t)^t * (G * R^t) = R * G^t * G * R^t = R * C * R^t,
548
569
  * where R is a rotation matrix, for example, ECEF to ENU.
549
570
  *
550
571
  * @param rotation_matrix 3 by 3 rotation matrix
@@ -809,14 +830,14 @@ protected:
809
830
  res.receiver_error, time_arrival,
810
831
  res.user_position, zero));
811
832
 
812
- if(prop.weight <= 0){
833
+ if(prop.range_sigma <= 0){
813
834
  continue; // intentionally excluded satellite
814
835
  }else{
815
836
  res.used_satellite_mask.set(it->prn);
816
837
  }
817
838
 
818
839
  if(coarse_estimation){
819
- prop.weight = 1;
840
+ prop.range_sigma = 1;
820
841
  }else{
821
842
  idx_rate_rel.push_back(std::make_pair(i_measurement, prop.rate_relative_neg));
822
843
  }
@@ -825,7 +846,7 @@ protected:
825
846
  geomat.G(i_row, 0) = prop.los_neg[0];
826
847
  geomat.G(i_row, 1) = prop.los_neg[1];
827
848
  geomat.G(i_row, 2) = prop.los_neg[2];
828
- geomat.W(i_row, i_row) = prop.weight;
849
+ geomat.W(i_row, i_row) = 1. / std::pow(prop.range_sigma, 2);
829
850
 
830
851
  ++i_row;
831
852
  }
@@ -60,6 +60,12 @@ struct GPS_PVT_RAIM_LSR : public PVT_BaseT {
60
60
  } FDE_min, FDE_2nd; ///< Fault exclusion
61
61
  };
62
62
 
63
+ template <class FloatT>
64
+ struct GPS_Solver_RAIM_LSR_Options {
65
+ bool skip_exclusion;
66
+ GPS_Solver_RAIM_LSR_Options() : skip_exclusion(false) {}
67
+ };
68
+
63
69
  /*
64
70
  * Comment on implementation of protection level (PL) calculation
65
71
  *
@@ -96,6 +102,27 @@ struct GPS_Solver_RAIM_LSR : public SolverBaseT {
96
102
  inheritate_type(measurement2_t);
97
103
  #undef inheritate_type
98
104
 
105
+ typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
106
+ GPS_Solver_RAIM_LSR_Options<float_t>, super_t> options_t;
107
+
108
+ protected:
109
+ GPS_Solver_RAIM_LSR_Options<float_t> _options;
110
+
111
+ public:
112
+ options_t available_options() const {
113
+ return options_t(super_t::available_options(), _options);
114
+ }
115
+
116
+ options_t available_options(const options_t &opt_wish) const {
117
+ GPS_Solver_RAIM_LSR_Options<float_t> opt(opt_wish);
118
+ return options_t(super_t::available_options(opt_wish), opt);
119
+ }
120
+
121
+ options_t update_options(const options_t &opt_wish){
122
+ _options = opt_wish;
123
+ return options_t(super_t::update_options(opt_wish), _options);
124
+ }
125
+
99
126
  typedef GPS_PVT_RAIM_LSR<float_t, typename super_t::user_pvt_t> user_pvt_t;
100
127
 
101
128
  typename super_t::template solver_interface_t<self_t> solve() const {
@@ -155,7 +182,8 @@ protected:
155
182
  user_position_init, receiver_error_init,
156
183
  opt);
157
184
 
158
- if(!pvt.position_solved()
185
+ if(_options.skip_exclusion
186
+ || !pvt.position_solved()
159
187
  || (!pvt.FD.valid)
160
188
  || (pvt.used_satellites < 6)){return;}
161
189
 
@@ -143,16 +143,17 @@ class MagneticFieldGeneric {
143
143
  field_components_res_t res;
144
144
 
145
145
  // @see http://mooring.ucsd.edu/software/matlab/mfiles/toolbox/geo/IGRF/geomag60.c
146
+ // @see https://web.archive.org/web/20171008182325/http://mooring.ucsd.edu/software/matlab/mfiles/toolbox/geo/IGRF/geomag60.c
146
147
 
147
- const FloatT slat(sin_geocentric_latitude);
148
- const FloatT clat(cos_geocentric_latitude);
148
+ const FloatT &slat(sin_geocentric_latitude);
149
+ const FloatT &clat(cos_geocentric_latitude);
149
150
 
150
151
  FloatT sl[13] = {sin(longitude_rad)};
151
152
  FloatT cl[13] = {cos(longitude_rad)};
152
153
 
153
154
  res.north = res.east = res.down = 0;
154
155
 
155
- FloatT aa, bb, cc, rr;
156
+ FloatT aa, bb, cc, rr(0); // rr(0) for suppression of warning of use of uninitialized variable
156
157
 
157
158
  aa = sqrt(3.0);
158
159
  FloatT p[118] = {
@@ -66,7 +66,7 @@ class NTCM_GL_Generic {
66
66
  return cos_Chi_star3
67
67
  + (C[0] * std::cos(v_D)
68
68
  + C[1] * std::cos(v_SD) + C[2] * std::sin(v_SD)
69
- + C[3] * std::cos(v_TD) + C[4] * std::sin(v_TD)) * cos_Chi_star;
69
+ + C[3] * std::cos(v_TD) + C[4] * std::sin(v_TD)) * cos_Chi_star2;
70
70
  }
71
71
 
72
72
  static const float_t doy_A, doy_SA;