gps_pvt 0.1.4 → 0.2.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +3 -2
- data/Rakefile +9 -4
- data/exe/gps_pvt +4 -4
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +5 -3
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +4229 -508
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +7 -5
- data/ext/ninja-scan-light/tool/navigation/GPS.h +5 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +62 -15
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +6 -5
- data/ext/ninja-scan-light/tool/navigation/QZSS.h +62 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +571 -113
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +2330 -0
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +329 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +197 -65
- data/lib/gps_pvt/receiver.rb +311 -132
- data/lib/gps_pvt/version.rb +1 -1
- metadata +9 -7
- data/gps_pvt.gemspec +0 -57
@@ -0,0 +1,329 @@
|
|
1
|
+
/**
|
2
|
+
* @file SBAS solver
|
3
|
+
*
|
4
|
+
*/
|
5
|
+
|
6
|
+
/*
|
7
|
+
* Copyright (c) 2020, M.Naruoka (fenrir)
|
8
|
+
* All rights reserved.
|
9
|
+
*
|
10
|
+
* Redistribution and use in source and binary forms, with or without modification,
|
11
|
+
* are permitted provided that the following conditions are met:
|
12
|
+
*
|
13
|
+
* - Redistributions of source code must retain the above copyright notice,
|
14
|
+
* this list of conditions and the following disclaimer.
|
15
|
+
* - Redistributions in binary form must reproduce the above copyright notice,
|
16
|
+
* this list of conditions and the following disclaimer in the documentation
|
17
|
+
* and/or other materials provided with the distribution.
|
18
|
+
* - Neither the name of the naruoka.org nor the names of its contributors
|
19
|
+
* may be used to endorse or promote products derived from this software
|
20
|
+
* without specific prior written permission.
|
21
|
+
*
|
22
|
+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
23
|
+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
24
|
+
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
25
|
+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
|
26
|
+
* BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
|
27
|
+
* OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
28
|
+
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
29
|
+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
30
|
+
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
31
|
+
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
32
|
+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
33
|
+
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
34
|
+
*
|
35
|
+
*/
|
36
|
+
|
37
|
+
#ifndef __SBAS_SOLVER_H__
|
38
|
+
#define __SBAS_SOLVER_H__
|
39
|
+
|
40
|
+
#include "SBAS.h"
|
41
|
+
#include "GPS_Solver_Base.h"
|
42
|
+
#include "GPS_Solver.h"
|
43
|
+
|
44
|
+
#include <cstddef>
|
45
|
+
|
46
|
+
template <class FloatT>
|
47
|
+
struct SBAS_SinglePositioning_Options : public GPS_Solver_GeneralOptions<FloatT> {
|
48
|
+
typedef GPS_Solver_GeneralOptions<FloatT> super_t;
|
49
|
+
|
50
|
+
typename GPS_Solver_Base<FloatT>::options_t::template exclude_prn_t<120, 158> exclude_prn; // SBAS PRN ranges from 120 to 158
|
51
|
+
SBAS_SinglePositioning_Options()
|
52
|
+
: super_t() {
|
53
|
+
// default: SBAS IGP, then broadcasted Klobuchar.
|
54
|
+
super_t::ionospheric_models[0] = super_t::IONOSPHERIC_SBAS;
|
55
|
+
super_t::ionospheric_models[1] = super_t::IONOSPHERIC_KLOBUCHAR;
|
56
|
+
|
57
|
+
exclude_prn.set(true); // SBAS ranging is default off.
|
58
|
+
}
|
59
|
+
};
|
60
|
+
|
61
|
+
template <class FloatT, class SolverBaseT = GPS_Solver_Base<FloatT> >
|
62
|
+
class SBAS_SinglePositioning : public SolverBaseT {
|
63
|
+
private:
|
64
|
+
SBAS_SinglePositioning<FloatT> &operator=(const SBAS_SinglePositioning<FloatT> &);
|
65
|
+
public:
|
66
|
+
typedef SBAS_SinglePositioning<FloatT> self_t;
|
67
|
+
typedef SolverBaseT base_t;
|
68
|
+
|
69
|
+
#if defined(__GNUC__) && (__GNUC__ < 5)
|
70
|
+
#define inheritate_type(x) typedef typename base_t::x x;
|
71
|
+
#else
|
72
|
+
#define inheritate_type(x) using typename base_t::x;
|
73
|
+
#endif
|
74
|
+
|
75
|
+
inheritate_type(float_t);
|
76
|
+
inheritate_type(prn_t);
|
77
|
+
|
78
|
+
typedef SBAS_SpaceNode<float_t> space_node_t;
|
79
|
+
inheritate_type(gps_time_t);
|
80
|
+
typedef typename space_node_t::Satellite satellite_t;
|
81
|
+
|
82
|
+
inheritate_type(xyz_t);
|
83
|
+
inheritate_type(enu_t);
|
84
|
+
|
85
|
+
inheritate_type(pos_t);
|
86
|
+
|
87
|
+
typedef typename base_t::measurement_t measurement_t;
|
88
|
+
typedef typename base_t::range_error_t range_error_t;
|
89
|
+
|
90
|
+
inheritate_type(relative_property_t);
|
91
|
+
#undef inheritate_type
|
92
|
+
|
93
|
+
typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
|
94
|
+
SBAS_SinglePositioning_Options<float_t>, base_t> options_t;
|
95
|
+
|
96
|
+
typedef GPS_SpaceNode<float_t> gps_space_node_t;
|
97
|
+
const gps_space_node_t *space_node_gps; ///< optional
|
98
|
+
protected:
|
99
|
+
const space_node_t &_space_node;
|
100
|
+
SBAS_SinglePositioning_Options<float_t> _options;
|
101
|
+
|
102
|
+
public:
|
103
|
+
const space_node_t &space_node() const {return _space_node;}
|
104
|
+
|
105
|
+
/**
|
106
|
+
* Check availability of ionospheric models
|
107
|
+
* If a model is completely unavailable, it will be replaced to IONOSPHERIC_SKIP.
|
108
|
+
* It implies that when a model has conditional applicability (such as SBAS), it will be retained.
|
109
|
+
*
|
110
|
+
* @return (int) number of (possibly) available models
|
111
|
+
*/
|
112
|
+
int filter_ionospheric_models(SBAS_SinglePositioning_Options<float_t> &opt) const {
|
113
|
+
int available_models(0);
|
114
|
+
for(std::size_t i(0); i < sizeof(opt.ionospheric_models) / sizeof(opt.ionospheric_models[0]); ++i){
|
115
|
+
bool usable(false);
|
116
|
+
switch(opt.ionospheric_models[i]){
|
117
|
+
case options_t::IONOSPHERIC_KLOBUCHAR:
|
118
|
+
// check whether Klobuchar parameters alpha and beta have been already received
|
119
|
+
if(space_node_gps && space_node_gps->is_valid_iono()){usable = true;}
|
120
|
+
break;
|
121
|
+
case options_t::IONOSPHERIC_SBAS:
|
122
|
+
usable = true;
|
123
|
+
break;
|
124
|
+
case options_t::IONOSPHERIC_NTCM_GL:
|
125
|
+
if(opt.f_10_7 >= 0){usable = true;}
|
126
|
+
// check F10.7 has been already configured
|
127
|
+
break;
|
128
|
+
default:
|
129
|
+
break;
|
130
|
+
}
|
131
|
+
if(usable){
|
132
|
+
available_models++;
|
133
|
+
}else{
|
134
|
+
opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
|
135
|
+
}
|
136
|
+
}
|
137
|
+
return available_models;
|
138
|
+
}
|
139
|
+
|
140
|
+
options_t available_options() const {
|
141
|
+
return options_t(base_t::available_options(), _options);
|
142
|
+
}
|
143
|
+
|
144
|
+
options_t available_options(const options_t &opt_wish) const {
|
145
|
+
SBAS_SinglePositioning_Options<float_t> opt(opt_wish);
|
146
|
+
filter_ionospheric_models(opt);
|
147
|
+
return options_t(base_t::available_options(opt_wish), opt);
|
148
|
+
}
|
149
|
+
|
150
|
+
options_t update_options(const options_t &opt_wish){
|
151
|
+
filter_ionospheric_models(_options = opt_wish);
|
152
|
+
return options_t(base_t::update_options(opt_wish), _options);
|
153
|
+
}
|
154
|
+
|
155
|
+
SBAS_SinglePositioning(const space_node_t &sn, const options_t &opt_wish = options_t())
|
156
|
+
: base_t(),
|
157
|
+
_space_node(sn), space_node_gps(NULL),
|
158
|
+
_options(available_options(opt_wish)) {}
|
159
|
+
|
160
|
+
~SBAS_SinglePositioning(){}
|
161
|
+
|
162
|
+
/**
|
163
|
+
* Check availability of a satellite with which observation is associated
|
164
|
+
*
|
165
|
+
* @param prn satellite number
|
166
|
+
* @param receiver_time receiver time
|
167
|
+
* @return (const satellite_t *) If available, const pointer to satellite is returned,
|
168
|
+
* otherwise NULL.
|
169
|
+
*/
|
170
|
+
const satellite_t *is_available(
|
171
|
+
const typename space_node_t::satellites_t::key_type &prn,
|
172
|
+
const gps_time_t &receiver_time) const {
|
173
|
+
|
174
|
+
const typename space_node_t::satellites_t &sats(_space_node.satellites());
|
175
|
+
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
|
176
|
+
if((it_sat == sats.end()) // has ephemeris?
|
177
|
+
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
178
|
+
return NULL;
|
179
|
+
}
|
180
|
+
|
181
|
+
return &(it_sat->second);
|
182
|
+
}
|
183
|
+
|
184
|
+
/**
|
185
|
+
* Calculate relative range and rate information to a satellite
|
186
|
+
*
|
187
|
+
* @param prn satellite number
|
188
|
+
* @param measurement measurement (per satellite) containing pseudo range
|
189
|
+
* @param receiver_error (temporal solution of) receiver clock error in meter
|
190
|
+
* @param time_arrival time when signal arrive at receiver
|
191
|
+
* @param usr_pos (temporal solution of) user position
|
192
|
+
* @param usr_vel (temporal solution of) user velocity
|
193
|
+
* @return (relative_property_t) relative information
|
194
|
+
*/
|
195
|
+
relative_property_t relative_property(
|
196
|
+
const prn_t &prn,
|
197
|
+
const typename measurement_t::mapped_type &measurement,
|
198
|
+
const float_t &receiver_error,
|
199
|
+
const gps_time_t &time_arrival,
|
200
|
+
const pos_t &usr_pos,
|
201
|
+
const xyz_t &usr_vel) const {
|
202
|
+
|
203
|
+
relative_property_t res = {0};
|
204
|
+
|
205
|
+
if(_options.exclude_prn[prn]){return res;}
|
206
|
+
|
207
|
+
float_t range;
|
208
|
+
range_error_t range_error;
|
209
|
+
if(!this->range(measurement, range, &range_error)){
|
210
|
+
return res; // If no range entry, return with weight = 0
|
211
|
+
}
|
212
|
+
|
213
|
+
const satellite_t *sat(is_available(prn, time_arrival));
|
214
|
+
if(!sat){return res;} // If satellite is unavailable, return with weight = 0
|
215
|
+
|
216
|
+
///< The following procedure is based on Appendix.S with modification
|
217
|
+
|
218
|
+
range -= receiver_error;
|
219
|
+
|
220
|
+
// Clock correction will be performed in the following constellation()
|
221
|
+
if(!(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)){
|
222
|
+
range += range_error.value[range_error_t::SATELLITE_CLOCK];
|
223
|
+
}
|
224
|
+
|
225
|
+
// TODO WAAS long term clock correction (2.1.1.4.11)
|
226
|
+
|
227
|
+
// Calculate satellite position and velocity
|
228
|
+
typename space_node_t::SatelliteProperties::constellation_t sat_pos_vel(
|
229
|
+
sat->ephemeris().constellation(time_arrival, range));
|
230
|
+
|
231
|
+
const xyz_t &sat_pos(sat_pos_vel.position);
|
232
|
+
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
233
|
+
|
234
|
+
// Calculate residual with Sagnac correction (A.4.4.11)
|
235
|
+
res.range_residual = range
|
236
|
+
+ space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
|
237
|
+
- geometric_range;
|
238
|
+
|
239
|
+
// Setup design matrix
|
240
|
+
res.los_neg[0] = -(sat_pos.x() - usr_pos.xyz.x()) / geometric_range;
|
241
|
+
res.los_neg[1] = -(sat_pos.y() - usr_pos.xyz.y()) / geometric_range;
|
242
|
+
res.los_neg[2] = -(sat_pos.z() - usr_pos.xyz.z()) / geometric_range;
|
243
|
+
|
244
|
+
enu_t relative_pos(enu_t::relative(sat_pos, usr_pos.xyz));
|
245
|
+
|
246
|
+
// Tropospheric (2.1.4.10.3, A.4.2.4)
|
247
|
+
res.range_residual += (range_error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
|
248
|
+
? _space_node.tropo_correction(time_arrival.year(), relative_pos, usr_pos.llh)
|
249
|
+
: range_error.value[range_error_t::TROPOSPHERIC];
|
250
|
+
|
251
|
+
// Ionospheric (2.1.4.10.2, A.4.4.10.4)
|
252
|
+
if(range_error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
|
253
|
+
// Ionospheric model selection, the fall back is no correction
|
254
|
+
bool iono_model_hit(false);
|
255
|
+
for(std::size_t i(0); i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]); ++i){
|
256
|
+
switch(_options.ionospheric_models[i]){
|
257
|
+
case options_t::IONOSPHERIC_KLOBUCHAR: // 20.3.3.5.2.5
|
258
|
+
res.range_residual += space_node_gps->iono_correction(relative_pos, usr_pos.llh, time_arrival);
|
259
|
+
break;
|
260
|
+
case options_t::IONOSPHERIC_SBAS: { // 2.1.4.10.2, A.4.4.10.4
|
261
|
+
typename space_node_t::IonosphericGridPoints::PointProperty prop(
|
262
|
+
sat->ionospheric_grid_points().iono_correction(relative_pos, usr_pos.llh));
|
263
|
+
if(!prop.is_available()){continue;}
|
264
|
+
res.range_residual += prop.delay;
|
265
|
+
break;
|
266
|
+
}
|
267
|
+
case options_t::IONOSPHERIC_NTCM_GL: {
|
268
|
+
// TODO f_10_7 setup, optimization (mag_model etc.)
|
269
|
+
typename gps_space_node_t::pierce_point_res_t pp(gps_space_node_t::pierce_point(relative_pos, usr_pos.llh));
|
270
|
+
res.range_residual -= gps_space_node_t::tec2delay(
|
271
|
+
gps_space_node_t::slant_factor(relative_pos)
|
272
|
+
* NTCM_GL_Generic<float_t>::tec_vert(
|
273
|
+
pp.latitude, pp.longitude,
|
274
|
+
time_arrival.year(), _options.f_10_7));
|
275
|
+
break;
|
276
|
+
}
|
277
|
+
case options_t::IONOSPHERIC_NONE:
|
278
|
+
break;
|
279
|
+
default:
|
280
|
+
continue;
|
281
|
+
}
|
282
|
+
iono_model_hit = true;
|
283
|
+
break;
|
284
|
+
}
|
285
|
+
}else{
|
286
|
+
res.range_residual += range_error.value[range_error_t::IONOSPHERIC];
|
287
|
+
}
|
288
|
+
|
289
|
+
// TODO Fast corrections (2.1.1.4.12)
|
290
|
+
|
291
|
+
// TODO Setup weight
|
292
|
+
if(std::abs(res.range_residual) > _options.residual_mask){
|
293
|
+
// If residual is too big, gently exclude it by decreasing its weight.
|
294
|
+
res.weight = 1E-8;
|
295
|
+
}else{
|
296
|
+
|
297
|
+
float_t elv(relative_pos.elevation());
|
298
|
+
if(elv < _options.elevation_mask){
|
299
|
+
res.weight = 0; // exclude it when elevation is less than threshold
|
300
|
+
}else{
|
301
|
+
// elevation weight based on "GPS���p�v���O���~���O" @see GPS_Solver.h
|
302
|
+
res.weight = std::pow(sin(elv)/0.8, 2);
|
303
|
+
if(res.weight < 1E-3){res.weight = 1E-3;}
|
304
|
+
}
|
305
|
+
}
|
306
|
+
|
307
|
+
res.range_corrected = range;
|
308
|
+
|
309
|
+
xyz_t rel_vel(sat_pos_vel.velocity - usr_vel); // Calculate velocity
|
310
|
+
|
311
|
+
// Note: clock rate error is already accounted for in constellation()
|
312
|
+
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
313
|
+
+ res.los_neg[1] * rel_vel.y()
|
314
|
+
+ res.los_neg[2] * rel_vel.z();
|
315
|
+
|
316
|
+
return res;
|
317
|
+
}
|
318
|
+
|
319
|
+
xyz_t *satellite_position(
|
320
|
+
const prn_t &prn,
|
321
|
+
const gps_time_t &time,
|
322
|
+
xyz_t &res) const {
|
323
|
+
|
324
|
+
const satellite_t *sat(is_available(prn, time));
|
325
|
+
return sat ? &(res = sat->ephemeris().constellation(time).position) : NULL;
|
326
|
+
}
|
327
|
+
};
|
328
|
+
|
329
|
+
#endif /* __SBAS_SOLVER_H__ */
|