gps_pvt 0.1.1

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.
Files changed (45) hide show
  1. checksums.yaml +7 -0
  2. data/.rspec +3 -0
  3. data/CHANGELOG.md +5 -0
  4. data/CODE_OF_CONDUCT.md +84 -0
  5. data/Gemfile +10 -0
  6. data/README.md +86 -0
  7. data/Rakefile +86 -0
  8. data/bin/console +15 -0
  9. data/bin/setup +8 -0
  10. data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +6613 -0
  11. data/ext/gps_pvt/GPS/GPS_wrap.cxx +16019 -0
  12. data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +21050 -0
  13. data/ext/gps_pvt/extconf.rb +70 -0
  14. data/ext/ninja-scan-light/tool/navigation/EGM.h +2971 -0
  15. data/ext/ninja-scan-light/tool/navigation/GPS.h +2432 -0
  16. data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +479 -0
  17. data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +1081 -0
  18. data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +199 -0
  19. data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +210 -0
  20. data/ext/ninja-scan-light/tool/navigation/MagneticField.h +928 -0
  21. data/ext/ninja-scan-light/tool/navigation/NTCM.h +211 -0
  22. data/ext/ninja-scan-light/tool/navigation/RINEX.h +1781 -0
  23. data/ext/ninja-scan-light/tool/navigation/WGS84.h +186 -0
  24. data/ext/ninja-scan-light/tool/navigation/coordinate.h +406 -0
  25. data/ext/ninja-scan-light/tool/param/bit_array.h +145 -0
  26. data/ext/ninja-scan-light/tool/param/complex.h +558 -0
  27. data/ext/ninja-scan-light/tool/param/matrix.h +4049 -0
  28. data/ext/ninja-scan-light/tool/param/matrix_fixed.h +665 -0
  29. data/ext/ninja-scan-light/tool/param/matrix_special.h +562 -0
  30. data/ext/ninja-scan-light/tool/param/quaternion.h +765 -0
  31. data/ext/ninja-scan-light/tool/param/vector3.h +651 -0
  32. data/ext/ninja-scan-light/tool/swig/Coordinate.i +177 -0
  33. data/ext/ninja-scan-light/tool/swig/GPS.i +1102 -0
  34. data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1234 -0
  35. data/ext/ninja-scan-light/tool/swig/extconf.rb +5 -0
  36. data/ext/ninja-scan-light/tool/swig/makefile +53 -0
  37. data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +417 -0
  38. data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +489 -0
  39. data/gps_pvt.gemspec +57 -0
  40. data/lib/gps_pvt/receiver.rb +375 -0
  41. data/lib/gps_pvt/ubx.rb +148 -0
  42. data/lib/gps_pvt/version.rb +5 -0
  43. data/lib/gps_pvt.rb +9 -0
  44. data/sig/gps_pvt.rbs +4 -0
  45. metadata +117 -0
