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.
- checksums.yaml +7 -0
- data/.rspec +3 -0
- data/CHANGELOG.md +5 -0
- data/CODE_OF_CONDUCT.md +84 -0
- data/Gemfile +10 -0
- data/README.md +86 -0
- data/Rakefile +86 -0
- data/bin/console +15 -0
- data/bin/setup +8 -0
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +6613 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +16019 -0
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +21050 -0
- data/ext/gps_pvt/extconf.rb +70 -0
- data/ext/ninja-scan-light/tool/navigation/EGM.h +2971 -0
- data/ext/ninja-scan-light/tool/navigation/GPS.h +2432 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +479 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +1081 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +199 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +210 -0
- data/ext/ninja-scan-light/tool/navigation/MagneticField.h +928 -0
- data/ext/ninja-scan-light/tool/navigation/NTCM.h +211 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +1781 -0
- data/ext/ninja-scan-light/tool/navigation/WGS84.h +186 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +406 -0
- data/ext/ninja-scan-light/tool/param/bit_array.h +145 -0
- data/ext/ninja-scan-light/tool/param/complex.h +558 -0
- data/ext/ninja-scan-light/tool/param/matrix.h +4049 -0
- data/ext/ninja-scan-light/tool/param/matrix_fixed.h +665 -0
- data/ext/ninja-scan-light/tool/param/matrix_special.h +562 -0
- data/ext/ninja-scan-light/tool/param/quaternion.h +765 -0
- data/ext/ninja-scan-light/tool/param/vector3.h +651 -0
- data/ext/ninja-scan-light/tool/swig/Coordinate.i +177 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +1102 -0
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1234 -0
- data/ext/ninja-scan-light/tool/swig/extconf.rb +5 -0
- data/ext/ninja-scan-light/tool/swig/makefile +53 -0
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +417 -0
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +489 -0
- data/gps_pvt.gemspec +57 -0
- data/lib/gps_pvt/receiver.rb +375 -0
- data/lib/gps_pvt/ubx.rb +148 -0
- data/lib/gps_pvt/version.rb +5 -0
- data/lib/gps_pvt.rb +9 -0
- data/sig/gps_pvt.rbs +4 -0
- 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__ */
|