1 | #include <cmath>
|
---|
2 | #include <iostream>
|
---|
3 | #include <sstream>
|
---|
4 | #include <iomanip>
|
---|
5 | #include <algorithm>
|
---|
6 | #include <newmatio.h>
|
---|
7 | #include "parlist.h"
|
---|
8 | #include "satobs.h"
|
---|
9 |
|
---|
10 | #include "station.h"
|
---|
11 | #include "bncutils.h"
|
---|
12 | #include "bncconst.h"
|
---|
13 | #include "pppClient.h"
|
---|
14 |
|
---|
15 | using namespace BNC;
|
---|
16 | using namespace std;
|
---|
17 |
|
---|
18 | // Constructor
|
---|
19 | ////////////////////////////////////////////////////////////////////////////
|
---|
20 | t_param::t_param(e_type type, const t_prn& prn, t_lc::type tLC,
|
---|
21 | const vector<t_satObs*>* obsVector) {
|
---|
22 |
|
---|
23 | _type = type;
|
---|
24 | _prn = prn;
|
---|
25 | _tLC = tLC;
|
---|
26 | _x0 = 0.0;
|
---|
27 | _indexOld = -1;
|
---|
28 | _indexNew = -1;
|
---|
29 | _noise = 0.0;
|
---|
30 | _ambInfo = 0;
|
---|
31 |
|
---|
32 | switch (_type) {
|
---|
33 | case crdX:
|
---|
34 | _epoSpec = true;
|
---|
35 | _sigma0 = OPT->_sigCrd[0];
|
---|
36 | break;
|
---|
37 | case crdY:
|
---|
38 | _epoSpec = true;
|
---|
39 | _sigma0 = OPT->_sigCrd[1];
|
---|
40 | break;
|
---|
41 | case crdZ:
|
---|
42 | _epoSpec = true;
|
---|
43 | _sigma0 = OPT->_sigCrd[2];
|
---|
44 | break;
|
---|
45 | case clkR:
|
---|
46 | _epoSpec = true;
|
---|
47 | _sigma0 = 1000.0;
|
---|
48 | break;
|
---|
49 | case amb:
|
---|
50 | _ambInfo = new t_ambInfo();
|
---|
51 | if (obsVector) {
|
---|
52 | for (unsigned ii = 0; ii < obsVector->size(); ii++) {
|
---|
53 | const t_satObs* obs = obsVector->at(ii);
|
---|
54 | if (obs->prn() == _prn) {
|
---|
55 | double offGG = 0;
|
---|
56 | if (_prn.system() == 'R' && tLC != t_lc::MW) {
|
---|
57 | offGG = PPP_CLIENT->offGG();
|
---|
58 | }
|
---|
59 | _x0 = floor((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC) + 0.5);
|
---|
60 | break;
|
---|
61 | }
|
---|
62 | }
|
---|
63 | }
|
---|
64 | _epoSpec = false;
|
---|
65 | _sigma0 = 100.0;
|
---|
66 | break;
|
---|
67 | case offGG:
|
---|
68 | _epoSpec = true;
|
---|
69 | _sigma0 = 1000.0;
|
---|
70 | _x0 = PPP_CLIENT->offGG();
|
---|
71 | break;
|
---|
72 | case trp:
|
---|
73 | _epoSpec = false;
|
---|
74 | _sigma0 = OPT->_sigTropo;
|
---|
75 | _noise = OPT->_noiseTropo;
|
---|
76 | break;
|
---|
77 | case bias:
|
---|
78 | _epoSpec = true;
|
---|
79 | _sigma0 = 10.0;
|
---|
80 | break;
|
---|
81 | }
|
---|
82 | }
|
---|
83 |
|
---|
84 | // Destructor
|
---|
85 | ////////////////////////////////////////////////////////////////////////////
|
---|
86 | t_param::~t_param() {
|
---|
87 | delete _ambInfo;
|
---|
88 | }
|
---|
89 |
|
---|
90 | //
|
---|
91 | ////////////////////////////////////////////////////////////////////////////
|
---|
92 | double t_param::partial(const bncTime& /* epoTime */, const t_satObs* obs,
|
---|
93 | const t_lc::type& tLC) const {
|
---|
94 |
|
---|
95 | // Special Case - Melbourne-Wuebbena
|
---|
96 | // ---------------------------------
|
---|
97 | if (tLC == t_lc::MW && _type != amb && _type != bias) {
|
---|
98 | return 0.0;
|
---|
99 | }
|
---|
100 |
|
---|
101 | const t_station* sta = PPP_CLIENT->staRover();
|
---|
102 | ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
|
---|
103 |
|
---|
104 | switch (_type) {
|
---|
105 | case crdX:
|
---|
106 | return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius();
|
---|
107 | case crdY:
|
---|
108 | return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius();
|
---|
109 | case crdZ:
|
---|
110 | return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius();
|
---|
111 | case clkR:
|
---|
112 | return 1.0;
|
---|
113 | case offGG:
|
---|
114 | return (obs->prn().system() == 'R') ? 1.0 : 0.0;
|
---|
115 | case amb:
|
---|
116 | if (obs->prn() == _prn) {
|
---|
117 | if (tLC == _tLC) {
|
---|
118 | return (obs->lambda(tLC));
|
---|
119 | }
|
---|
120 | else if (tLC == t_lc::lIF && _tLC == t_lc::MW) {
|
---|
121 | return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2);
|
---|
122 | }
|
---|
123 | else {
|
---|
124 | ColumnVector coeff(4);
|
---|
125 | obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
|
---|
126 | if (_tLC == t_lc::l1) {
|
---|
127 | return obs->lambda(t_lc::l1) * coeff(1);
|
---|
128 | }
|
---|
129 | else if (_tLC == t_lc::l2) {
|
---|
130 | return obs->lambda(t_lc::l2) * coeff(2);
|
---|
131 | }
|
---|
132 | }
|
---|
133 | }
|
---|
134 | return 0.0;
|
---|
135 | case trp:
|
---|
136 | return 1.0 / sin(obs->eleSat());
|
---|
137 | case bias:
|
---|
138 | if (tLC == _tLC && obs->prn().system() == _prn.system()) {
|
---|
139 | return 1.0;
|
---|
140 | }
|
---|
141 | else {
|
---|
142 | return 0.0;
|
---|
143 | }
|
---|
144 | }
|
---|
145 |
|
---|
146 | return 0.0;
|
---|
147 | }
|
---|
148 |
|
---|
149 | //
|
---|
150 | ////////////////////////////////////////////////////////////////////////////
|
---|
151 | string t_param::toString() const {
|
---|
152 | stringstream ss;
|
---|
153 | switch (_type) {
|
---|
154 | case crdX:
|
---|
155 | ss << "CRD_X";
|
---|
156 | break;
|
---|
157 | case crdY:
|
---|
158 | ss << "CRD_Y";
|
---|
159 | break;
|
---|
160 | case crdZ:
|
---|
161 | ss << "CRD_Z";
|
---|
162 | break;
|
---|
163 | case clkR:
|
---|
164 | ss << "CLK ";
|
---|
165 | break;
|
---|
166 | case amb:
|
---|
167 | ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString();
|
---|
168 | break;
|
---|
169 | case offGG:
|
---|
170 | ss << "OGG ";
|
---|
171 | break;
|
---|
172 | case trp:
|
---|
173 | ss << "TRP ";
|
---|
174 | break;
|
---|
175 | case bias:
|
---|
176 | ss << "BIAS " << _prn.system() << ' ' << left << setw(3) << t_lc::toString(_tLC);
|
---|
177 | break;
|
---|
178 | }
|
---|
179 | return ss.str();
|
---|
180 | }
|
---|
181 |
|
---|
182 | // Constructor
|
---|
183 | ////////////////////////////////////////////////////////////////////////////
|
---|
184 | t_parlist::t_parlist() {
|
---|
185 | }
|
---|
186 |
|
---|
187 | // Destructor
|
---|
188 | ////////////////////////////////////////////////////////////////////////////
|
---|
189 | t_parlist::~t_parlist() {
|
---|
190 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
191 | delete _params[ii];
|
---|
192 | }
|
---|
193 | }
|
---|
194 |
|
---|
195 | //
|
---|
196 | ////////////////////////////////////////////////////////////////////////////
|
---|
197 | t_irc t_parlist::set(const bncTime& epoTime, const vector<t_lc::type>& ambLCs,
|
---|
198 | const vector<t_satObs*>& obsVector) {
|
---|
199 |
|
---|
200 | // Remove some Parameters
|
---|
201 | // ----------------------
|
---|
202 | vector<t_param*>::iterator it = _params.begin();
|
---|
203 | while (it != _params.end()) {
|
---|
204 | t_param* par = *it;
|
---|
205 |
|
---|
206 | bool remove = false;
|
---|
207 |
|
---|
208 | if (par->epoSpec()) {
|
---|
209 | remove = true;
|
---|
210 | }
|
---|
211 |
|
---|
212 | else if (par->type() == t_param::amb) {
|
---|
213 | if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) {
|
---|
214 | remove = true;
|
---|
215 | }
|
---|
216 | }
|
---|
217 |
|
---|
218 | else if (par->type() == t_param::amb) {
|
---|
219 | if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) {
|
---|
220 | remove = true;
|
---|
221 | }
|
---|
222 | }
|
---|
223 |
|
---|
224 | if (remove) {
|
---|
225 | delete par;
|
---|
226 | it = _params.erase(it);
|
---|
227 | }
|
---|
228 | else {
|
---|
229 | ++it;
|
---|
230 | }
|
---|
231 | }
|
---|
232 |
|
---|
233 | // Check whether parameters have observations
|
---|
234 | // ------------------------------------------
|
---|
235 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
236 | t_param* par = _params[ii];
|
---|
237 | if (par->prn() == 0) {
|
---|
238 | par->setLastObsTime(epoTime);
|
---|
239 | if (par->firstObsTime().undef()) {
|
---|
240 | par->setFirstObsTime(epoTime);
|
---|
241 | }
|
---|
242 | }
|
---|
243 | else {
|
---|
244 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
245 | const t_satObs* satObs = obsVector[jj];
|
---|
246 | if (satObs->prn() == par->prn()) {
|
---|
247 | par->setLastObsTime(epoTime);
|
---|
248 | if (par->firstObsTime().undef()) {
|
---|
249 | par->setFirstObsTime(epoTime);
|
---|
250 | }
|
---|
251 | break;
|
---|
252 | }
|
---|
253 | }
|
---|
254 | }
|
---|
255 | }
|
---|
256 |
|
---|
257 | // Required Set of Parameters
|
---|
258 | // --------------------------
|
---|
259 | vector<t_param*> required;
|
---|
260 |
|
---|
261 | // Coordinates
|
---|
262 | // -----------
|
---|
263 | required.push_back(new t_param(t_param::crdX, t_prn(), t_lc::dummy));
|
---|
264 | required.push_back(new t_param(t_param::crdY, t_prn(), t_lc::dummy));
|
---|
265 | required.push_back(new t_param(t_param::crdZ, t_prn(), t_lc::dummy));
|
---|
266 |
|
---|
267 | // Receiver Clock
|
---|
268 | // --------------
|
---|
269 | required.push_back(new t_param(t_param::clkR, t_prn(), t_lc::dummy));
|
---|
270 |
|
---|
271 | // GPS-Glonass Clock Offset
|
---|
272 | // ------------------------
|
---|
273 | if (OPT->useGlonass()) {
|
---|
274 | required.push_back(new t_param(t_param::offGG, t_prn(), t_lc::dummy));
|
---|
275 | }
|
---|
276 |
|
---|
277 | // Receiver Biases
|
---|
278 | // ---------------
|
---|
279 | for (unsigned ii = 0; ii < OPT->_estBias.size(); ii++) {
|
---|
280 | const t_options::t_optBias& optBias = OPT->_estBias[ii];
|
---|
281 | required.push_back(new t_param(t_param::bias, t_prn(optBias._system, 1), optBias._tLC));
|
---|
282 | }
|
---|
283 |
|
---|
284 | // Troposphere
|
---|
285 | // -----------
|
---|
286 | if (OPT->estTropo()) {
|
---|
287 | required.push_back(new t_param(t_param::trp, t_prn(), t_lc::dummy));
|
---|
288 | }
|
---|
289 |
|
---|
290 | // Ambiguities
|
---|
291 | // -----------
|
---|
292 | for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
|
---|
293 | const t_lc::type& tLC = ambLCs[ii];
|
---|
294 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
295 | const t_satObs* satObs = obsVector[jj];
|
---|
296 | required.push_back(new t_param(t_param::amb, satObs->prn(), tLC, &obsVector));
|
---|
297 | }
|
---|
298 | }
|
---|
299 |
|
---|
300 | // Check if all required parameters are present
|
---|
301 | // --------------------------------------------
|
---|
302 | for (unsigned ii = 0; ii < required.size(); ii++) {
|
---|
303 | t_param* parReq = required[ii];
|
---|
304 |
|
---|
305 | bool found = false;
|
---|
306 | for (unsigned jj = 0; jj < _params.size(); jj++) {
|
---|
307 | t_param* parOld = _params[jj];
|
---|
308 | if (parOld->isEqual(parReq)) {
|
---|
309 | found = true;
|
---|
310 | break;
|
---|
311 | }
|
---|
312 | }
|
---|
313 | if (found) {
|
---|
314 | delete parReq;
|
---|
315 | }
|
---|
316 | else {
|
---|
317 | _params.push_back(parReq);
|
---|
318 | }
|
---|
319 | }
|
---|
320 |
|
---|
321 | // Set Parameter Indices
|
---|
322 | // ---------------------
|
---|
323 | sort(_params.begin(), _params.end(), t_param::sortFunction);
|
---|
324 |
|
---|
325 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
326 | t_param* par = _params[ii];
|
---|
327 | par->setIndex(ii);
|
---|
328 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
329 | const t_satObs* satObs = obsVector[jj];
|
---|
330 | if (satObs->prn() == par->prn()) {
|
---|
331 | par->setAmbEleSat(satObs->eleSat());
|
---|
332 | par->stepAmbNumEpo();
|
---|
333 | }
|
---|
334 | }
|
---|
335 | }
|
---|
336 |
|
---|
337 | return success;
|
---|
338 | }
|
---|
339 |
|
---|
340 | //
|
---|
341 | ////////////////////////////////////////////////////////////////////////////
|
---|
342 | void t_parlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
|
---|
343 | const ColumnVector& xx, double ambFixRate) const {
|
---|
344 |
|
---|
345 | string epoTimeStr = string(epoTime);
|
---|
346 |
|
---|
347 | LOG << endl;
|
---|
348 |
|
---|
349 | t_param* parX = 0;
|
---|
350 | t_param* parY = 0;
|
---|
351 | t_param* parZ = 0;
|
---|
352 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
353 | t_param* par = _params[ii];
|
---|
354 | if (par->type() == t_param::crdX) {
|
---|
355 | parX = par;
|
---|
356 | }
|
---|
357 | else if (par->type() == t_param::crdY) {
|
---|
358 | parY = par;
|
---|
359 | }
|
---|
360 | else if (par->type() == t_param::crdZ) {
|
---|
361 | parZ = par;
|
---|
362 | }
|
---|
363 | else {
|
---|
364 | int ind = par->indexNew();
|
---|
365 | LOG << epoTimeStr << ' ' << par->toString() << ' '
|
---|
366 | << setw(10) << setprecision(4) << par->x0() << ' '
|
---|
367 | << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
|
---|
368 | << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
|
---|
369 | if (par->type() == t_param::amb) {
|
---|
370 | LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
|
---|
371 | << " epo = " << setw(4) << par->ambNumEpo();
|
---|
372 | }
|
---|
373 | LOG << endl;
|
---|
374 | }
|
---|
375 | }
|
---|
376 |
|
---|
377 | if (parX && parY && parZ) {
|
---|
378 | const t_station* sta = PPP_CLIENT->staRover();
|
---|
379 |
|
---|
380 | ColumnVector xyz(3);
|
---|
381 | xyz[0] = xx[parX->indexNew()];
|
---|
382 | xyz[1] = xx[parY->indexNew()];
|
---|
383 | xyz[2] = xx[parZ->indexNew()];
|
---|
384 |
|
---|
385 | ColumnVector neu(3);
|
---|
386 | xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
|
---|
387 |
|
---|
388 | SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
|
---|
389 |
|
---|
390 | SymmetricMatrix QQneu(3);
|
---|
391 | covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
|
---|
392 |
|
---|
393 | LOG << epoTimeStr
|
---|
394 | << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
|
---|
395 | << setprecision(4) << sqrt(QQxyz[0][0])
|
---|
396 |
|
---|
397 | << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
|
---|
398 | << setprecision(4) << sqrt(QQxyz[1][1])
|
---|
399 |
|
---|
400 | << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
|
---|
401 | << setprecision(4) << sqrt(QQxyz[2][2])
|
---|
402 |
|
---|
403 | << " dN = " << setprecision(4) << neu[0] << " +- "
|
---|
404 | << setprecision(4) << sqrt(QQneu[0][0])
|
---|
405 |
|
---|
406 | << " dE = " << setprecision(4) << neu[1] << " +- "
|
---|
407 | << setprecision(4) << sqrt(QQneu[1][1])
|
---|
408 |
|
---|
409 | << " dU = " << setprecision(4) << neu[2] << " +- "
|
---|
410 | << setprecision(4) << sqrt(QQneu[2][2]);
|
---|
411 | if (ambFixRate > 0.0) {
|
---|
412 | LOG << " fix ";
|
---|
413 | }
|
---|
414 | else {
|
---|
415 | LOG << " flt ";
|
---|
416 | }
|
---|
417 | LOG << int(100*ambFixRate) << " %\n";
|
---|
418 | }
|
---|
419 | }
|
---|
420 |
|
---|