gps_pvt 0.2.3 → 0.4.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 +35 -7
- data/Rakefile +2 -0
- data/exe/gps_pvt +65 -2
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +5898 -395
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +453 -429
- data/ext/ninja-scan-light/tool/algorithm/integral.h +91 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +1270 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +306 -0
- data/ext/ninja-scan-light/tool/navigation/GPS.h +7 -1
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +0 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +9 -2
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +389 -4
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +0 -0
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +0 -0
- data/ext/ninja-scan-light/tool/param/bit_array.h +0 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +310 -13
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +38 -15
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +7 -1
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +13 -3
- data/gps_pvt.gemspec +0 -0
- data/lib/gps_pvt/receiver.rb +101 -20
- data/lib/gps_pvt/version.rb +1 -1
- metadata +9 -6
@@ -0,0 +1,306 @@
|
|
1
|
+
/**
|
2
|
+
* @file GLONASS solver
|
3
|
+
*
|
4
|
+
*/
|
5
|
+
|
6
|
+
/*
|
7
|
+
* Copyright (c) 2022, 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 __GLONASS_SOLVER_H__
|
38
|
+
#define __GLONASS_SOLVER_H__
|
39
|
+
|
40
|
+
#include "GLONASS.h"
|
41
|
+
#include "GPS.h"
|
42
|
+
#include "GPS_Solver_Base.h"
|
43
|
+
#include "GPS_Solver.h"
|
44
|
+
|
45
|
+
#include <cstddef>
|
46
|
+
|
47
|
+
template <class FloatT>
|
48
|
+
struct GLONASS_SinglePositioning_Options : public GPS_Solver_GeneralOptions<FloatT> {
|
49
|
+
typedef GPS_Solver_GeneralOptions<FloatT> super_t;
|
50
|
+
|
51
|
+
typename GPS_Solver_Base<FloatT>::options_t::template exclude_prn_t<1, 24> exclude_prn; // GLONASS svid ranges from 1 to 24
|
52
|
+
GLONASS_SinglePositioning_Options()
|
53
|
+
: super_t(), exclude_prn() {
|
54
|
+
exclude_prn.set(true); // GLONASS ranging is default off.
|
55
|
+
}
|
56
|
+
};
|
57
|
+
|
58
|
+
template <class FloatT, class SolverBaseT = GPS_Solver_Base<FloatT> >
|
59
|
+
class GLONASS_SinglePositioning : public SolverBaseT {
|
60
|
+
private:
|
61
|
+
GLONASS_SinglePositioning<FloatT> &operator=(const GLONASS_SinglePositioning<FloatT> &);
|
62
|
+
public:
|
63
|
+
typedef GLONASS_SinglePositioning<FloatT> self_t;
|
64
|
+
typedef SolverBaseT base_t;
|
65
|
+
typedef SolverBaseT super_t;
|
66
|
+
|
67
|
+
#if defined(__GNUC__) && (__GNUC__ < 5)
|
68
|
+
#define inheritate_type(x) typedef typename base_t::x x;
|
69
|
+
#else
|
70
|
+
#define inheritate_type(x) using typename base_t::x;
|
71
|
+
#endif
|
72
|
+
|
73
|
+
inheritate_type(float_t);
|
74
|
+
inheritate_type(prn_t);
|
75
|
+
|
76
|
+
typedef GLONASS_SpaceNode<float_t> space_node_t;
|
77
|
+
inheritate_type(gps_time_t);
|
78
|
+
typedef typename space_node_t::Satellite satellite_t;
|
79
|
+
|
80
|
+
inheritate_type(xyz_t);
|
81
|
+
inheritate_type(enu_t);
|
82
|
+
|
83
|
+
inheritate_type(pos_t);
|
84
|
+
|
85
|
+
typedef typename base_t::measurement_t measurement_t;
|
86
|
+
inheritate_type(range_error_t);
|
87
|
+
inheritate_type(range_corrector_t);
|
88
|
+
inheritate_type(range_correction_t);
|
89
|
+
|
90
|
+
inheritate_type(relative_property_t);
|
91
|
+
typedef typename super_t::measurement_items_t measurement_items_t;
|
92
|
+
#undef inheritate_type
|
93
|
+
|
94
|
+
static const typename base_t::measurement_item_set_t L1OF;
|
95
|
+
|
96
|
+
typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
|
97
|
+
GLONASS_SinglePositioning_Options<float_t>, base_t> options_t;
|
98
|
+
|
99
|
+
protected:
|
100
|
+
const space_node_t &_space_node;
|
101
|
+
GLONASS_SinglePositioning_Options<float_t> _options;
|
102
|
+
|
103
|
+
public:
|
104
|
+
const space_node_t &space_node() const {return _space_node;}
|
105
|
+
|
106
|
+
range_correction_t ionospheric_correction, tropospheric_correction;
|
107
|
+
|
108
|
+
options_t available_options() const {
|
109
|
+
return options_t(base_t::available_options(), _options);
|
110
|
+
}
|
111
|
+
|
112
|
+
options_t available_options(const options_t &opt_wish) const {
|
113
|
+
GLONASS_SinglePositioning_Options<float_t> opt(opt_wish);
|
114
|
+
return options_t(base_t::available_options(opt_wish), opt);
|
115
|
+
}
|
116
|
+
|
117
|
+
options_t update_options(const options_t &opt_wish){
|
118
|
+
_options = opt_wish;
|
119
|
+
return options_t(base_t::update_options(opt_wish), _options);
|
120
|
+
}
|
121
|
+
|
122
|
+
GLONASS_SinglePositioning(const space_node_t &sn, const options_t &opt_wish = options_t())
|
123
|
+
: base_t(), _space_node(sn), _options(available_options(opt_wish)),
|
124
|
+
ionospheric_correction(), tropospheric_correction() {
|
125
|
+
|
126
|
+
// default ionospheric correction: no correction
|
127
|
+
ionospheric_correction.push_front(&base_t::no_correction);
|
128
|
+
|
129
|
+
// default troposheric correction: no correction
|
130
|
+
tropospheric_correction.push_front(&base_t::no_correction);
|
131
|
+
}
|
132
|
+
|
133
|
+
~GLONASS_SinglePositioning(){}
|
134
|
+
|
135
|
+
virtual const float_t *rate(
|
136
|
+
const typename measurement_t::mapped_type &values, float_t &buf) const {
|
137
|
+
const float_t *res;
|
138
|
+
if((res = super_t::find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){
|
139
|
+
return res;
|
140
|
+
}
|
141
|
+
// Fall back to doppler * frequency
|
142
|
+
float_t doppler, freq;
|
143
|
+
if(super_t::find_value(values, measurement_items_t::L1_DOPPLER, doppler)
|
144
|
+
&& super_t::find_value(values, measurement_items_t::L1_FREQUENCY, freq)){
|
145
|
+
return &(buf = -doppler * (space_node_t::light_speed / freq));
|
146
|
+
}
|
147
|
+
return NULL;
|
148
|
+
}
|
149
|
+
|
150
|
+
virtual const float_t *rate_sigma(
|
151
|
+
const typename measurement_t::mapped_type &values, float_t &buf) const {
|
152
|
+
const float_t *res;
|
153
|
+
if((res = super_t::find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){
|
154
|
+
return res;
|
155
|
+
}
|
156
|
+
// Fall back to doppler * frequency
|
157
|
+
float_t doppler, freq;
|
158
|
+
if(super_t::find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, doppler)
|
159
|
+
&& super_t::find_value(values, measurement_items_t::L1_FREQUENCY, freq)){
|
160
|
+
return &(buf = doppler * (space_node_t::light_speed / freq));
|
161
|
+
}
|
162
|
+
return NULL;
|
163
|
+
}
|
164
|
+
|
165
|
+
/**
|
166
|
+
* Check availability of a satellite with which observation is associated
|
167
|
+
*
|
168
|
+
* @param prn satellite number
|
169
|
+
* @param receiver_time receiver time
|
170
|
+
* @return (const satellite_t *) If available, const pointer to satellite is returned,
|
171
|
+
* otherwise NULL.
|
172
|
+
*/
|
173
|
+
const satellite_t *is_available(
|
174
|
+
const typename space_node_t::satellites_t::key_type &prn,
|
175
|
+
const gps_time_t &receiver_time) const {
|
176
|
+
|
177
|
+
const typename space_node_t::satellites_t &sats(_space_node.satellites());
|
178
|
+
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn & 0xFF));
|
179
|
+
if((it_sat == sats.end()) // has ephemeris?
|
180
|
+
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
181
|
+
return NULL;
|
182
|
+
}
|
183
|
+
|
184
|
+
return &(it_sat->second);
|
185
|
+
}
|
186
|
+
|
187
|
+
/**
|
188
|
+
* Calculate relative range and rate information to a satellite
|
189
|
+
*
|
190
|
+
* @param prn satellite number
|
191
|
+
* @param measurement measurement (per satellite) containing pseudo range
|
192
|
+
* @param receiver_error (temporal solution of) receiver clock error in meter
|
193
|
+
* @param time_arrival time when signal arrive at receiver
|
194
|
+
* @param usr_pos (temporal solution of) user position
|
195
|
+
* @param usr_vel (temporal solution of) user velocity
|
196
|
+
* @return (relative_property_t) relative information
|
197
|
+
*/
|
198
|
+
relative_property_t relative_property(
|
199
|
+
const prn_t &prn,
|
200
|
+
const typename measurement_t::mapped_type &measurement,
|
201
|
+
const float_t &receiver_error,
|
202
|
+
const gps_time_t &time_arrival,
|
203
|
+
const pos_t &usr_pos,
|
204
|
+
const xyz_t &usr_vel) const {
|
205
|
+
|
206
|
+
relative_property_t res = {0};
|
207
|
+
|
208
|
+
if(_options.exclude_prn[prn & 0xFF]){return res;}
|
209
|
+
|
210
|
+
float_t range;
|
211
|
+
range_error_t range_error;
|
212
|
+
if(!this->range(measurement, range, &range_error)){
|
213
|
+
return res; // If no range entry, return with weight = 0
|
214
|
+
}
|
215
|
+
|
216
|
+
const satellite_t *sat(is_available(prn, time_arrival));
|
217
|
+
if(!sat){return res;} // If satellite is unavailable, return with weight = 0
|
218
|
+
|
219
|
+
range -= receiver_error;
|
220
|
+
|
221
|
+
// Clock correction, which will be considered in the next constellation()
|
222
|
+
// as extra transmission time by using extra psuedo range.
|
223
|
+
if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){
|
224
|
+
range += (sat->clock_error(time_arrival, range) * space_node_t::light_speed);
|
225
|
+
}else{
|
226
|
+
range += range_error.value[range_error_t::SATELLITE_CLOCK];
|
227
|
+
}
|
228
|
+
|
229
|
+
// Calculate satellite position and velocity
|
230
|
+
typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t sat_pos_vel(
|
231
|
+
sat->constellation(time_arrival, range));
|
232
|
+
|
233
|
+
const xyz_t &sat_pos(sat_pos_vel.position);
|
234
|
+
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
235
|
+
|
236
|
+
// Calculate residual
|
237
|
+
res.range_residual = range - 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
|
247
|
+
res.range_residual += (range_error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
|
248
|
+
? tropospheric_correction(time_arrival, usr_pos, relative_pos)
|
249
|
+
: range_error.value[range_error_t::TROPOSPHERIC];
|
250
|
+
|
251
|
+
// Ionospheric
|
252
|
+
if(range_error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
|
253
|
+
float_t freq(space_node_t::L1_frequency_base); // ch.0 is fallback
|
254
|
+
super_t::find_value(measurement, measurement_items_t::L1_FREQUENCY, freq);
|
255
|
+
// ionospheric models are assumed to work with GPS L1
|
256
|
+
res.range_residual +=
|
257
|
+
(ionospheric_correction(time_arrival, usr_pos, relative_pos)
|
258
|
+
* GPS_SpaceNode<float_t>::gamma_per_L1(freq));
|
259
|
+
}else{
|
260
|
+
res.range_residual += range_error.value[range_error_t::IONOSPHERIC];
|
261
|
+
}
|
262
|
+
|
263
|
+
// Setup weight
|
264
|
+
if(std::abs(res.range_residual) > _options.residual_mask){
|
265
|
+
// If residual is too big, gently exclude it by decreasing its weight.
|
266
|
+
res.weight = 1E-8;
|
267
|
+
}else{
|
268
|
+
|
269
|
+
float_t elv(relative_pos.elevation());
|
270
|
+
if(elv < _options.elevation_mask){
|
271
|
+
res.weight = 0; // exclude it when elevation is less than threshold
|
272
|
+
}else{
|
273
|
+
// elevation weight based on "GPS���p�v���O���~���O" @see GPS_Solver.h
|
274
|
+
res.weight = std::pow(sin(elv)/0.8, 2);
|
275
|
+
if(res.weight < 1E-3){res.weight = 1E-3;}
|
276
|
+
}
|
277
|
+
}
|
278
|
+
|
279
|
+
res.range_corrected = range;
|
280
|
+
|
281
|
+
xyz_t rel_vel(sat_pos_vel.velocity - usr_vel); // Calculate velocity
|
282
|
+
|
283
|
+
// Note: clock rate error is already accounted for in constellation()
|
284
|
+
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
285
|
+
+ res.los_neg[1] * rel_vel.y()
|
286
|
+
+ res.los_neg[2] * rel_vel.z();
|
287
|
+
|
288
|
+
return res;
|
289
|
+
}
|
290
|
+
|
291
|
+
xyz_t *satellite_position(
|
292
|
+
const prn_t &prn,
|
293
|
+
const gps_time_t &time,
|
294
|
+
xyz_t &res) const {
|
295
|
+
|
296
|
+
const satellite_t *sat(is_available(prn, time));
|
297
|
+
return sat ? &(res = sat->position(time)) : NULL;
|
298
|
+
}
|
299
|
+
};
|
300
|
+
|
301
|
+
template <class FloatT, class SolverBaseT>
|
302
|
+
const typename SolverBaseT::measurement_item_set_t
|
303
|
+
GLONASS_SinglePositioning<FloatT, SolverBaseT>::L1OF
|
304
|
+
= SolverBaseT::L1CA;
|
305
|
+
|
306
|
+
#endif /* __GLONASS_SOLVER_H__ */
|
@@ -339,10 +339,16 @@ struct GPS_Time {
|
|
339
339
|
return t.operator<=(*this);
|
340
340
|
}
|
341
341
|
|
342
|
+
/**
|
343
|
+
* Convert to std::tm struct
|
344
|
+
* @param leap_seconds If offset of GPS time relative to UTC is known,
|
345
|
+
* specify it by using this parameter.
|
346
|
+
* As of Jan. 1st, 2022, +18 seconds are specified.
|
347
|
+
*/
|
342
348
|
std::tm c_tm(const float_t &leap_seconds = 0) const {
|
343
349
|
std::tm t;
|
344
350
|
|
345
|
-
GPS_Time mod_t((*this)
|
351
|
+
GPS_Time mod_t((*this) - leap_seconds);
|
346
352
|
|
347
353
|
std::div_t min_sec(std::div((int)mod_t.seconds, 60));
|
348
354
|
t.tm_sec = min_sec.rem;
|
File without changes
|
@@ -271,6 +271,13 @@ struct GPS_Solver_Base {
|
|
271
271
|
remove(v);
|
272
272
|
super_t::push_front(v);
|
273
273
|
}
|
274
|
+
void add(const super_t &values){
|
275
|
+
for(typename super_t::const_reverse_iterator
|
276
|
+
it(values.rbegin()), it_end(values.rend());
|
277
|
+
it != it_end; ++it){
|
278
|
+
add(*it);
|
279
|
+
}
|
280
|
+
}
|
274
281
|
};
|
275
282
|
|
276
283
|
/**
|
@@ -652,14 +659,14 @@ protected:
|
|
652
659
|
// Least square
|
653
660
|
matrix_t delta_x(geomat.partial(res.used_satellites).least_square());
|
654
661
|
|
655
|
-
xyz_t delta_user_position(delta_x.partial(3, 1
|
662
|
+
xyz_t delta_user_position(delta_x.partial(3, 1));
|
656
663
|
res.user_position.xyz += delta_user_position;
|
657
664
|
res.user_position.llh = res.user_position.xyz.llh();
|
658
665
|
|
659
666
|
const float_t &delta_receiver_error(delta_x(3, 0));
|
660
667
|
res.receiver_error += delta_receiver_error;
|
661
668
|
|
662
|
-
return (
|
669
|
+
return (delta_x.partial(4, 1).norm2F() <= 1E-6); // equivalent to abs(x) <= 1E-3 [m]
|
663
670
|
}
|
664
671
|
|
665
672
|
struct user_pvt_opt_t {
|