gps_pvt 0.6.0 → 0.6.3
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/exe/to_ubx +213 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +219 -514
- data/ext/ninja-scan-light/tool/algorithm/interpolate.h +106 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +37 -35
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +16 -13
- data/ext/ninja-scan-light/tool/navigation/GPS.h +29 -25
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +19 -12
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +31 -11
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +22 -10
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +21 -13
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +19 -16
- data/ext/ninja-scan-light/tool/navigation/SP3.h +45 -84
- data/ext/ninja-scan-light/tool/swig/GPS.i +33 -39
- data/gps_pvt.gemspec +3 -0
- data/lib/gps_pvt/receiver.rb +3 -3
- data/lib/gps_pvt/version.rb +5 -5
- metadata +6 -3
@@ -34,6 +34,9 @@
|
|
34
34
|
|
35
35
|
#include <cstddef>
|
36
36
|
#include <vector>
|
37
|
+
#include <map>
|
38
|
+
#include <algorithm>
|
39
|
+
#include <stdexcept>
|
37
40
|
|
38
41
|
/*
|
39
42
|
* perform Neville's interpolation (with derivative)
|
@@ -118,4 +121,107 @@ Ty interpolate_Neville(
|
|
118
121
|
return interpolate_Neville(x_given, y_given, x, y_buf, n)[0];
|
119
122
|
}
|
120
123
|
|
124
|
+
template <class Tx, class Ty, class Tx_delta = Tx>
|
125
|
+
struct InterpolatableSet {
|
126
|
+
struct condition_t {
|
127
|
+
std::size_t max_x_size; ///< maximum number of data used for interpolation
|
128
|
+
Tx_delta max_dx_range; ///< maximum acceptable absolute difference between x and x0
|
129
|
+
};
|
130
|
+
|
131
|
+
typedef typename std::vector<std::pair<Tx, Ty> > buffer_t;
|
132
|
+
buffer_t buf;
|
133
|
+
Tx x0, x_lower, x_upper;
|
134
|
+
std::vector<Tx_delta> dx;
|
135
|
+
bool ready;
|
136
|
+
|
137
|
+
InterpolatableSet() : x0(), dx(), ready(false) {}
|
138
|
+
|
139
|
+
/**
|
140
|
+
* update interpolation source
|
141
|
+
* @param force_update If true, update is forcibly performed irrespective of current state
|
142
|
+
* @param cnd condition for source data selection
|
143
|
+
*/
|
144
|
+
InterpolatableSet &update(
|
145
|
+
const Tx &x, const std::map<Tx, Ty> &xy_table,
|
146
|
+
const condition_t &cnd,
|
147
|
+
const bool &force_update = false){
|
148
|
+
|
149
|
+
do{
|
150
|
+
if(force_update){break;}
|
151
|
+
if(dx.size() <= 2){break;}
|
152
|
+
if(x < x_lower){break;}
|
153
|
+
if(x > x_upper){break;}
|
154
|
+
return *this;
|
155
|
+
}while(false);
|
156
|
+
|
157
|
+
// If the 1st and 2nd nearest items are changed, then recalculate interpolation targets.
|
158
|
+
struct {
|
159
|
+
const Tx &x_base;
|
160
|
+
bool operator()(
|
161
|
+
const typename std::map<Tx, Ty>::value_type &rhs,
|
162
|
+
const typename std::map<Tx, Ty>::value_type &lhs) const {
|
163
|
+
return std::abs(rhs.first - x_base) < std::abs(lhs.first - x_base);
|
164
|
+
}
|
165
|
+
} cmp = {(x0 = x)};
|
166
|
+
|
167
|
+
buf.resize(cnd.max_x_size);
|
168
|
+
dx.clear();
|
169
|
+
// extract x where x0-dx_range <= x <= x0+dx_range, then sort by ascending order of |x-x0|
|
170
|
+
for(typename buffer_t::const_iterator
|
171
|
+
it(buf.begin()),
|
172
|
+
it_end(std::partial_sort_copy(
|
173
|
+
xy_table.lower_bound(x - cnd.max_dx_range),
|
174
|
+
xy_table.upper_bound(x + cnd.max_dx_range),
|
175
|
+
buf.begin(), buf.end(), cmp));
|
176
|
+
it != it_end; ++it){
|
177
|
+
dx.push_back(it->first - x0);
|
178
|
+
}
|
179
|
+
if(dx.size() >= 2){ // calculate a necessary condition to update samples for new x
|
180
|
+
// According to Nth nearest points, x range in which the order of extracted samples
|
181
|
+
// is retained can be calculated.
|
182
|
+
bool ascending(buf[0].first <= buf[1].first);
|
183
|
+
Tx &xa(ascending ? x_lower : x_upper), &xb(ascending ? x_upper : x_lower);
|
184
|
+
xa = x0;
|
185
|
+
for(std::size_t i(2); i < buf.size(); ++i){
|
186
|
+
if(ascending ? (dx[1] <= dx[i]) : (dx[1] >= dx[i])){continue;}
|
187
|
+
xa = x0 + (dx[1] + dx[i]) / 2;
|
188
|
+
break;
|
189
|
+
}
|
190
|
+
xb = x0 + (dx[0] + dx[1]) / 2;
|
191
|
+
}
|
192
|
+
ready = (dx.size() >= cnd.max_x_size);
|
193
|
+
|
194
|
+
return *this;
|
195
|
+
}
|
196
|
+
|
197
|
+
template <class Ty2, class Ty2_Iterator>
|
198
|
+
Ty2 interpolate2(
|
199
|
+
const Tx &x, const Ty2_Iterator &y,
|
200
|
+
Ty2 *derivative = NULL) const {
|
201
|
+
int order(dx.size() - 1);
|
202
|
+
do{
|
203
|
+
if(order > 0){break;}
|
204
|
+
if((order == 0) && (!derivative)){return y[0];}
|
205
|
+
throw std::range_error("Insufficient records for interpolation");
|
206
|
+
}while(false);
|
207
|
+
std::vector<Ty2> y_buf(order), dy_buf(order);
|
208
|
+
interpolate_Neville(
|
209
|
+
dx, y, x - x0, y_buf, order,
|
210
|
+
&dy_buf, derivative ? 1 : 0);
|
211
|
+
if(derivative){*derivative = dy_buf[0];}
|
212
|
+
return y_buf[0];
|
213
|
+
}
|
214
|
+
|
215
|
+
Ty interpolate(const Tx &x, Ty *derivative = NULL) const {
|
216
|
+
struct second_iterator : public buffer_t::const_iterator {
|
217
|
+
second_iterator(const typename buffer_t::const_iterator &it)
|
218
|
+
: buffer_t::const_iterator(it) {}
|
219
|
+
const Ty &operator[](const int &n) const {
|
220
|
+
return buffer_t::const_iterator::operator[](n).second;
|
221
|
+
}
|
222
|
+
} y_it(buf.begin());
|
223
|
+
return interpolate2(x, y_it, derivative);
|
224
|
+
}
|
225
|
+
};
|
226
|
+
|
121
227
|
#endif /* __INTERPOLATE_H__ */
|
@@ -998,17 +998,18 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
998
998
|
bool is_equivalent(const Ephemeris_with_Time &eph) const {
|
999
999
|
return Ephemeris::is_equivalent(eph) && TimeProperties::is_equivalent(eph);
|
1000
1000
|
}
|
1001
|
-
float_t calculate_clock_error(
|
1002
|
-
delta_t -= pseudo_range / light_speed;
|
1001
|
+
float_t calculate_clock_error(const float_t &delta_t) const {
|
1003
1002
|
return -TimeProperties::tau_c - Ephemeris::tau_n + Ephemeris::gamma_n * delta_t;
|
1004
1003
|
}
|
1005
1004
|
/**
|
1006
|
-
* @param
|
1005
|
+
* @param t_depature_onboard signal depature time in onboard time scale,
|
1007
1006
|
* which is along to glonass time scale (UTC + 3 hr) but is shifted in gradually changing length.
|
1008
1007
|
*/
|
1009
|
-
float_t clock_error(
|
1010
|
-
|
1011
|
-
|
1008
|
+
float_t clock_error(const float_t &t_depature_onboard) const {
|
1009
|
+
return calculate_clock_error(t_depature_onboard - Ephemeris::t_b);
|
1010
|
+
}
|
1011
|
+
const float_t &clock_error_dot() const {
|
1012
|
+
return Ephemeris::gamma_n;
|
1012
1013
|
}
|
1013
1014
|
/**
|
1014
1015
|
* Calculate absolute constellation(t) based on constellation(t_0).
|
@@ -1064,23 +1065,22 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1064
1065
|
* @param pseudo_range measured pusedo_range to correct delta_t, default is 0.
|
1065
1066
|
*/
|
1066
1067
|
constellation_t calculate_constellation(
|
1067
|
-
const float_t &delta_t, const float_t &
|
1068
|
+
const float_t &delta_t, const float_t &dt_transit = 0) const {
|
1068
1069
|
|
1069
|
-
float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed);
|
1070
1070
|
constellation_t res(
|
1071
|
-
constellation_abs(
|
1071
|
+
constellation_abs(delta_t, xa_t_b, float_t(0)));
|
1072
1072
|
|
1073
1073
|
return res.rel_corrdinate(
|
1074
|
-
sidereal_t_b_rad + (constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02
|
1074
|
+
sidereal_t_b_rad + (constellation_t::omega_E * (delta_t + dt_transit))); // transform from abs to PZ-90.02
|
1075
1075
|
}
|
1076
1076
|
/**
|
1077
|
-
* @param
|
1077
|
+
* @param t_depature_glonass signal depature time in glonass time scale,
|
1078
1078
|
* which is the corrected onboard time by removing clock error.
|
1079
1079
|
*/
|
1080
1080
|
constellation_t constellation(
|
1081
|
-
const float_t &
|
1081
|
+
const float_t &t_depature_glonass, const float_t &dt_transit = 0) const {
|
1082
1082
|
return calculate_constellation(
|
1083
|
-
|
1083
|
+
t_depature_glonass - Ephemeris::t_b, dt_transit); // measure in UTC + 3hr scale
|
1084
1084
|
}
|
1085
1085
|
};
|
1086
1086
|
|
@@ -1107,14 +1107,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1107
1107
|
return std::abs(t_b_gps.interval(t)) <= 60 * 60; // 1 hour
|
1108
1108
|
}
|
1109
1109
|
using Ephemeris_with_Time::clock_error;
|
1110
|
-
float_t clock_error(
|
1111
|
-
|
1112
|
-
return Ephemeris_with_Time::calculate_clock_error(t_arrival_onboard - t_b_gps, pseudo_range);
|
1110
|
+
float_t clock_error(const GPS_Time<float_t> &t_depature_onboard) const {
|
1111
|
+
return Ephemeris_with_Time::calculate_clock_error(t_depature_onboard - t_b_gps);
|
1113
1112
|
}
|
1114
1113
|
using Ephemeris_with_Time::constellation;
|
1115
1114
|
typename Ephemeris_with_Time::constellation_t constellation(
|
1116
|
-
const GPS_Time<float_t> &
|
1117
|
-
return this->calculate_constellation(
|
1115
|
+
const GPS_Time<float_t> &t_depature_gps, const float_t &dt_transit = 0) const {
|
1116
|
+
return this->calculate_constellation(t_depature_gps - t_b_gps, dt_transit);
|
1118
1117
|
}
|
1119
1118
|
};
|
1120
1119
|
};
|
@@ -1130,25 +1129,25 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1130
1129
|
eph_cached_t(const eph_t &eph) : eph_t(eph), xa_t_0(eph.xa_t_b), t_0_from_t_b(0) {}
|
1131
1130
|
using eph_t::constellation;
|
1132
1131
|
typename eph_t::constellation_t constellation(
|
1133
|
-
const GPS_Time<float_t> &
|
1134
|
-
float_t delta_t(
|
1135
|
-
float_t
|
1132
|
+
const GPS_Time<float_t> &t_depature_gps, const float_t &dt_transit = 0) const {
|
1133
|
+
float_t delta_t(t_depature_gps - eph_t::t_b_gps);
|
1134
|
+
float_t delta_t_depature_from_t_0(delta_t - t_0_from_t_b);
|
1136
1135
|
|
1137
|
-
float_t t_step_max(
|
1136
|
+
float_t t_step_max(delta_t_depature_from_t_0 >= 0
|
1138
1137
|
? eph_t::time_step_max_per_one_integration
|
1139
1138
|
: -eph_t::time_step_max_per_one_integration);
|
1140
1139
|
|
1141
|
-
int big_steps(std::floor(
|
1140
|
+
int big_steps(std::floor(delta_t_depature_from_t_0 / t_step_max));
|
1142
1141
|
if(big_steps > 0){ // perform integration and update cache
|
1143
1142
|
xa_t_0 = eph_t::constellation_abs(t_step_max, big_steps, xa_t_0, t_0_from_t_b);
|
1144
1143
|
float_t delta_t_updated(t_step_max * big_steps);
|
1145
1144
|
t_0_from_t_b += delta_t_updated;
|
1146
|
-
|
1145
|
+
delta_t_depature_from_t_0 -= delta_t_updated;
|
1147
1146
|
}
|
1148
1147
|
|
1149
|
-
return eph_t::constellation_abs(
|
1150
|
-
.rel_corrdinate(
|
1151
|
-
eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t));
|
1148
|
+
return eph_t::constellation_abs(delta_t_depature_from_t_0, 1, xa_t_0, t_0_from_t_b)
|
1149
|
+
.rel_corrdinate( // transform from abs to PZ-90.02
|
1150
|
+
eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * (delta_t + dt_transit)));
|
1152
1151
|
}
|
1153
1152
|
};
|
1154
1153
|
|
@@ -1190,22 +1189,25 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1190
1189
|
return eph_history.select(target_time, &eph_t::is_valid);
|
1191
1190
|
}
|
1192
1191
|
|
1193
|
-
float_t clock_error(const GPS_Time<float_t> &
|
1194
|
-
return ephemeris().clock_error(
|
1192
|
+
float_t clock_error(const GPS_Time<float_t> &t_tx) const{
|
1193
|
+
return ephemeris().clock_error(t_tx);
|
1194
|
+
}
|
1195
|
+
float_t clock_error_dot(const GPS_Time<float_t> &t_tx) const{
|
1196
|
+
return ephemeris().clock_error_dot();
|
1195
1197
|
}
|
1196
1198
|
|
1197
1199
|
typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t constellation(
|
1198
|
-
const GPS_Time<float_t> &
|
1200
|
+
const GPS_Time<float_t> &t_tx, const float_t &dt_transit = 0) const {
|
1199
1201
|
return (typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t)(
|
1200
|
-
eph_history.current().constellation(
|
1202
|
+
eph_history.current().constellation(t_tx, dt_transit));
|
1201
1203
|
}
|
1202
1204
|
|
1203
|
-
typename GPS_SpaceNode<float_t>::xyz_t position(const GPS_Time<float_t> &
|
1204
|
-
return constellation(
|
1205
|
+
typename GPS_SpaceNode<float_t>::xyz_t position(const GPS_Time<float_t> &t_tx, const float_t &dt_transit= 0) const {
|
1206
|
+
return constellation(t_tx, dt_transit).position;
|
1205
1207
|
}
|
1206
1208
|
|
1207
|
-
typename GPS_SpaceNode<float_t>::xyz_t velocity(const GPS_Time<float_t> &
|
1208
|
-
return constellation(
|
1209
|
+
typename GPS_SpaceNode<float_t>::xyz_t velocity(const GPS_Time<float_t> &t_tx, const float_t &dt_transit = 0) const {
|
1210
|
+
return constellation(t_tx, dt_transit).velocity;
|
1209
1211
|
}
|
1210
1212
|
};
|
1211
1213
|
public:
|
@@ -119,18 +119,17 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
119
119
|
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
120
120
|
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
121
121
|
}
|
122
|
-
static xyz_t position(const void *ptr, const gps_time_t &
|
123
|
-
return sat(ptr).constellation(
|
122
|
+
static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
123
|
+
return sat(ptr).constellation(t_tx, dt_transit).position;
|
124
124
|
}
|
125
|
-
static xyz_t velocity(const void *ptr, const gps_time_t &
|
126
|
-
return sat(ptr).constellation(
|
125
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
126
|
+
return sat(ptr).constellation(t_tx, dt_transit).velocity;
|
127
127
|
}
|
128
|
-
static float_t clock_error(const void *ptr, const gps_time_t &
|
129
|
-
return sat(ptr).clock_error(
|
128
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
|
129
|
+
return sat(ptr).clock_error(t_tx);
|
130
130
|
}
|
131
|
-
static float_t clock_error_dot(const void *ptr, const gps_time_t &
|
132
|
-
|
133
|
-
return 0;
|
131
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
|
132
|
+
return sat(ptr).clock_error_dot(t_tx);
|
134
133
|
}
|
135
134
|
};
|
136
135
|
satellite_t res = {
|
@@ -250,16 +249,20 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
250
249
|
|
251
250
|
range -= receiver_error;
|
252
251
|
|
252
|
+
static const float_t &c(space_node_t::light_speed);
|
253
|
+
|
253
254
|
// Clock correction, which will be considered in the next constellation()
|
254
255
|
// as extra transmission time by using extra psuedo range.
|
255
256
|
if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){
|
256
|
-
range += (sat.clock_error(time_arrival
|
257
|
+
range += (sat.clock_error(time_arrival - range / c) * c);
|
257
258
|
}else{
|
258
259
|
range += range_error.value[range_error_t::SATELLITE_CLOCK];
|
259
260
|
}
|
260
261
|
|
261
262
|
// Calculate satellite position
|
262
|
-
|
263
|
+
float_t dt_transit(range / c);
|
264
|
+
gps_time_t t_tx(time_arrival - dt_transit);
|
265
|
+
xyz_t sat_pos(sat.position(t_tx, dt_transit));
|
263
266
|
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
264
267
|
|
265
268
|
// Calculate residual
|
@@ -307,12 +310,12 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
307
310
|
|
308
311
|
res.range_corrected = range;
|
309
312
|
|
310
|
-
xyz_t rel_vel(sat.velocity(
|
313
|
+
xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
|
311
314
|
|
312
315
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
313
316
|
+ res.los_neg[1] * rel_vel.y()
|
314
317
|
+ res.los_neg[2] * rel_vel.z()
|
315
|
-
+ sat.clock_error_dot(
|
318
|
+
+ sat.clock_error_dot(t_tx) * c;
|
316
319
|
|
317
320
|
return res;
|
318
321
|
}
|
@@ -1113,16 +1113,14 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
1113
1113
|
* Calculate correction value in accordance with clock error model
|
1114
1114
|
*
|
1115
1115
|
* @param t current time
|
1116
|
-
* @param pseudo_range pseudo range in meters
|
1117
1116
|
* @param gamma factor for compensation of group delay
|
1118
1117
|
* L1 = 1, L2 = (77/60)^2, see ICD 20.3.3.3.3.2 L1 - L2 Correction
|
1119
1118
|
* @return in seconds
|
1120
1119
|
*/
|
1121
|
-
float_t clock_error(const gps_time_t &t,
|
1120
|
+
float_t clock_error(const gps_time_t &t,
|
1122
1121
|
const float_t &gamma = 1) const{
|
1123
1122
|
|
1124
|
-
float_t
|
1125
|
-
float_t tk(period_from_time_of_clock(t) - transit_time);
|
1123
|
+
float_t tk(period_from_time_of_clock(t));
|
1126
1124
|
float_t Ek(eccentric_anomaly(tk));
|
1127
1125
|
|
1128
1126
|
// Relativistic correction term
|
@@ -1134,10 +1132,9 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
1134
1132
|
return dt_sv - (gamma * t_GD);
|
1135
1133
|
}
|
1136
1134
|
|
1137
|
-
float_t clock_error_dot(const gps_time_t &t
|
1135
|
+
float_t clock_error_dot(const gps_time_t &t) const {
|
1138
1136
|
|
1139
|
-
float_t
|
1140
|
-
float_t tk(period_from_time_of_clock(t) - transit_time);
|
1137
|
+
float_t tk(period_from_time_of_clock(t));
|
1141
1138
|
float_t Ek(eccentric_anomaly(tk));
|
1142
1139
|
float_t Ek_dot(eccentric_anomaly_dot(Ek));
|
1143
1140
|
|
@@ -1150,17 +1147,24 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
1150
1147
|
return dt_sv_dot;
|
1151
1148
|
}
|
1152
1149
|
|
1150
|
+
/**
|
1151
|
+
* Return satellite position and velocity at the transmission time in EFEC.
|
1152
|
+
* @param t_tx transmission time
|
1153
|
+
* @param dt_transit Transit time. default is zero.
|
1154
|
+
* If zero, the returned value is along with the ECEF at the transmission time.
|
1155
|
+
* Otherwise (non-zero), they are along with the ECEF at the reception time,
|
1156
|
+
* that is, the transmission time added by the transit time.
|
1157
|
+
* @param with_velocity If true, velocity calculation is performed,
|
1158
|
+
* otherwise invalid velocity is returned.
|
1159
|
+
*/
|
1153
1160
|
constellation_t constellation(
|
1154
|
-
const gps_time_t &
|
1161
|
+
const gps_time_t &t_tx, const float_t &dt_transit = 0,
|
1155
1162
|
const bool &with_velocity = true) const {
|
1156
1163
|
|
1157
1164
|
constellation_t res;
|
1158
1165
|
|
1159
1166
|
// Time from ephemeris reference epoch (tk)
|
1160
|
-
float_t
|
1161
|
-
|
1162
|
-
// Remove transit time
|
1163
|
-
float_t tk(tk0 - pseudo_range / light_speed);
|
1167
|
+
float_t tk(period_from_time_of_ephemeris(t_tx));
|
1164
1168
|
|
1165
1169
|
// Eccentric Anomaly (Ek)
|
1166
1170
|
float_t Ek(eccentric_anomaly(tk));
|
@@ -1207,12 +1211,12 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
1207
1211
|
float_t Omegak(Omega0);
|
1208
1212
|
if(false){ // __MISUNDERSTANDING_ABOUT_OMEGA0_CORRECTION__
|
1209
1213
|
Omegak += (
|
1210
|
-
(dot_Omega0 - WGS84::Omega_Earth_IAU) *
|
1214
|
+
(dot_Omega0 - WGS84::Omega_Earth_IAU) * tk
|
1211
1215
|
- WGS84::Omega_Earth_IAU * t_oe);
|
1212
1216
|
}else{
|
1213
1217
|
Omegak += (
|
1214
|
-
dot_Omega0 * tk
|
1215
|
-
- WGS84::Omega_Earth_IAU * (t_oe +
|
1218
|
+
dot_Omega0 * tk // correction (at the transmission)
|
1219
|
+
- WGS84::Omega_Earth_IAU * (t_oe + tk + dt_transit)); // correction and coordinate transformation
|
1216
1220
|
}
|
1217
1221
|
|
1218
1222
|
float_t Omegak_sin(sin(Omegak)),
|
@@ -2013,26 +2017,26 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
2013
2017
|
&eph_t::period_from_time_of_clock) || is_valid;
|
2014
2018
|
}
|
2015
2019
|
|
2016
|
-
float_t clock_error(const gps_time_t &
|
2017
|
-
return ephemeris().clock_error(
|
2020
|
+
float_t clock_error(const gps_time_t &t_tx) const{
|
2021
|
+
return ephemeris().clock_error(t_tx);
|
2018
2022
|
}
|
2019
2023
|
|
2020
|
-
float_t clock_error_dot(const gps_time_t &
|
2021
|
-
return ephemeris().clock_error_dot(
|
2024
|
+
float_t clock_error_dot(const gps_time_t &t_tx) const {
|
2025
|
+
return ephemeris().clock_error_dot(t_tx);
|
2022
2026
|
}
|
2023
2027
|
|
2024
2028
|
typename SatelliteProperties::constellation_t constellation(
|
2025
|
-
const gps_time_t &
|
2029
|
+
const gps_time_t &t_tx, const float_t &dt_transit = 0,
|
2026
2030
|
const bool &with_velocity = true) const {
|
2027
|
-
return ephemeris().constellation(
|
2031
|
+
return ephemeris().constellation(t_tx, dt_transit, with_velocity);
|
2028
2032
|
}
|
2029
2033
|
|
2030
|
-
xyz_t position(const gps_time_t &
|
2031
|
-
return constellation(
|
2034
|
+
xyz_t position(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
|
2035
|
+
return constellation(t_tx, dt_transit, false).position;
|
2032
2036
|
}
|
2033
2037
|
|
2034
|
-
xyz_t velocity(const gps_time_t &
|
2035
|
-
return constellation(
|
2038
|
+
xyz_t velocity(const gps_time_t &t_tx, const float_t &dt_transit = 0) const {
|
2039
|
+
return constellation(t_tx, dt_transit, true).velocity;
|
2036
2040
|
}
|
2037
2041
|
};
|
2038
2042
|
public:
|
@@ -143,17 +143,17 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
143
143
|
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
144
144
|
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
145
145
|
}
|
146
|
-
static xyz_t position(const void *ptr, const gps_time_t &
|
147
|
-
return sat(ptr).position(
|
146
|
+
static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
147
|
+
return sat(ptr).position(t_tx, dt_transit);
|
148
148
|
}
|
149
|
-
static xyz_t velocity(const void *ptr, const gps_time_t &
|
150
|
-
return sat(ptr).velocity(
|
149
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
150
|
+
return sat(ptr).velocity(t_tx, dt_transit);
|
151
151
|
}
|
152
|
-
static float_t clock_error(const void *ptr, const gps_time_t &
|
153
|
-
return sat(ptr).clock_error(
|
152
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
|
153
|
+
return sat(ptr).clock_error(t_tx);
|
154
154
|
}
|
155
|
-
static float_t clock_error_dot(const void *ptr, const gps_time_t &
|
156
|
-
return sat(ptr).clock_error_dot(
|
155
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
|
156
|
+
return sat(ptr).clock_error_dot(t_tx);
|
157
157
|
}
|
158
158
|
};
|
159
159
|
satellite_t res = {
|
@@ -272,13 +272,16 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
272
272
|
residual_t &residual,
|
273
273
|
const range_error_t &error = range_error_t::not_corrected) const {
|
274
274
|
|
275
|
+
static const float_t &c(space_node_t::light_speed);
|
276
|
+
|
275
277
|
// Clock error correction
|
276
278
|
range += ((error.unknown_flag & range_error_t::SATELLITE_CLOCK)
|
277
|
-
? (sat.clock_error(time_arrival
|
279
|
+
? (sat.clock_error(time_arrival - range / c) * c)
|
278
280
|
: error.value[range_error_t::SATELLITE_CLOCK]);
|
279
281
|
|
280
282
|
// Calculate satellite position
|
281
|
-
|
283
|
+
float_t dt_transit(range / c);
|
284
|
+
xyz_t sat_pos(sat.position(time_arrival - dt_transit, dt_transit));
|
282
285
|
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
283
286
|
|
284
287
|
// Calculate residual
|
@@ -344,11 +347,15 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
344
347
|
const xyz_t &usr_vel,
|
345
348
|
const float_t &los_neg_x, const float_t &los_neg_y, const float_t &los_neg_z) const {
|
346
349
|
|
347
|
-
|
350
|
+
static const float_t &c(space_node_t::light_speed);
|
351
|
+
|
352
|
+
float_t dt_transit(range / c);
|
353
|
+
gps_time_t t_tx(time_arrival - dt_transit);
|
354
|
+
xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
|
348
355
|
return los_neg_x * rel_vel.x()
|
349
356
|
+ los_neg_y * rel_vel.y()
|
350
357
|
+ los_neg_z * rel_vel.z()
|
351
|
-
+ sat.clock_error_dot(
|
358
|
+
+ sat.clock_error_dot(t_tx) * c; // considering clock error rate
|
352
359
|
}
|
353
360
|
|
354
361
|
/**
|
@@ -230,29 +230,49 @@ struct GPS_Solver_Base {
|
|
230
230
|
const void *impl;
|
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
|
-
float_t (*impl_clock_error)(const void *, const gps_time_t
|
234
|
-
float_t (*impl_clock_error_dot)(const void *, const gps_time_t
|
233
|
+
float_t (*impl_clock_error)(const void *, const gps_time_t &);
|
234
|
+
float_t (*impl_clock_error_dot)(const void *, const gps_time_t &);
|
235
235
|
inline bool is_available() const {
|
236
236
|
return impl != NULL;
|
237
237
|
}
|
238
|
-
|
239
|
-
|
238
|
+
/**
|
239
|
+
* Return satellite position at the transmission time in EFEC.
|
240
|
+
* @param t_tx transmission time
|
241
|
+
* @param dt_transit Transit time. default is zero.
|
242
|
+
* If zero, the returned value is along with the ECEF at the transmission time.
|
243
|
+
* Otherwise (non-zero), they are along with the ECEF at the reception time,
|
244
|
+
* that is, the transmission time added by the transit time.
|
245
|
+
*/
|
246
|
+
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);
|
240
248
|
}
|
241
|
-
|
242
|
-
|
249
|
+
/**
|
250
|
+
* Return satellite velocity at the transmission time in EFEC.
|
251
|
+
* @see position
|
252
|
+
*/
|
253
|
+
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);
|
243
255
|
}
|
244
|
-
|
245
|
-
|
256
|
+
/**
|
257
|
+
* Return satellite clock error [s] at the transmission time.
|
258
|
+
* @param t_tx transmission time
|
259
|
+
*/
|
260
|
+
inline float_t clock_error(const gps_time_t &t_tx) const {
|
261
|
+
return impl_clock_error(impl, t_tx);
|
246
262
|
}
|
247
|
-
|
248
|
-
|
263
|
+
/**
|
264
|
+
* Return satellite clock error derivative [s/s] at the transmission time.
|
265
|
+
* @param t_tx transmission time
|
266
|
+
*/
|
267
|
+
inline float_t clock_error_dot(const gps_time_t &t_tx) const {
|
268
|
+
return impl_clock_error_dot(impl, t_tx);
|
249
269
|
}
|
250
270
|
static const satellite_t &unavailable() {
|
251
271
|
struct impl_t {
|
252
272
|
static xyz_t v3(const void *, const gps_time_t &, const float_t &){
|
253
273
|
return xyz_t(0, 0, 0);
|
254
274
|
}
|
255
|
-
static float_t v(const void *, const gps_time_t
|
275
|
+
static float_t v(const void *, const gps_time_t &){
|
256
276
|
return float_t(0);
|
257
277
|
}
|
258
278
|
};
|
@@ -846,29 +846,37 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
846
846
|
int year, month, day;
|
847
847
|
FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity
|
848
848
|
int leap_sec;
|
849
|
+
int flags;
|
850
|
+
enum {
|
851
|
+
TAU_C_NEG = 0x01,
|
852
|
+
TAU_GPS = 0x02,
|
853
|
+
LEAP_SEC = 0x04,
|
854
|
+
};
|
849
855
|
};
|
850
856
|
static const typename super_t::convert_item_t t_corr_glonass_v2[4];
|
851
857
|
|
852
858
|
bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const {
|
853
|
-
|
859
|
+
t_corr_glonass.flags = 0;
|
854
860
|
super_t::header_t::const_iterator it;
|
855
861
|
|
856
|
-
if(
|
862
|
+
if((it = _header.find("CORR TO SYSTEM TIME")) != _header.end()){
|
857
863
|
super_t::convert(t_corr_glonass_v2, it->second.front(), &t_corr_glonass);
|
864
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
|
858
865
|
}
|
859
866
|
|
860
|
-
if(
|
867
|
+
if((it = _header.find("LEAP SECONDS")) != _header.end()){
|
861
868
|
iono_utc_t iono_utc;
|
862
869
|
super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
|
863
870
|
t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
|
871
|
+
t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
|
864
872
|
}
|
865
873
|
|
866
|
-
return
|
874
|
+
return t_corr_glonass.flags > 0;
|
867
875
|
}
|
868
876
|
|
869
877
|
bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const {
|
870
878
|
iono_utc_t iono_utc;
|
871
|
-
|
879
|
+
t_corr_glonass.flags = 0;
|
872
880
|
typedef super_t::header_t::const_iterator it_t;
|
873
881
|
typedef super_t::header_t::mapped_type::const_iterator it2_t;
|
874
882
|
|
@@ -880,11 +888,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
880
888
|
super_t::convert(utc_v3, *it2, &iono_utc);
|
881
889
|
t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0;
|
882
890
|
t_corr_glonass.tau_c_neg = iono_utc.A0;
|
891
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
|
883
892
|
}else if(it2->find("GLGP") != it2->npos){
|
884
893
|
super_t::convert(utc_v3, *it2, &iono_utc);
|
885
894
|
t_corr_glonass.tau_GPS = iono_utc.A0;
|
895
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_GPS;
|
886
896
|
}
|
887
|
-
utc = true;
|
888
897
|
}
|
889
898
|
}
|
890
899
|
|
@@ -895,10 +904,10 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
895
904
|
super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
|
896
905
|
}
|
897
906
|
t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
|
898
|
-
|
907
|
+
t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
|
899
908
|
}
|
900
909
|
|
901
|
-
return
|
910
|
+
return t_corr_glonass.flags > 0;
|
902
911
|
}
|
903
912
|
|
904
913
|
struct space_node_list_t {
|
@@ -955,8 +964,11 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
955
964
|
typename message_glonass_t::eph_t eph0(reader.msg_glonass);
|
956
965
|
eph0.tau_c = -t_corr_glonass.tau_c_neg;
|
957
966
|
eph0.tau_GPS = t_corr_glonass.tau_GPS;
|
958
|
-
typename GLONASS_SpaceNode<FloatT
|
959
|
-
|
967
|
+
typename GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time eph(
|
968
|
+
eph0,
|
969
|
+
(t_corr_glonass.flags & t_corr_glonass_t::LEAP_SEC)
|
970
|
+
? t_corr_glonass.leap_sec
|
971
|
+
: GPS_Time<FloatT>::guess_leap_seconds(reader.msg_glonass.date_tm));
|
960
972
|
space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph);
|
961
973
|
res++;
|
962
974
|
break;
|