gps_pvt 0.10.0 → 0.10.1
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +5 -4
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +837 -52
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +11 -3
- data/ext/ninja-scan-light/tool/navigation/GPS.h +2 -1
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +20 -16
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +84 -46
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +2 -1
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +10 -2
- data/ext/ninja-scan-light/tool/swig/GPS.i +76 -5
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +7 -7
- data/lib/gps_pvt/receiver/extension.rb +0 -73
- data/lib/gps_pvt/receiver.rb +5 -4
- data/lib/gps_pvt/rtcm3.rb +4 -4
- data/lib/gps_pvt/supl.rb +3 -3
- data/lib/gps_pvt/version.rb +1 -1
- metadata +2 -2
@@ -134,7 +134,9 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
134
134
|
return sat(ptr).clock_error_dot(t_tx);
|
135
135
|
}
|
136
136
|
static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
|
137
|
-
return
|
137
|
+
return std::sqrt(
|
138
|
+
std::pow(sat(ptr).ephemeris().F_T, 2)
|
139
|
+
+ std::pow(4.5 / 1.96, 2)); // TODO change? (currently same as GPS)
|
138
140
|
}
|
139
141
|
};
|
140
142
|
satellite_t res = {
|
@@ -229,7 +231,7 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
229
231
|
* @param prn satellite number
|
230
232
|
* @param measurement measurement (per satellite) containing pseudo range
|
231
233
|
* @param receiver_error (temporal solution of) receiver clock error in meter
|
232
|
-
* @param time_arrival time when signal
|
234
|
+
* @param time_arrival time when signal arrives at receiver
|
233
235
|
* @param usr_pos (temporal solution of) user position
|
234
236
|
* @param usr_vel (temporal solution of) user velocity
|
235
237
|
* @return (relative_property_t) relative information
|
@@ -325,11 +327,17 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
325
327
|
res.range_corrected = range;
|
326
328
|
|
327
329
|
xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
|
328
|
-
|
329
330
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
330
331
|
+ res.los_neg[1] * rel_vel.y()
|
331
332
|
+ res.los_neg[2] * rel_vel.z()
|
332
333
|
+ sat.clock_error_dot(t_tx) * c;
|
334
|
+
res.rate_sigma = sat.rate_sigma(time_arrival);
|
335
|
+
|
336
|
+
if(_options.use_external_sigma){
|
337
|
+
// Use standard deviation of pseudorange and/or its rate if they are provided by receiver
|
338
|
+
this->range_sigma(measurement, res.range_sigma);
|
339
|
+
this->rate_sigma(measurement, res.rate_sigma);
|
340
|
+
}
|
333
341
|
|
334
342
|
return res;
|
335
343
|
}
|
@@ -1156,7 +1156,8 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
|
|
1156
1156
|
|
1157
1157
|
// Subframe.1
|
1158
1158
|
uint_t WN; ///< Week number
|
1159
|
-
float_t URA;
|
1159
|
+
float_t URA; ///< User range accuracy (m) including all errors
|
1160
|
+
///< for which the Space and Control Segments are responsible. (ICD 6.2.1)
|
1160
1161
|
uint_t SV_health; ///< Health status
|
1161
1162
|
int_t iodc; ///< Issue of clock data
|
1162
1163
|
float_t t_GD; ///< Group delay (s)
|
@@ -55,10 +55,12 @@ template <class FloatT>
|
|
55
55
|
struct GPS_Solver_GeneralOptions {
|
56
56
|
FloatT elevation_mask;
|
57
57
|
FloatT residual_mask;
|
58
|
+
bool use_external_sigma;
|
58
59
|
|
59
60
|
GPS_Solver_GeneralOptions()
|
60
61
|
: elevation_mask(0), // elevation mask default is 0 [deg]
|
61
|
-
residual_mask(
|
62
|
+
residual_mask(1E3), // range residual mask is 1000 [m]
|
63
|
+
use_external_sigma(false) { // default is to ignore external sigma inputs
|
62
64
|
|
63
65
|
}
|
64
66
|
};
|
@@ -158,7 +160,9 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
158
160
|
return sat(ptr).clock_error_dot(t_tx);
|
159
161
|
}
|
160
162
|
static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
|
161
|
-
return
|
163
|
+
return std::sqrt(
|
164
|
+
std::pow(sat(ptr).ephemeris().URA, 2)
|
165
|
+
+ std::pow(4.5 / 1.96, 2)); // UEE of modern receiver Table.B2-1 of April 2020 GPS SPS PS;
|
162
166
|
}
|
163
167
|
};
|
164
168
|
satellite_t res = {
|
@@ -262,9 +266,9 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
262
266
|
*
|
263
267
|
* @param sat satellite
|
264
268
|
* @param range "corrected" pseudo range subtracted by (temporal solution of) receiver clock error in meter
|
265
|
-
* @param time_arrival time when signal
|
269
|
+
* @param time_arrival time when signal arrives at receiver
|
266
270
|
* @param usr_pos (temporal solution of) user position
|
267
|
-
* @param residual calculated residual with line of
|
271
|
+
* @param residual calculated residual with line of sight vector, and pseudorange standard deviation (sigma);
|
268
272
|
* When sigma is equal to or less than zero, the calculated results should not be used.
|
269
273
|
* @param error Some correction can be overwritten. If its unknown_flag is zero,
|
270
274
|
* corrections will be skipped as possible. @see range_errors_t
|
@@ -280,7 +284,7 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
280
284
|
|
281
285
|
static const float_t &c(space_node_t::light_speed);
|
282
286
|
|
283
|
-
//
|
287
|
+
// Satellite clock error correction
|
284
288
|
range += ((error.unknown_flag & range_error_t::SATELLITE_CLOCK)
|
285
289
|
? (sat.clock_error(time_arrival - range / c) * c)
|
286
290
|
: error.value[range_error_t::SATELLITE_CLOCK]);
|
@@ -294,20 +298,21 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
294
298
|
// Calculate residual
|
295
299
|
residual.residual = range - geometric_range;
|
296
300
|
|
297
|
-
// Setup design matrix
|
301
|
+
// Setup design matrix (direction is negative; satellite -> user)
|
298
302
|
residual.los_neg_x = -(sat_pos.x() - usr_pos.xyz.x()) / geometric_range;
|
299
303
|
residual.los_neg_y = -(sat_pos.y() - usr_pos.xyz.y()) / geometric_range;
|
300
304
|
residual.los_neg_z = -(sat_pos.z() - usr_pos.xyz.z()) / geometric_range;
|
301
305
|
|
302
306
|
enu_t relative_pos(enu_t::relative(sat_pos, usr_pos.xyz));
|
303
307
|
|
308
|
+
// Ionospheric correction
|
304
309
|
if(error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
|
305
310
|
residual.residual += ionospheric_correction(time_arrival, usr_pos, relative_pos);
|
306
311
|
}else{
|
307
312
|
residual.residual += error.value[range_error_t::IONOSPHERIC];
|
308
313
|
}
|
309
314
|
|
310
|
-
// Tropospheric
|
315
|
+
// Tropospheric correction
|
311
316
|
residual.residual += (error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
|
312
317
|
? tropospheric_correction(time_arrival, usr_pos, relative_pos)
|
313
318
|
: error.value[range_error_t::TROPOSPHERIC];
|
@@ -347,9 +352,9 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
347
352
|
* @param range "corrected" pseudo range subtracted by (temporal solution of) receiver clock error in meter
|
348
353
|
* @param time_arrival time when signal arrive at receiver
|
349
354
|
* @param usr_vel (temporal solution of) user velocity
|
350
|
-
* @param los_neg_x line of
|
351
|
-
* @param los_neg_y line of
|
352
|
-
* @param los_neg_z line of
|
355
|
+
* @param los_neg_x line of sight X
|
356
|
+
* @param los_neg_y line of sight Y
|
357
|
+
* @param los_neg_z line of sight Z
|
353
358
|
* @return (float_t) relative rate.
|
354
359
|
*/
|
355
360
|
float_t rate_relative_neg(
|
@@ -425,14 +430,13 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
425
430
|
usr_pos, residual, range_error);
|
426
431
|
res.rate_relative_neg = rate_relative_neg(sat, res.range_corrected, time_arrival, usr_vel,
|
427
432
|
res.los_neg[0], res.los_neg[1], res.los_neg[2]);
|
433
|
+
res.rate_sigma = sat.rate_sigma(time_arrival);
|
428
434
|
|
429
|
-
|
430
|
-
|
431
|
-
|
432
|
-
|
433
|
-
res.range_sigma = 1E0; // TODO range error variance [m]
|
435
|
+
if(_options.use_external_sigma){
|
436
|
+
// Use standard deviation of pseudorange and/or its rate if they are provided by receiver
|
437
|
+
this->range_sigma(measurement, res.range_sigma);
|
438
|
+
this->rate_sigma(measurement, res.rate_sigma);
|
434
439
|
}
|
435
|
-
#endif
|
436
440
|
|
437
441
|
return res;
|
438
442
|
}
|
@@ -148,7 +148,7 @@ struct GPS_Solver_Base {
|
|
148
148
|
* of measurement_t::mapped_type
|
149
149
|
* @param values key-value map
|
150
150
|
* @param key key
|
151
|
-
* @param buf buffer
|
151
|
+
* @param buf buffer for storing found value, and preserving original value if value is not found
|
152
152
|
* @return (float_t *) When value is found, pointer of buf will be returned.
|
153
153
|
* Otherwise, NULL is returned.
|
154
154
|
*/
|
@@ -209,8 +209,13 @@ struct GPS_Solver_Base {
|
|
209
209
|
if((res = find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){
|
210
210
|
|
211
211
|
}else if((res = find_value(values, measurement_items_t::L1_DOPPLER, buf))){
|
212
|
-
// Fall back to
|
213
|
-
|
212
|
+
// Fall back to Doppler
|
213
|
+
float_t freq;
|
214
|
+
if(find_value(values, measurement_items_t::L1_FREQUENCY, freq)){
|
215
|
+
buf *= -(space_node_t::light_speed / freq);
|
216
|
+
}else{
|
217
|
+
buf *= -space_node_t::L1_WaveLength();
|
218
|
+
}
|
214
219
|
}
|
215
220
|
return res;
|
216
221
|
}
|
@@ -221,8 +226,13 @@ struct GPS_Solver_Base {
|
|
221
226
|
if((res = find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){
|
222
227
|
|
223
228
|
}else if((res = find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, buf))){
|
224
|
-
// Fall back to
|
225
|
-
|
229
|
+
// Fall back to Doppler
|
230
|
+
float_t freq;
|
231
|
+
if(find_value(values, measurement_items_t::L1_FREQUENCY, freq)){
|
232
|
+
buf *= (space_node_t::light_speed / freq);
|
233
|
+
}else{
|
234
|
+
buf *= space_node_t::L1_WaveLength();
|
235
|
+
}
|
226
236
|
}
|
227
237
|
return res;
|
228
238
|
}
|
@@ -279,7 +289,8 @@ struct GPS_Solver_Base {
|
|
279
289
|
inline float_t range_sigma(const gps_time_t &t_tx) const {
|
280
290
|
return impl_range_sigma
|
281
291
|
? impl_range_sigma(impl_error, t_tx)
|
282
|
-
:
|
292
|
+
: (std::sqrt((9.7 * 9.7) + (4.5 * 4.5)) / 1.96); // 5.45 [m]
|
293
|
+
// UERE (User Equivalent Range Error) of modern receiver in Eq.(B-8) of April 2020 GPS SPS PS;
|
283
294
|
}
|
284
295
|
/**
|
285
296
|
* Return expected user range rate accuracy (URRA) in standard deviation (1-sigma)
|
@@ -289,7 +300,12 @@ struct GPS_Solver_Base {
|
|
289
300
|
inline float_t rate_sigma(const gps_time_t &t_tx) const {
|
290
301
|
return impl_rate_sigma
|
291
302
|
? impl_rate_sigma(impl_error, t_tx)
|
292
|
-
:
|
303
|
+
: 5E-1; // (Empirical value)
|
304
|
+
// Alternatives (may occur failure);
|
305
|
+
// 1) (std::sqrt(((2.0 * 2.0) + (1.0 * 1.0)) * 2) / 1.96); // 1.61 [m/s]
|
306
|
+
// assume differential range with error consisting of "Receiver noise and resolution"
|
307
|
+
// and "Other user segment errors" in UEE Table B.2-1 of April 2020 GPS SPS PS
|
308
|
+
// 2) 0.006 m/s(95%) SPS SIS URRE in Table 3.4-2 is not considered.
|
293
309
|
}
|
294
310
|
static const satellite_t &unavailable() {
|
295
311
|
struct impl_t {
|
@@ -388,8 +404,9 @@ struct GPS_Solver_Base {
|
|
388
404
|
float_t range_sigma; ///< Standard deviation of pseudorange; If zero or negative, other values are invalid.
|
389
405
|
float_t range_corrected; ///< corrected range just including delay, and excluding receiver/satellite error
|
390
406
|
float_t range_residual; ///< residual range
|
391
|
-
float_t
|
392
|
-
float_t
|
407
|
+
float_t rate_sigma; ///< Standard deviation of range rate;
|
408
|
+
float_t rate_relative_neg; /// relative rate (taking negative value if range is increasing)
|
409
|
+
float_t los_neg[3]; ///< line of sight vector from satellite to user in ECEF coordinate
|
393
410
|
};
|
394
411
|
|
395
412
|
/**
|
@@ -433,16 +450,16 @@ struct GPS_Solver_Base {
|
|
433
450
|
float_t receiver_error;
|
434
451
|
enu_t user_velocity_enu;
|
435
452
|
float_t receiver_error_rate;
|
436
|
-
struct
|
453
|
+
struct precision_t {
|
437
454
|
float_t g, p, h, v, t;
|
438
|
-
|
439
|
-
|
440
|
-
: g(std::sqrt(
|
441
|
-
p(std::sqrt(
|
442
|
-
h(std::sqrt(
|
443
|
-
v(std::sqrt(
|
444
|
-
t(std::sqrt(
|
445
|
-
} dop;
|
455
|
+
precision_t() {}
|
456
|
+
precision_t(const matrix_t &C_or_P_enu)
|
457
|
+
: g(std::sqrt(C_or_P_enu.trace())),
|
458
|
+
p(std::sqrt(C_or_P_enu.partial(3, 3).trace())),
|
459
|
+
h(std::sqrt(C_or_P_enu.partial(2, 2).trace())),
|
460
|
+
v(std::sqrt(C_or_P_enu(2, 2))),
|
461
|
+
t(std::sqrt(C_or_P_enu(3, 3))) {}
|
462
|
+
} dop, sigma_pos, sigma_vel;
|
446
463
|
unsigned int used_satellites;
|
447
464
|
typedef BitArray<0x400> satellite_mask_t;
|
448
465
|
satellite_mask_t used_satellite_mask; ///< bit pattern(use=1, otherwise=0), PRN 1(LSB) to 32 for GPS
|
@@ -565,27 +582,48 @@ protected:
|
|
565
582
|
return (G.transpose() * G).inverse();
|
566
583
|
}
|
567
584
|
/**
|
568
|
-
*
|
569
|
-
*
|
585
|
+
* Calculate P matrix, where P = E[x^t x] = (G^t * W * G)^{-1} to be used for estimated accuracy calculation
|
586
|
+
* @return P matrix
|
587
|
+
*/
|
588
|
+
matrix_t P() const {
|
589
|
+
return (G.transpose() * W * G).inverse();
|
590
|
+
}
|
591
|
+
/**
|
592
|
+
* Transform coordinate of matrix C/P
|
593
|
+
* C' = ((G * R^t)^t * (G * R^t))^-1 = (R * G^t * G * R^t)^-1
|
594
|
+
* = (R^t)^-1 * (G^t * G)^-1 * R^-1 = R * C * R^t,
|
595
|
+
* P' = ((G * R^t)^t * W * (G * R^t))^-1 = R * P * R^t,
|
570
596
|
* where R is a rotation matrix, for example, ECEF to ENU.
|
571
597
|
*
|
572
598
|
* @param rotation_matrix 3 by 3 rotation matrix
|
573
|
-
* @return transformed matrix C'
|
599
|
+
* @return transformed matrix C' or P'
|
574
600
|
* @see rotate_G
|
575
601
|
*/
|
576
|
-
static matrix_t
|
577
|
-
unsigned int n(
|
602
|
+
static matrix_t rotate_CP(const matrix_t &C_or_P, const matrix_t &rotation_matrix){
|
603
|
+
unsigned int n(C_or_P.rows()); // Normally n=4
|
578
604
|
matrix_t res(n, n);
|
579
605
|
res.partial(3, 3).replace( // upper left
|
580
|
-
rotation_matrix *
|
606
|
+
rotation_matrix * C_or_P.partial(3, 3) * rotation_matrix.transpose());
|
581
607
|
res.partial(3, n - 3, 0, 3).replace( // upper right
|
582
|
-
rotation_matrix *
|
608
|
+
rotation_matrix * C_or_P.partial(3, n - 3, 0, 3));
|
583
609
|
res.partial(n - 3, 3, 3, 0).replace( // lower left
|
584
|
-
|
610
|
+
C_or_P.partial(n - 3, 3, 3, 0) * rotation_matrix.transpose());
|
585
611
|
res.partial(n - 3, n - 3, 3, 3).replace( // lower right
|
586
|
-
|
612
|
+
C_or_P.partial(n - 3, n - 3, 3, 3));
|
587
613
|
return res;
|
588
614
|
}
|
615
|
+
typename user_pvt_t::precision_t dop(){
|
616
|
+
return typename user_pvt_t::precision_t(C());
|
617
|
+
}
|
618
|
+
typename user_pvt_t::precision_t dop(const matrix_t &rotation_matrix){
|
619
|
+
return typename user_pvt_t::precision_t(rotate_CP(C(), rotation_matrix));
|
620
|
+
}
|
621
|
+
typename user_pvt_t::precision_t sigma(){
|
622
|
+
return typename user_pvt_t::precision_t(P());
|
623
|
+
}
|
624
|
+
typename user_pvt_t::precision_t sigma(const matrix_t &rotation_matrix){
|
625
|
+
return typename user_pvt_t::precision_t(rotate_CP(P(), rotation_matrix));
|
626
|
+
}
|
589
627
|
/**
|
590
628
|
* Solve x of linear equation (y = G x + v) to minimize sigma{v^t * v}
|
591
629
|
* where v =~ N(0, sigma), and y and G are observation delta (=delta_r variable)
|
@@ -594,7 +632,7 @@ protected:
|
|
594
632
|
*
|
595
633
|
* 4 by row(y) S matrix (=(G^t * W * G)^{-1} * (G^t * W)) will be used to calculate protection level
|
596
634
|
* to investigate relationship between bias on each satellite and solution.
|
597
|
-
* residual v = (I -
|
635
|
+
* residual v = (I - G S) y, which is irrelevant to rotation,
|
598
636
|
* because P = G R R^{t} S = G' S'.
|
599
637
|
*
|
600
638
|
* @param S (output) coefficient matrix to calculate solution, i.e., (G^t * W * G)^{-1} * (G^t * W)
|
@@ -701,14 +739,13 @@ protected:
|
|
701
739
|
delta_r.circular(offset, 0, size, 1));
|
702
740
|
}
|
703
741
|
template <class MatrixT2>
|
704
|
-
void
|
742
|
+
void copy_G_rowwise(const linear_solver_t<MatrixT2> &src,
|
705
743
|
const unsigned int &i_src, const unsigned int &i_dst){
|
706
744
|
unsigned int c_dst(G.columns()), c_src(src.G.columns()),
|
707
745
|
c(c_dst > c_src ? c_src : c_dst); // Normally c=4
|
708
746
|
for(unsigned int j(0); j < c; ++j){
|
709
747
|
G(i_dst, j) = src.G(i_src, j);
|
710
748
|
}
|
711
|
-
W(i_dst, i_dst) = src.W(i_src, i_src);
|
712
749
|
}
|
713
750
|
};
|
714
751
|
|
@@ -806,15 +843,15 @@ protected:
|
|
806
843
|
res.receiver_error = receiver_error_init;
|
807
844
|
|
808
845
|
geometric_matrices_t geomat(measurement.size());
|
809
|
-
typedef std::vector<std::pair<unsigned int,
|
810
|
-
|
811
|
-
|
846
|
+
typedef std::vector<std::pair<unsigned int, relative_property_t> > index_prop_t;
|
847
|
+
index_prop_t idx_prop; // [(index of measurement, relative property), ...]
|
848
|
+
idx_prop.reserve(measurement.size());
|
812
849
|
|
813
850
|
// If initialization is not appropriate, more iteration will be performed.
|
814
851
|
bool converged(false);
|
815
852
|
for(int i_trial(-opt.warmup); i_trial < opt.max_trial; i_trial++){
|
816
853
|
|
817
|
-
|
854
|
+
idx_prop.clear();
|
818
855
|
unsigned int i_row(0), i_measurement(0);
|
819
856
|
res.used_satellite_mask.clear();
|
820
857
|
|
@@ -841,7 +878,7 @@ protected:
|
|
841
878
|
if(coarse_estimation){
|
842
879
|
prop.range_sigma = 1;
|
843
880
|
}else{
|
844
|
-
|
881
|
+
idx_prop.push_back(std::make_pair(i_measurement, prop));
|
845
882
|
}
|
846
883
|
|
847
884
|
geomat.delta_r(i_row, 0) = prop.range_residual;
|
@@ -872,9 +909,11 @@ protected:
|
|
872
909
|
return;
|
873
910
|
}
|
874
911
|
|
912
|
+
matrix_t rot(res.user_position.ecef2enu());
|
875
913
|
try{
|
876
|
-
|
877
|
-
|
914
|
+
typename geometric_matrices_t::partial_t geomat_used(geomat.partial(res.used_satellites));
|
915
|
+
res.dop = geomat_used.dop(rot);
|
916
|
+
res.sigma_pos = geomat_used.sigma(rot);
|
878
917
|
}catch(const std::runtime_error &e){ // expect to detect matrix operation error
|
879
918
|
res.error_code = user_pvt_t::ERROR_DOP;
|
880
919
|
return;
|
@@ -892,7 +931,7 @@ protected:
|
|
892
931
|
geometric_matrices_t geomat2(res.used_satellites);
|
893
932
|
int i_range(0), i_rate(0);
|
894
933
|
|
895
|
-
for(typename
|
934
|
+
for(typename index_prop_t::const_iterator it(idx_prop.begin()), it_end(idx_prop.end());
|
896
935
|
it != it_end;
|
897
936
|
++it, ++i_range){
|
898
937
|
|
@@ -901,9 +940,10 @@ protected:
|
|
901
940
|
*(measurement[it->first].k_v_map), // const version of measurement[PRN]
|
902
941
|
rate))){continue;}
|
903
942
|
|
904
|
-
// Copy design matrix and set rate
|
905
|
-
geomat2.
|
906
|
-
geomat2.delta_r(i_rate, 0) = rate + it->second;
|
943
|
+
// Copy design matrix and set rate and weight
|
944
|
+
geomat2.copy_G_rowwise(geomat, i_range, i_rate);
|
945
|
+
geomat2.delta_r(i_rate, 0) = rate + it->second.rate_relative_neg;
|
946
|
+
geomat2.W(i_rate, i_rate) = 1. / std::pow(it->second.rate_sigma, 2);
|
907
947
|
|
908
948
|
++i_rate;
|
909
949
|
}
|
@@ -915,12 +955,14 @@ protected:
|
|
915
955
|
|
916
956
|
try{
|
917
957
|
// Least square
|
918
|
-
|
958
|
+
typename geometric_matrices_t::partial_t geomat2_used(geomat2.partial(i_rate));
|
959
|
+
matrix_t sol(geomat2_used.least_square());
|
919
960
|
|
920
961
|
xyz_t vel_xyz(sol.partial(3, 1, 0, 0));
|
921
962
|
res.user_velocity_enu = enu_t::relative_rel(
|
922
963
|
vel_xyz, res.user_position.llh);
|
923
964
|
res.receiver_error_rate = sol(3, 0);
|
965
|
+
res.sigma_vel = geomat2_used.sigma(rot);
|
924
966
|
}catch(const std::runtime_error &e){ // expect to detect matrix operation error
|
925
967
|
res.error_code = user_pvt_t::ERROR_VELOCITY_LS;
|
926
968
|
return;
|
@@ -1145,10 +1187,6 @@ public:
|
|
1145
1187
|
solver_interface_t<GPS_Solver_Base<FloatT> > solve() const {
|
1146
1188
|
return solver_interface_t<GPS_Solver_Base<FloatT> >(*this);
|
1147
1189
|
}
|
1148
|
-
|
1149
|
-
static typename user_pvt_t::dop_t dop(const matrix_t &C, const pos_t &user_position) {
|
1150
|
-
return typename user_pvt_t::dop_t(geometric_matrices_t::rotate_C(C, user_position.ecef2enu()));
|
1151
|
-
}
|
1152
1190
|
};
|
1153
1191
|
|
1154
1192
|
template <class FloatT>
|
@@ -56,7 +56,7 @@ struct GPS_PVT_RAIM_LSR : public PVT_BaseT {
|
|
56
56
|
typename GPS_Solver_Base<FloatT>::prn_t excluded;
|
57
57
|
typename GPS_Solver_Base<FloatT>::pos_t user_position;
|
58
58
|
typename GPS_Solver_Base<FloatT>::float_t receiver_error;
|
59
|
-
typename GPS_Solver_Base<FloatT>::user_pvt_t::
|
59
|
+
typename GPS_Solver_Base<FloatT>::user_pvt_t::precision_t dop, sigma_pos;
|
60
60
|
} FDE_min, FDE_2nd; ///< Fault exclusion
|
61
61
|
};
|
62
62
|
|
@@ -231,6 +231,7 @@ protected:
|
|
231
231
|
target->user_position = pvt_FDE.user_position;
|
232
232
|
target->receiver_error = pvt_FDE.receiver_error;
|
233
233
|
target->dop = pvt_FDE.dop;
|
234
|
+
target->sigma_pos = pvt_FDE.sigma_pos;
|
234
235
|
}
|
235
236
|
}
|
236
237
|
};
|
@@ -129,7 +129,9 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
129
129
|
return sat(ptr).ephemeris().clock_error_dot(t_tx);
|
130
130
|
}
|
131
131
|
static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) {
|
132
|
-
return
|
132
|
+
return std::sqrt(
|
133
|
+
std::pow(sat(ptr).ephemeris().URA, 2)
|
134
|
+
+ std::pow(4.5 / 1.96, 2)); // TODO change? (currently same as GPS)
|
133
135
|
}
|
134
136
|
};
|
135
137
|
satellite_t res = {
|
@@ -330,11 +332,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
330
332
|
res.range_corrected = range;
|
331
333
|
|
332
334
|
xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
|
333
|
-
|
334
335
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
335
336
|
+ res.los_neg[1] * rel_vel.y()
|
336
337
|
+ res.los_neg[2] * rel_vel.z()
|
337
338
|
+ sat.clock_error_dot(t_tx) * c;
|
339
|
+
res.rate_sigma = sat.rate_sigma(time_arrival);
|
340
|
+
|
341
|
+
if(_options.use_external_sigma){
|
342
|
+
// Use standard deviation of pseudorange and/or its rate if they are provided by receiver
|
343
|
+
this->range_sigma(measurement, res.range_sigma);
|
344
|
+
this->rate_sigma(measurement, res.rate_sigma);
|
345
|
+
}
|
338
346
|
|
339
347
|
return res;
|
340
348
|
}
|
@@ -368,6 +368,22 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
368
368
|
System_XYZ<FloatT, WGS84> position, velocity;
|
369
369
|
FloatT clock_error, clock_error_dot;
|
370
370
|
};
|
371
|
+
|
372
|
+
int get_URA_index() const {
|
373
|
+
return GPS_Ephemeris<FloatT>::URA_index(this->URA);
|
374
|
+
}
|
375
|
+
int set_URA_index(const int &idx) {
|
376
|
+
this->URA = GPS_Ephemeris<FloatT>::URA_meter(idx);
|
377
|
+
return get_URA_index();
|
378
|
+
}
|
379
|
+
FloatT set_fit_interval(const bool &flag) {
|
380
|
+
// set svid/iodc before invocation of this function
|
381
|
+
if((this->svid >= 193) && (this->svid <= 202)){ // QZSS
|
382
|
+
return (this->fit_interval = (flag ? (std::numeric_limits<FloatT>::max)() : (2 * 60 * 60)));
|
383
|
+
}else{
|
384
|
+
return (this->fit_interval = GPS_Ephemeris<FloatT>::raw_t::fit_interval(flag, this->iodc));
|
385
|
+
}
|
386
|
+
}
|
371
387
|
};
|
372
388
|
%}
|
373
389
|
%extend GPS_Ephemeris {
|
@@ -403,6 +419,9 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
403
419
|
MAKE_ACCESSOR(dot_Omega0, FloatT);
|
404
420
|
MAKE_ACCESSOR(dot_i0, FloatT);
|
405
421
|
|
422
|
+
%rename(%str(URA_index=)) set_URA_index;
|
423
|
+
%rename(%str(URA_index)) get_URA_index;
|
424
|
+
|
406
425
|
MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int));
|
407
426
|
%apply int *OUTPUT { int *subframe_no, int *iodc_or_iode };
|
408
427
|
void parse(const unsigned int buf[10], int *subframe_no, int *iodc_or_iode){
|
@@ -597,11 +616,18 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
597
616
|
return SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
|
598
617
|
}
|
599
618
|
GPS_Time<FloatT> t_applicable() const {return GPS_Time<FloatT>(this->WN, this->t_0);}
|
619
|
+
int get_URA_index() const {
|
620
|
+
return SBAS_Ephemeris<FloatT>::URA_index(this->URA);
|
621
|
+
}
|
622
|
+
int set_URA_index(const int &idx) {
|
623
|
+
this->URA = SBAS_Ephemeris<FloatT>::URA_meter(idx);
|
624
|
+
return get_URA_index();
|
625
|
+
}
|
600
626
|
};
|
601
627
|
%}
|
602
628
|
%extend SBAS_Ephemeris {
|
603
629
|
MAKE_ACCESSOR(svid, unsigned int);
|
604
|
-
|
630
|
+
|
605
631
|
MAKE_ACCESSOR(WN, unsigned int);
|
606
632
|
MAKE_ACCESSOR(t_0, FloatT);
|
607
633
|
MAKE_ACCESSOR(URA, FloatT);
|
@@ -610,6 +636,9 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
610
636
|
MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
|
611
637
|
MAKE_ACCESSOR(a_Gf0, FloatT);
|
612
638
|
MAKE_ACCESSOR(a_Gf1, FloatT);
|
639
|
+
%rename(%str(URA_index=)) set_URA_index;
|
640
|
+
%rename(%str(URA_index)) get_URA_index;
|
641
|
+
|
613
642
|
/**
|
614
643
|
* Return broadcasted raw data of SBAS ephemeris
|
615
644
|
* @param buf_brdc pointer to store raw data of Type 9 message.
|
@@ -711,6 +740,21 @@ struct GLONASS_Ephemeris
|
|
711
740
|
(typename GLONASS_SpaceNode<FloatT>::TimeProperties)(*this)),
|
712
741
|
deltaT));
|
713
742
|
}
|
743
|
+
|
744
|
+
unsigned char get_F_T_index() const {
|
745
|
+
return GLONASS_Ephemeris<FloatT>::F_T_index();
|
746
|
+
}
|
747
|
+
unsigned char set_F_T_index(const unsigned char &idx) {
|
748
|
+
this->F_T = GLONASS_Ephemeris<FloatT>::raw_t::F_T_value(idx);
|
749
|
+
return get_F_T_index();
|
750
|
+
}
|
751
|
+
unsigned char get_P1_index() const {
|
752
|
+
return GLONASS_Ephemeris<FloatT>::P1_index();
|
753
|
+
}
|
754
|
+
unsigned char set_P1_index(const unsigned char &idx) {
|
755
|
+
this->P1 = GLONASS_Ephemeris<FloatT>::raw_t::P1_value(idx);
|
756
|
+
return get_P1_index();
|
757
|
+
}
|
714
758
|
};
|
715
759
|
%}
|
716
760
|
%extend GLONASS_Ephemeris {
|
@@ -743,12 +787,27 @@ struct GLONASS_Ephemeris
|
|
743
787
|
MAKE_ACCESSOR2(year, date.year, int);
|
744
788
|
MAKE_ACCESSOR2(day_of_year, date.day_of_year, int);
|
745
789
|
|
790
|
+
%rename(%str(F_T_index=)) set_F_T_index;
|
791
|
+
%rename(%str(F_T_index)) get_F_T_index;
|
792
|
+
%rename(%str(P1_index=)) set_P1_index;
|
793
|
+
%rename(%str(P1_index)) get_P1_index;
|
794
|
+
|
746
795
|
void set_date(const unsigned int &N_4, const unsigned int &NA) {
|
747
796
|
self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::raw_t::raw2date(N_4, NA);
|
748
797
|
}
|
749
798
|
void set_date(const std::tm &t) {
|
750
799
|
self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::date_t::from_c_tm(t);
|
751
800
|
}
|
801
|
+
unsigned char N_4() const {
|
802
|
+
unsigned char res;
|
803
|
+
GLONASS_Ephemeris<FloatT>::TimeProperties::raw_t::date2raw(self->date, &res, NULL);
|
804
|
+
return res;
|
805
|
+
}
|
806
|
+
unsigned short NA() const {
|
807
|
+
unsigned short res;
|
808
|
+
GLONASS_Ephemeris<FloatT>::TimeProperties::raw_t::date2raw(self->date, NULL, &res);
|
809
|
+
return res;
|
810
|
+
}
|
752
811
|
|
753
812
|
FloatT frequency_L1() const {
|
754
813
|
return self->L1_frequency();
|
@@ -939,6 +998,10 @@ struct GPS_User_PVT
|
|
939
998
|
const FloatT &hdop() const {return base_t::dop.h;}
|
940
999
|
const FloatT &vdop() const {return base_t::dop.v;}
|
941
1000
|
const FloatT &tdop() const {return base_t::dop.t;}
|
1001
|
+
const FloatT &hsigma() const {return base_t::sigma_pos.h;}
|
1002
|
+
const FloatT &vsigma() const {return base_t::sigma_pos.v;}
|
1003
|
+
const FloatT &tsigma() const {return base_t::sigma_pos.t;}
|
1004
|
+
const FloatT &vel_sigma() const {return base_t::sigma_vel.p;}
|
942
1005
|
const unsigned int &used_satellites() const {return base_t::used_satellites;}
|
943
1006
|
std::vector<int> used_satellite_list() const {return base_t::used_satellite_mask.indices_one();}
|
944
1007
|
bool position_solved() const {return base_t::position_solved();}
|
@@ -963,7 +1026,7 @@ struct GPS_User_PVT
|
|
963
1026
|
return linear_solver().C();
|
964
1027
|
}
|
965
1028
|
Matrix<FloatT, Array2D_Dense<FloatT> > C_enu() const {
|
966
|
-
return proxy_t::linear_solver_t::
|
1029
|
+
return proxy_t::linear_solver_t::rotate_CP(C(), base_t::user_position.ecef2enu());
|
967
1030
|
}
|
968
1031
|
Matrix<FloatT, Array2D_Dense<FloatT> > S() const {
|
969
1032
|
Matrix<FloatT, Array2D_Dense<FloatT> > res;
|
@@ -1193,6 +1256,7 @@ struct GPS_Measurement {
|
|
1193
1256
|
%extend GPS_SolverOptions_Common {
|
1194
1257
|
MAKE_ACCESSOR2(elevation_mask, cast_general()->elevation_mask, FloatT);
|
1195
1258
|
MAKE_ACCESSOR2(residual_mask, cast_general()->residual_mask, FloatT);
|
1259
|
+
MAKE_ACCESSOR2(use_external_sigma, cast_general()->use_external_sigma, bool);
|
1196
1260
|
MAKE_VECTOR2ARRAY(int);
|
1197
1261
|
%ignore cast_general;
|
1198
1262
|
}
|
@@ -1375,7 +1439,7 @@ struct HookableSolver : public BaseT {
|
|
1375
1439
|
const GPS_Solver<FloatT>::base_t::relative_property_t &res_orig) const {
|
1376
1440
|
union {
|
1377
1441
|
base_t::relative_property_t prop;
|
1378
|
-
FloatT values[
|
1442
|
+
FloatT values[8];
|
1379
1443
|
} res = {res_orig};
|
1380
1444
|
#ifdef SWIGRUBY
|
1381
1445
|
do{
|
@@ -1383,15 +1447,19 @@ struct HookableSolver : public BaseT {
|
|
1383
1447
|
static const int prop_items(sizeof(res.values) / sizeof(res.values[0]));
|
1384
1448
|
VALUE hook(rb_hash_lookup(hooks, key));
|
1385
1449
|
if(NIL_P(hook)){break;}
|
1386
|
-
FloatT
|
1450
|
+
FloatT weight_range((res.prop.range_sigma > 0)
|
1387
1451
|
? (1. / std::pow(res.prop.range_sigma, 2)) // weight=1/(sigma^2)
|
1388
1452
|
: res.prop.range_sigma);
|
1453
|
+
FloatT weight_rate((res.prop.rate_sigma > 0)
|
1454
|
+
? (1. / std::pow(res.prop.rate_sigma, 2)) // weight=1/(sigma^2)
|
1455
|
+
: res.prop.rate_sigma);
|
1389
1456
|
VALUE values[] = {
|
1390
1457
|
SWIG_From(int)(prn), // prn
|
1391
1458
|
rb_ary_new_from_args(prop_items, // relative_property
|
1392
|
-
swig::from(
|
1459
|
+
swig::from(weight_range),
|
1393
1460
|
swig::from(res.prop.range_corrected),
|
1394
1461
|
swig::from(res.prop.range_residual),
|
1462
|
+
swig::from(weight_rate),
|
1395
1463
|
swig::from(res.prop.rate_relative_neg),
|
1396
1464
|
swig::from(res.prop.los_neg[0]),
|
1397
1465
|
swig::from(res.prop.los_neg[1]),
|
@@ -1424,6 +1492,9 @@ struct HookableSolver : public BaseT {
|
|
1424
1492
|
if(res.values[0] > 0){
|
1425
1493
|
res.values[0] = std::pow(1. / res.values[0], 0.5); // sigma=(1/weight)^0.5
|
1426
1494
|
}
|
1495
|
+
if(res.values[3] > 0){
|
1496
|
+
res.values[3] = std::pow(1. / res.values[3], 0.5); // sigma=(1/weight)^0.5
|
1497
|
+
}
|
1427
1498
|
}while(false);
|
1428
1499
|
#endif
|
1429
1500
|
return res.prop;
|