gps_pvt 0.6.1 → 0.6.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/ext/gps_pvt/GPS/GPS_wrap.cxx +219 -514
- 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/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 +8 -13
- data/ext/ninja-scan-light/tool/swig/GPS.i +33 -39
- data/lib/gps_pvt/version.rb +1 -1
- metadata +2 -2
@@ -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
|
};
|
@@ -1768,30 +1768,38 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1768
1768
|
return (delta_t < 0) || (delta_t > Timing::values[Timing::GEO_NAVIGATION_DATA].interval);
|
1769
1769
|
}
|
1770
1770
|
|
1771
|
-
|
1772
|
-
|
1773
|
-
|
1771
|
+
float_t clock_error(const gps_time_t &t) const {
|
1772
|
+
float_t t_G(-t.interval(WN, 0)); // t_0 is expected to be modified as time of week
|
1773
|
+
return a_Gf0 + a_Gf1 * (t_G - t_0); // Eq.(A-45), delay is positive
|
1774
|
+
}
|
1774
1775
|
|
1775
|
-
|
1776
|
+
float_t clock_error_dot(const gps_time_t &t) const {
|
1777
|
+
return a_Gf1;
|
1778
|
+
}
|
1776
1779
|
|
1777
|
-
|
1778
|
-
|
1780
|
+
constellation_t constellation(
|
1781
|
+
const gps_time_t &t_tx, const float_t &dt_transit = 0,
|
1782
|
+
const bool &with_velocity = true) const {
|
1779
1783
|
|
1780
|
-
float_t delta_t(
|
1784
|
+
float_t delta_t(-t_tx.interval(WN, t_0)), delta_t2(delta_t * delta_t / 2);
|
1781
1785
|
|
1782
1786
|
constellation_t res = {
|
1783
1787
|
xyz_t(
|
1784
1788
|
x + dx * delta_t + ddx * delta_t2,
|
1785
1789
|
y + dy * delta_t + ddy * delta_t2,
|
1786
|
-
z + dz * delta_t + ddz * delta_t2), // Eq. (A-44)
|
1790
|
+
z + dz * delta_t + ddz * delta_t2).after(dt_transit), // Eq. (A-44)
|
1787
1791
|
xyz_t(
|
1788
|
-
|
1789
|
-
|
1790
|
-
|
1792
|
+
dx + ddx * delta_t,
|
1793
|
+
dy + ddy * delta_t,
|
1794
|
+
dz + ddz * delta_t).after(dt_transit),
|
1791
1795
|
};
|
1792
1796
|
|
1793
|
-
|
1794
|
-
|
1797
|
+
/* Sagnac correction, which is expected to perform before geometric distance calculation,
|
1798
|
+
* can be skipped by using non-zero dt_transit.
|
1799
|
+
* This is because the return values is corrected to be along with the coordinate
|
1800
|
+
* at the reception time.
|
1801
|
+
* @see SBAS_SpaceNode::sagnac_correction
|
1802
|
+
*/
|
1795
1803
|
|
1796
1804
|
return res;
|
1797
1805
|
}
|
@@ -114,19 +114,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
114
114
|
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
115
115
|
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
116
116
|
}
|
117
|
-
static xyz_t position(const void *ptr, const gps_time_t &
|
118
|
-
return sat(ptr).ephemeris().constellation(
|
117
|
+
static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
118
|
+
return sat(ptr).ephemeris().constellation(t_tx, dt_transit, false).position;
|
119
119
|
}
|
120
|
-
static xyz_t velocity(const void *ptr, const gps_time_t &
|
121
|
-
return sat(ptr).ephemeris().constellation(
|
120
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
121
|
+
return sat(ptr).ephemeris().constellation(t_tx, dt_transit, true).velocity;
|
122
122
|
}
|
123
|
-
static float_t clock_error(const void *ptr, const gps_time_t &
|
124
|
-
|
125
|
-
return 0;
|
123
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
|
124
|
+
return sat(ptr).ephemeris().clock_error(t_tx);
|
126
125
|
}
|
127
|
-
static float_t clock_error_dot(const void *ptr, const gps_time_t &
|
128
|
-
|
129
|
-
return 0;
|
126
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
|
127
|
+
return sat(ptr).ephemeris().clock_error_dot(t_tx);
|
130
128
|
}
|
131
129
|
};
|
132
130
|
satellite_t res = {
|
@@ -257,20 +255,25 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
257
255
|
|
258
256
|
range -= receiver_error;
|
259
257
|
|
258
|
+
static const float_t &c(GPS_SpaceNode<float_t>::light_speed);
|
259
|
+
|
260
260
|
// Clock error correction
|
261
261
|
range += ((range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)
|
262
|
-
? (sat.clock_error(time_arrival
|
262
|
+
? (sat.clock_error(time_arrival - range / c) * c)
|
263
263
|
: range_error.value[range_error_t::SATELLITE_CLOCK]);
|
264
264
|
|
265
265
|
// TODO WAAS long term clock correction (2.1.1.4.11)
|
266
266
|
|
267
267
|
// Calculate satellite position
|
268
|
-
|
268
|
+
float_t dt_transit(range / c);
|
269
|
+
gps_time_t t_tx(time_arrival - dt_transit);
|
270
|
+
xyz_t sat_pos(sat.position(t_tx, dt_transit));
|
269
271
|
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
270
272
|
|
271
|
-
// Calculate residual
|
273
|
+
// Calculate residual without Sagnac correction (A.4.4.11),
|
274
|
+
// because of the satellite position is calculated in the reception time ECEF.
|
272
275
|
res.range_residual = range
|
273
|
-
+ space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
|
276
|
+
// + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
|
274
277
|
- geometric_range;
|
275
278
|
|
276
279
|
// Setup design matrix
|
@@ -312,12 +315,12 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
312
315
|
|
313
316
|
res.range_corrected = range;
|
314
317
|
|
315
|
-
xyz_t rel_vel(sat.velocity(
|
318
|
+
xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
|
316
319
|
|
317
320
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
318
321
|
+ res.los_neg[1] * rel_vel.y()
|
319
322
|
+ res.los_neg[2] * rel_vel.z()
|
320
|
-
+ sat.clock_error_dot(
|
323
|
+
+ sat.clock_error_dot(t_tx) * c;
|
321
324
|
|
322
325
|
return res;
|
323
326
|
}
|
@@ -224,22 +224,17 @@ struct SP3_Product {
|
|
224
224
|
static inline const per_satellite_t &sat(const void *ptr) {
|
225
225
|
return *reinterpret_cast<const per_satellite_t *>(ptr);
|
226
226
|
}
|
227
|
-
static
|
228
|
-
return
|
227
|
+
static xyz_t position(const void *ptr, const gt_t &t_tx, const float_t &dt_transit) {
|
228
|
+
return sat(ptr).position(t_tx).after(dt_transit);
|
229
229
|
}
|
230
|
-
static xyz_t
|
231
|
-
|
232
|
-
return sat(ptr).position(t - delta_t).after(delta_t);
|
230
|
+
static xyz_t velocity(const void *ptr, const gt_t &t_tx, const float_t &dt_transit) {
|
231
|
+
return sat(ptr).velocity(t_tx).after(dt_transit);
|
233
232
|
}
|
234
|
-
static
|
235
|
-
|
236
|
-
return sat(ptr).velocity(t - delta_t).after(delta_t);
|
233
|
+
static float_t clock_error(const void *ptr, const gt_t &t_tx) {
|
234
|
+
return sat(ptr).clock_error(t_tx);
|
237
235
|
}
|
238
|
-
static float_t
|
239
|
-
return sat(ptr).
|
240
|
-
}
|
241
|
-
static float_t clock_error_dot(const void *ptr, const gt_t &t, const float_t &pr) {
|
242
|
-
return sat(ptr).clock_error_dot(t - pr2sec(pr));
|
236
|
+
static float_t clock_error_dot(const void *ptr, const gt_t &t_tx) {
|
237
|
+
return sat(ptr).clock_error_dot(t_tx);
|
243
238
|
}
|
244
239
|
};
|
245
240
|
typename GPS_Solver_Base<FloatT>::satellite_t res = {
|