@@ -0,0 +1,199 @@
1
+ /**
2
+ * @file GPS multi-frequency 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 __GPS_SOLVER_MULTI_FREQUENCY_H__
38
+ #define __GPS_SOLVER_MULTI_FREQUENCY_H__
39
+
40
+ #include "GPS_Solver.h"
41
+
42
+ template <class FloatT>
43
+ struct GPS_Solver_MultiFrequency_Options {
44
+ bool exclude_L2C;
45
+ GPS_Solver_MultiFrequency_Options() : exclude_L2C(false) {}
46
+ };
47
+
48
+ template <class BaseSolver = GPS_SinglePositioning<double> >
49
+ class GPS_Solver_MultiFrequency : public BaseSolver {
50
+ public:
51
+ typedef GPS_Solver_MultiFrequency<BaseSolver> self_t;
52
+ typedef BaseSolver super_t;
53
+ private:
54
+ self_t &operator=(const self_t &);
55
+ public:
56
+ #if defined(__GNUC__) && (__GNUC__ < 5)
57
+ #define inheritate_type(x) typedef typename super_t::x x;
58
+ #else
59
+ #define inheritate_type(x) using typename super_t::x;
60
+ #endif
61
+ inheritate_type(float_t);
62
+ inheritate_type(space_node_t);
63
+ inheritate_type(range_error_t);
64
+ inheritate_type(measurement_item_set_t);
65
+ #undef inheritate_type
66
+
67
+ typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
68
+ GPS_Solver_MultiFrequency_Options<float_t>, super_t> options_t;
69
+
70
+ GPS_Solver_MultiFrequency_Options<float_t> options_frequency;
71
+
72
+ public:
73
+ options_t available_options() const {
74
+ return options_t(super_t::available_options());
75
+ }
76
+
77
+ options_t available_options(const options_t &opt_wish) const {
78
+ return options_t(super_t::available_options(opt_wish), opt_wish);
79
+ }
80
+
81
+ options_t update_options(const options_t &opt_wish){
82
+ return options_t(
83
+ super_t::update_options(opt_wish),
84
+ options_frequency = opt_wish);
85
+ }
86
+
87
+ GPS_Solver_MultiFrequency(const space_node_t &sn)
88
+ : super_t(sn), options_frequency() {}
89
+
90
+ ~GPS_Solver_MultiFrequency(){}
91
+
92
+ struct measurement_items_t : public super_t::measurement_items_t {
93
+ enum {
94
+ PREDEFINED_LAST = super_t::measurement_items_t::MEASUREMENT_ITEMS_PREDEFINED - 1,
95
+ #define make_entry(key) \
96
+ L2CM_ ## key, \
97
+ L2CL_ ## key
98
+ #define make_entry2(key) \
99
+ make_entry(key), \
100
+ make_entry(key ## _SIGMA)
101
+ make_entry2(PSEUDORANGE),
102
+ make_entry2(CARRIER_PHASE),
103
+ make_entry2(DOPPLER),
104
+ make_entry2(RANGE_RATE),
105
+ make_entry(SIGNAL_STRENGTH_dBHz),
106
+ make_entry(LOCK_SEC),
107
+ #undef make_entry2
108
+ #undef make_entry
109
+ MEASUREMENT_ITEMS_PREDEFINED,
110
+ };
111
+ };
112
+
113
+ /**
114
+ * Extract range information from measurement per satellite
115
+ * @param values measurement[prn]
116
+ * @param buf buffer into which range is stored
117
+ * @param error optional argument in which error components of range will be returned
118
+ * @return If valid range information is found, the pointer of buf will be returned; otherwise NULL
119
+ */
120
+ virtual const float_t *range(
121
+ const typename super_t::measurement_t::mapped_type &values, float_t &buf,
122
+ range_error_t *error) const {
123
+ float_t l1, l2;
124
+ const float_t
125
+ *l1_p(super_t::find_value(values, measurement_items_t::L1_PSEUDORANGE, l1)),
126
+ *l2_p(NULL);
127
+ options_frequency.exclude_L2C // if false then look for L2CM and L2CL. L2CM is higher priority than L2CL
128
+ || (l2_p = super_t::find_value(values, measurement_items_t::L2CM_PSEUDORANGE, l2))
129
+ || (l2_p = super_t::find_value(values, measurement_items_t::L2CL_PSEUDORANGE, l2));
130
+
131
+ if(error){
132
+ *error = range_error_t::not_corrected;
133
+ }
134
+
135
+ if(l1_p){
136
+ if(l2_p && error){ // L1 and L2
137
+ error->unknown_flag &= ~(range_error_t::MASK_IONOSPHERIC);
138
+ error->value[range_error_t::IONOSPHERIC]
139
+ = (l2 - l1) / (space_node_t::gamma_L1_L2 - 1);
140
+ /* @see IS-GPS-200H 20.3.3.3.3.3
141
+ * PR = PR_L1 + (PR_L2 - PR_L1)/(1 - gamma), and PR + error = PR_L1.
142
+ * Therefore, error = (PR_L2 - PR_L1)/(gamma - 1)
143
+ */
144
+ }
145
+ return &(buf = l1);
146
+ }
147
+ #if 0
148
+ /* TODO L2 only, because rate and deviation without L2 has not
149
+ * yet been implemented.
150
+ */
151
+ else if(l2_p){
152
+ return &(buf = l2);
153
+ }
154
+ #endif
155
+ else{ // no range information
156
+ return NULL;
157
+ }
158
+ }
159
+
160
+ static const measurement_item_set_t L2CM, L2CL;
161
+ };
162
+
163
+ template <class BaseSolver>
164
+ const typename GPS_Solver_MultiFrequency<BaseSolver>::measurement_item_set_t
165
+ GPS_Solver_MultiFrequency<BaseSolver>::L2CM = {
166
+ #define make_entry(key) \
167
+ GPS_Solver_MultiFrequency<BaseSolver>::measurement_items_t::L2CM_ ## key
168
+ #define make_entry2(key) { \
169
+ make_entry(key), \
170
+ make_entry(key ## _SIGMA)}
171
+ make_entry2(PSEUDORANGE),
172
+ make_entry2(DOPPLER),
173
+ make_entry2(CARRIER_PHASE),
174
+ make_entry2(RANGE_RATE),
175
+ make_entry(SIGNAL_STRENGTH_dBHz),
176
+ make_entry(LOCK_SEC),
177
+ #undef make_entry2
178
+ #undef make_entry
179
+ };
180
+
181
+ template <class BaseSolver>
182
+ const typename GPS_Solver_MultiFrequency<BaseSolver>::measurement_item_set_t
183
+ GPS_Solver_MultiFrequency<BaseSolver>::L2CL = {
184
+ #define make_entry(key) \
185
+ GPS_Solver_MultiFrequency<BaseSolver>::measurement_items_t::L2CL_ ## key
186
+ #define make_entry2(key) { \
187
+ make_entry(key), \
188
+ make_entry(key ## _SIGMA)}
189
+ make_entry2(PSEUDORANGE),
190
+ make_entry2(DOPPLER),
191
+ make_entry2(CARRIER_PHASE),
192
+ make_entry2(RANGE_RATE),
193
+ make_entry(SIGNAL_STRENGTH_dBHz),
194
+ make_entry(LOCK_SEC),
195
+ #undef make_entry2
196
+ #undef make_entry
197
+ };
198
+
199
+ #endif /* __GPS_SOLVER_MULTI_FREQUENCY_H__ */
@@ -0,0 +1,210 @@
1
+ /**
2
+ * @file GPS solver with RAIM (Receiver Autonomous Integrity Monitoring)
3
+ */
4
+ /*
5
+ * Copyright (c) 2021, M.Naruoka (fenrir)
6
+ * All rights reserved.
7
+ *
8
+ * Redistribution and use in source and binary forms, with or without modification,
9
+ * are permitted provided that the following conditions are met:
10
+ *
11
+ * - Redistributions of source code must retain the above copyright notice,
12
+ * this list of conditions and the following disclaimer.
13
+ * - Redistributions in binary form must reproduce the above copyright notice,
14
+ * this list of conditions and the following disclaimer in the documentation
15
+ * and/or other materials provided with the distribution.
16
+ * - Neither the name of the naruoka.org nor the names of its contributors
17
+ * may be used to endorse or promote products derived from this software
18
+ * without specific prior written permission.
19
+ *
20
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
24
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
25
+ * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
31
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32
+ *
33
+ */
34
+
35
+ #ifndef __GPS_SOLVER_RAIM_H__
36
+ #define __GPS_SOLVER_RAIM_H__
37
+
38
+ #include "GPS_Solver_Base.h"
39
+
40
+ #include <algorithm>
41
+ #include <deque>
42
+
43
+ template <class FloatT, class PVT_BaseT = typename GPS_Solver_Base<FloatT>::user_pvt_t>
44
+ struct GPS_PVT_RAIM_LSR : public PVT_BaseT {
45
+ struct detection_t {
46
+ bool valid;
47
+ struct slope_t {
48
+ FloatT max;
49
+ typename GPS_Solver_Base<FloatT>::prn_t prn;
50
+ } slope_HV[2];
51
+ FloatT wssr;
52
+ FloatT wssr_sf; ///< for nominal bias consideration, sum(eigen.values(W (I - P)))
53
+ FloatT weight_max;
54
+ } FD; ///< Fault detection
55
+ struct exclusion_t : public detection_t {
56
+ typename GPS_Solver_Base<FloatT>::prn_t excluded;
57
+ typename GPS_Solver_Base<FloatT>::pos_t user_position;
58
+ typename GPS_Solver_Base<FloatT>::float_t receiver_error;
59
+ typename GPS_Solver_Base<FloatT>::user_pvt_t::dop_t dop;
60
+ } FDE_min, FDE_2nd; ///< Fault exclusion
61
+ };
62
+
63
+ /*
64
+ * Comment on implementation of protection level (PL) calculation
65
+ *
66
+ * To calculate PL, WSSR threshold is firstly required.
67
+ * WSSR threshold is a function of P_fa (false alarm), DoF (degree of freedom)
68
+ * corresponding to number of usable satellites minus 4, and nominal biases.
69
+ * If nominal biases are configured as zeros, the threshold can be implemented
70
+ * as a map whose key is DoF, without online calculation of CFD of chi-squred distribution.
71
+ * However, in order to consider nominal biases, online calculation is required,
72
+ * and chi-squred distribution also must be implemented.
73
+ * Therefore, currently, PL is assumed to be calculated outside this program,
74
+ * which can be performed with wssr, wssr_sf, slope_HV.
75
+ */
76
+
77
+ template <class FloatT, class SolverBaseT = GPS_Solver_Base<FloatT> >
78
+ struct GPS_Solver_RAIM_LSR : public SolverBaseT {
79
+ typedef SolverBaseT super_t;
80
+ typedef GPS_Solver_RAIM_LSR<FloatT, SolverBaseT> self_t;
81
+ virtual ~GPS_Solver_RAIM_LSR() {}
82
+
83
+ #if defined(__GNUC__) && (__GNUC__ < 5)
84
+ #define inheritate_type(x) typedef typename super_t::x x;
85
+ #else
86
+ #define inheritate_type(x) using typename super_t::x;
87
+ #endif
88
+
89
+ inheritate_type(float_t);
90
+ inheritate_type(matrix_t);
91
+
92
+ inheritate_type(gps_time_t);
93
+ inheritate_type(pos_t);
94
+
95
+ inheritate_type(geometric_matrices_t);
96
+ inheritate_type(measurement2_t);
97
+ #undef inheritate_type
98
+
99
+ typedef GPS_PVT_RAIM_LSR<float_t, typename super_t::user_pvt_t> user_pvt_t;
100
+
101
+ typename super_t::template solver_interface_t<self_t> solve() const {
102
+ return typename super_t::template solver_interface_t<self_t>(*this);
103
+ }
104
+
105
+ protected:
106
+ typedef GPS_Solver_Base<FloatT> base_t;
107
+ bool update_position_solution( // overriding function
108
+ const geometric_matrices_t &geomat,
109
+ typename base_t::user_pvt_t &res) const {
110
+
111
+ if(!super_t::update_position_solution(geomat, res)){return false;}
112
+
113
+ user_pvt_t &pvt(static_cast<user_pvt_t &>(res));
114
+ if(!(pvt.FD.valid = (pvt.used_satellites >= 5))){return true;}
115
+
116
+ // Perform least square again for Fault Detection
117
+ matrix_t S, W_dash;
118
+ typename geometric_matrices_t::partial_t geomat2(geomat.partial(res.used_satellites));
119
+ geomat2.least_square(S);
120
+ pvt.FD.wssr = geomat2.wssr_S(S, &W_dash);
121
+ pvt.FD.wssr_sf = W_dash.trace();
122
+ pvt.FD.weight_max = *std::max_element(geomat2.W.cbegin(), geomat2.W.cend());
123
+ matrix_t slope_HV(geomat2.slope_HV(S, res.user_position.ecef2enu()));
124
+ std::vector<int> prn_list(res.used_satellite_mask.indices_one());
125
+ for(unsigned i(0); i < 2; ++i){ // horizontal, vertical
126
+ typename matrix_t::partial_t slope_HV_i(slope_HV.partial(slope_HV.rows(), 1, 0, i));
127
+ typename matrix_t::partial_t::const_iterator it(
128
+ std::max_element(slope_HV_i.cbegin(), slope_HV_i.cend()));
129
+ unsigned int row(it.row());
130
+ pvt.FD.slope_HV[i].max = *it;
131
+ pvt.FD.slope_HV[i].prn = (typename GPS_Solver_Base<FloatT>::prn_t)prn_list[row];
132
+ }
133
+
134
+ return true;
135
+ }
136
+
137
+ public:
138
+ using super_t::user_pvt;
139
+ protected:
140
+ void user_pvt( // overriding function
141
+ typename base_t::user_pvt_t &res,
142
+ const measurement2_t &measurement,
143
+ const gps_time_t &receiver_time,
144
+ const pos_t &user_position_init,
145
+ const float_t &receiver_error_init,
146
+ const typename base_t::user_pvt_opt_t &opt
147
+ = typename base_t::user_pvt_opt_t()) const {
148
+
149
+ user_pvt_t &pvt(static_cast<user_pvt_t &>(res));
150
+ pvt.FD.valid = pvt.FDE_min.valid = pvt.FDE_2nd.valid = false;
151
+
152
+ // Solution with full satellites
153
+ super_t::user_pvt(res,
154
+ measurement, receiver_time,
155
+ user_position_init, receiver_error_init,
156
+ opt);
157
+
158
+ if(!pvt.position_solved()
159
+ || (!pvt.FD.valid)
160
+ || (pvt.used_satellites < 6)){return;}
161
+
162
+ // Generate full set
163
+ std::deque<typename measurement2_t::value_type> fullset;
164
+ for(typename measurement2_t::const_iterator it(measurement.begin()), it_end(measurement.end());
165
+ it != it_end; ++it){
166
+ if(!pvt.used_satellite_mask[it->prn]){continue;}
167
+ fullset.push_back(*it);
168
+ }
169
+
170
+ // Subset calculation for Fault Exclusion
171
+ pvt.FDE_min.wssr = pvt.FDE_2nd.wssr = pvt.FD.wssr;
172
+ user_pvt_t pvt_FDE(pvt);
173
+ typename base_t::user_pvt_opt_t opt_subset(true, false); // good init, without velocity
174
+ for(unsigned int i(0); i < pvt.used_satellites - 1;
175
+ ++i, fullset.push_back(fullset.front()), fullset.pop_front()){
176
+ pvt_FDE.FD.valid = false;
177
+ measurement2_t subset(fullset.begin(), fullset.end() - 1);
178
+ super_t::user_pvt(pvt_FDE,
179
+ subset, receiver_time,
180
+ pvt.user_position, pvt.receiver_error,
181
+ opt_subset);
182
+
183
+ if(!pvt_FDE.FD.valid){continue;}
184
+ switch(pvt_FDE.error_code){
185
+ case user_pvt_t::ERROR_NO:
186
+ case user_pvt_t::ERROR_VELOCITY_SKIPPED:
187
+ break;
188
+ default:
189
+ continue;
190
+ }
191
+
192
+ typename user_pvt_t::exclusion_t *target(NULL);
193
+ if(pvt_FDE.FD.wssr < pvt.FDE_min.wssr){
194
+ pvt.FDE_2nd = pvt.FDE_min;
195
+ target = &pvt.FDE_min;
196
+ }else if(pvt_FDE.FD.wssr < pvt.FDE_2nd.wssr){
197
+ target = &pvt.FDE_2nd;
198
+ }else{
199
+ continue;
200
+ }
201
+ (typename user_pvt_t::detection_t &)(*target) = pvt_FDE.FD;
202
+ target->excluded = fullset.back().prn;
203
+ target->user_position = pvt_FDE.user_position;
204
+ target->receiver_error = pvt_FDE.receiver_error;
205
+ target->dop = pvt_FDE.dop;
206
+ }
207
+ }
208
+ };
209
+
210
+ #endif /* __GPS_SOLVER_RAIM_H__ */