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.
- checksums.yaml +4 -4
- data/README.md +2 -1
- data/Rakefile +1 -0
- data/exe/gps_pvt +4 -2
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +993 -296
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +2 -3
- data/ext/ninja-scan-light/tool/navigation/ANTEX.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +4 -11
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +25 -13
- data/ext/ninja-scan-light/tool/navigation/GPS.h +21 -18
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +42 -25
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +44 -23
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +29 -1
- data/ext/ninja-scan-light/tool/navigation/MagneticField.h +4 -3
- data/ext/ninja-scan-light/tool/navigation/NTCM.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +31 -20
- data/ext/ninja-scan-light/tool/navigation/RINEX_Clock.h +458 -0
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +34 -6
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +26 -14
- data/ext/ninja-scan-light/tool/navigation/SP3.h +51 -43
- data/ext/ninja-scan-light/tool/param/bit_array.h +2 -2
- data/ext/ninja-scan-light/tool/param/quaternion.h +2 -2
- data/ext/ninja-scan-light/tool/param/vector3.h +2 -2
- data/ext/ninja-scan-light/tool/swig/GPS.i +124 -52
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1 -2
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +134 -13
- data/lib/gps_pvt/receiver.rb +32 -7
- data/lib/gps_pvt/version.rb +1 -1
- metadata +3 -2
@@ -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(
|
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(
|
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
|
-
|
818
|
-
|
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
|
-
|
1319
|
-
|
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
|
-
|
297
|
-
|
298
|
-
|
299
|
-
|
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.
|
304
|
-
|
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
|
-
|
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
|
-
|
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 <
|
1070
|
-
?
|
1071
|
-
:
|
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
|
-
|
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(
|
1077
|
-
if(meter <=
|
1080
|
+
for(std::size_t i(0); i < N; ++i){
|
1081
|
+
if(meter <= table[i]){return i;}
|
1078
1082
|
}
|
1079
|
-
return
|
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 &
|
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
|
262
|
-
* When
|
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
|
-
|
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
|
-
|
310
|
-
|
311
|
-
residual
|
312
|
-
|
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.
|
317
|
-
|
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
|
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
|
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.
|
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
|
-
|
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 *
|
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
|
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(
|
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(
|
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(
|
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(
|
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 = {
|
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
|
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
|
-
|
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,
|
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() *
|
564
|
+
return (G.transpose() * G).inverse();
|
544
565
|
}
|
545
566
|
/**
|
546
|
-
* Transform coordinate of matrix C
|
547
|
-
* C' = (G * R^t)^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.
|
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.
|
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.
|
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(
|
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)) *
|
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;
|