source: ntrip/trunk/BNC/src/PPP/pppParlist.cpp@ 11208

Last change on this file since 11208 was 11183, checked in by stuerze, 11 days ago

minor changes

  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 14.5 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: t_pppParlist
6 *
7 * Purpose: List of estimated parameters
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Jul-2014
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <cmath>
18#include <iostream>
19#include <sstream>
20#include <iomanip>
21#include <algorithm>
22#include <newmatio.h>
23
24#include "pppParlist.h"
25#include "pppSatObs.h"
26#include "pppStation.h"
27#include "bncutils.h"
28#include "bncconst.h"
29#include "pppClient.h"
30
31using namespace BNC_PPP;
32using namespace std;
33
34// Constructor
35////////////////////////////////////////////////////////////////////////////
36t_pppParam::t_pppParam(e_type type, const t_prn& prn, t_lc LC,
37 const vector<t_pppSatObs*>* obsVector) {
38
39 _type = type;
40 _prn = prn;
41 _sys = '?';
42 _LC = LC;
43 _x0 = 0.0;
44 _indexOld = -1;
45 _indexNew = -1;
46 _noise = 0.0;
47 _ambInfo = new t_ambInfo();
48
49 switch (_type) {
50 case crdX:
51 _epoSpec = false;
52 _sigma0 = OPT->_aprSigCrd[0];
53 _noise = OPT->_noiseCrd[0];
54 break;
55 case crdY:
56 _epoSpec = false;
57 _sigma0 = OPT->_aprSigCrd[1];
58 _noise = OPT->_noiseCrd[1];
59 break;
60 case crdZ:
61 _epoSpec = false;
62 _sigma0 = OPT->_aprSigCrd[2];
63 _noise = OPT->_noiseCrd[2];
64 break;
65 case rClk:
66 _epoSpec = true;
67 _sigma0 = OPT->_aprSigClk;
68 break;
69 case amb:
70 _epoSpec = false;
71 _sigma0 = OPT->_aprSigAmb;
72 if (obsVector) {
73 for (unsigned ii = 0; ii < obsVector->size(); ii++) {
74 const t_pppSatObs* obs = obsVector->at(ii);
75 if (obs->prn() == _prn) {
76 _x0 = floor((obs->obsValue(LC) - obs->cmpValue(LC)) / obs->lambda(LC) + 0.5);
77 break;
78 }
79 }
80 }
81 break;
82 case trp:
83 _epoSpec = false;
84 _sigma0 = OPT->_aprSigTrp;
85 _noise = OPT->_noiseTrp;
86 break;
87 case ion:
88 _epoSpec = true;
89 _sigma0 = OPT->_aprSigIon;
90 break;
91 case bias:
92 _epoSpec = true;
93 _sigma0 = OPT->_aprSigCodeBias;
94 break;
95 }
96}
97
98// Destructor
99////////////////////////////////////////////////////////////////////////////
100t_pppParam::~t_pppParam() {
101 delete _ambInfo;
102}
103//
104////////////////////////////////////////////////////////////////////////////
105double t_pppParam::partial(const bncTime& /* epoTime */, const t_pppSatObs* obs,
106 const t_lc& obsLC) const {
107
108 // Special Case - Melbourne-Wuebbena
109 // ---------------------------------
110 if (obsLC._type == t_lc::MW && _type != amb) {
111 return 0.0;
112 }
113
114 // Special Case - GIM (single-differenced: A[ION_ref]=+1, A[ION_i]=-1)
115 // -------------------------------------------------------------------
116 if (obsLC._type == t_lc::GIM) {
117 if (_type == ion) {
118 if (obs->prn() == _prn) {
119 return -1.0;
120 }
121 if (_prn == _refPrn && _prn.system() == obs->prn().system()) {
122 return 1.0;
123 }
124 }
125 return 0.0;
126 }
127
128 const t_pppStation* sta = PPP_CLIENT->staRover();
129 ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
130
131 map<t_frequency::type, double> codeCoeff;
132 map<t_frequency::type, double> phaseCoeff;
133 map<t_frequency::type, double> ionoCoeff;
134 obs->lcCoeff(obsLC, codeCoeff, phaseCoeff, ionoCoeff);
135
136 switch (_type) {
137 case crdX:
138 return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.NormFrobenius();
139 case crdY:
140 return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.NormFrobenius();
141 case crdZ:
142 return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.NormFrobenius();
143 case rClk:
144 return (_sys != obsLC.system() || obsLC.isGeometryFree()) ? 0.0 : 1.0;
145 case amb:
146 if (obs->prn() == _prn) {
147 if (obsLC == _LC) {
148 return (obs->lambda(obsLC));
149 }
150 else if (_LC._type == t_lc::phase) {
151 return obs->lambda(_LC) * phaseCoeff[_LC._frq1];
152 }
153 else if (obsLC._type == t_lc::phaseIF && _LC._type == t_lc::MW) {
154 return obs->lambda(obsLC) * obs->lambda(_LC) / obs->lambda(t_lc(t_lc::phase, _LC._frq2));
155 }
156 }
157 break;
158 case trp:
159 return 1.0 / sin(obs->eleSat());
160 case ion:
161 if (obs->prn() == _prn) {
162 if (obsLC._type == t_lc::code || obsLC._type == t_lc::phase) {
163 return ionoCoeff[obsLC._frq1];
164 }
165 }
166 break;
167 case bias:
168 return (_LC == obsLC ? 1.0 : 0.0);
169 }
170 return 0.0;
171}
172
173//
174////////////////////////////////////////////////////////////////////////////
175string t_pppParam::toString() const {
176 stringstream ss;
177 switch (_type) {
178 case crdX:
179 ss << "CRD_X";
180 break;
181 case crdY:
182 ss << "CRD_Y";
183 break;
184 case crdZ:
185 ss << "CRD_Z";
186 break;
187 case rClk:
188 ss << "REC_CLK " << _sys << " ";
189 break;
190 case trp:
191 ss << "TRP ";
192 break;
193 case amb:
194 ss << "AMB " << left << setw(3) << _LC.toString() << right << ' ' << _prn.toString();
195 break;
196 case ion:
197 ss << "ION " << left << setw(3) << _LC.toString() << right << ' ' << _prn.toString();
198 break;
199 case bias:
200 char sys = t_frequency::toSystem(_LC._frq1);
201 ss << "BIA " << left << setw(3) << _LC.toString() << right << ' ' << sys << " ";
202 break;
203 }
204 return ss.str();
205}
206
207// Constructor
208////////////////////////////////////////////////////////////////////////////
209t_pppParlist::t_pppParlist() {
210}
211
212// Destructor
213////////////////////////////////////////////////////////////////////////////
214t_pppParlist::~t_pppParlist() {
215
216 for (unsigned ii = 0; ii < _params.size(); ii++) {
217 delete _params[ii];
218 }
219}
220
221//
222////////////////////////////////////////////////////////////////////////////
223t_irc t_pppParlist::set(const bncTime& epoTime, const std::vector<t_pppSatObs*>& obsVector) {
224
225 // Remove some Parameters
226 // ----------------------
227 vector<t_pppParam*>::iterator it = _params.begin();
228 while (it != _params.end()) {
229 t_pppParam* par = *it;
230
231 bool remove = false;
232
233 if (par->epoSpec()) {
234 remove = true;
235 }
236
237 else if (par->type() == t_pppParam::amb ||
238 par->type() == t_pppParam::crdX ||
239 par->type() == t_pppParam::crdY ||
240 par->type() == t_pppParam::crdZ ||
241 par->type() == t_pppParam::ion ) {
242 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 60.0)) {
243 remove = true;
244 }
245 }
246
247 if (remove) {
248 delete par;
249 it = _params.erase(it);
250 }
251 else {
252 ++it;
253 }
254 }
255
256 // Check which systems have observations
257 // -------------------------------------
258 vector<char> systems = OPT->systems();
259 for (char sys : systems) {
260 _usedSystems[sys] = 0;
261 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
262 const t_pppSatObs* satObs = obsVector[jj];
263 if (satObs->prn().system() == sys) {
264 _usedSystems[sys]++;
265 }
266 }
267 };
268
269 // Check whether parameters have observations
270 // ------------------------------------------
271 for (unsigned ii = 0; ii < _params.size(); ii++) {
272 t_pppParam* par = _params[ii];
273 if (par->prn() == 0) {
274 par->setLastObsTime(epoTime);
275 if (par->firstObsTime().undef()) {
276 par->setFirstObsTime(epoTime);
277 }
278 }
279 else {
280 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
281 const t_pppSatObs* satObs = obsVector[jj];
282 if (satObs->prn() == par->prn()) {
283 par->setLastObsTime(epoTime);
284 if (par->firstObsTime().undef()) {
285 par->setFirstObsTime(epoTime);
286 }
287 break;
288 }
289 }
290 }
291 }
292
293 // Required Set of Parameters
294 // --------------------------
295 vector<t_pppParam*> required;
296
297 // Coordinates
298 // -----------
299 required.push_back(new t_pppParam(t_pppParam::crdX, t_prn()));
300 required.push_back(new t_pppParam(t_pppParam::crdY, t_prn()));
301 required.push_back(new t_pppParam(t_pppParam::crdZ, t_prn()));
302
303 // Receiver Clocks
304 // ---------------
305 for (const auto& [sys, numObs] : _usedSystems) {
306 if (numObs > 0) {
307 t_pppParam* clk = new t_pppParam(t_pppParam::rClk, t_prn());
308 clk->setSystem(sys);
309 required.push_back(clk);
310 }
311 }
312
313 // Troposphere
314 // -----------
315 if (OPT->estTrp()) {
316 required.push_back(new t_pppParam(t_pppParam::trp, t_prn()));
317 }
318
319 // Ionosphere
320 // ----------
321 if (OPT->_ionoModelType == OPT->est) {
322 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
323 const t_pppSatObs* satObs = obsVector[jj];
324 std::vector<t_lc> LCs = OPT->LCs(satObs->prn().system());
325 for (auto it = LCs.begin(); it != LCs.end(); ++it) {
326 const t_lc& lc = *it;
327 if (!lc.isIonoFree()) {
328 required.push_back(new t_pppParam(t_pppParam::ion, satObs->prn()));
329 break;
330 }
331 }
332 }
333 }
334
335 // Reference satellite PRN per system (from satellite marked by preparePseudoObs)
336 // -------------------------------------------------------------------------------
337 if (OPT->_pseudoObsIono) {
338 map<char, t_prn> refPrnMap;
339 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
340 if (obsVector[jj]->isReference()) {
341 refPrnMap[obsVector[jj]->prn().system()] = obsVector[jj]->prn();
342 }
343 }
344 for (unsigned ii = 0; ii < required.size(); ii++) {
345 if (required[ii]->type() == t_pppParam::ion) {
346 char sys = required[ii]->prn().system();
347 if (refPrnMap.count(sys)) {
348 required[ii]->setRefPrn(refPrnMap[sys]);
349 }
350 }
351 }
352 }
353
354 // Ambiguities
355 // -----------
356 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
357 const t_pppSatObs* satObs = obsVector[jj];
358 char sys = satObs->prn().system();
359 const vector<t_lc>& ambLCs = OPT->ambLCs(sys);
360 for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
361 if (ambLCs[ii]._frq1 == t_frequency::G5 && !satObs->isValid(ambLCs[ii])) {
362 continue;
363 }
364 required.push_back(new t_pppParam(t_pppParam::amb, satObs->prn(), ambLCs[ii], &obsVector));
365 }
366 }
367
368 // Biases
369 // ------
370 int maxSkip = (OPT->_pseudoObsIono ? 1 : 2);
371 for (const auto& [sys, numObs] : _usedSystems) {
372 if (numObs > 0) {
373 bool ar = OPT->arSystem(sys);
374 int skip = 0;
375 vector<t_lc> LCs = OPT->LCs(sys);
376 for (const t_lc& lc : LCs) {
377 if (ar) {
378 if (skip < maxSkip && lc.includesPhase()) {
379 skip += 1;
380 }
381 else {
382 required.push_back(new t_pppParam(t_pppParam::bias, t_prn(), lc));
383 }
384 }
385 else {
386 if (lc.includesCode()) {
387 if (skip < maxSkip) {
388 skip += 1;
389 }
390 else {
391 required.push_back(new t_pppParam(t_pppParam::bias, t_prn(), lc));
392 }
393 }
394 }
395 }
396 }
397 }
398
399 // Check if all required parameters are present
400 // --------------------------------------------
401 for (unsigned ii = 0; ii < required.size(); ii++) {
402 t_pppParam* parReq = required[ii];
403
404 bool found = false;
405 for (unsigned jj = 0; jj < _params.size(); jj++) {
406 t_pppParam* parOld = _params[jj];
407 if (parOld->isEqual(parReq)) {
408 found = true;
409 break;
410 }
411 }
412 if (found) {
413 delete parReq;
414 }
415 else {
416 _params.push_back(parReq);
417 }
418 }
419
420 // Set Parameter Indices
421 // ---------------------
422 sort(_params.begin(), _params.end(), t_pppParam::sortFunction);
423
424 for (unsigned ii = 0; ii < _params.size(); ii++) {
425 t_pppParam* par = _params[ii];
426 par->setIndex(ii);
427 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
428 const t_pppSatObs* satObs = obsVector[jj];
429 if (satObs->prn() == par->prn()) {
430 par->setAmbEleSat(satObs->eleSat());
431 par->stepAmbNumEpo();
432 }
433 }
434 }
435
436 return success;
437}
438
439//
440////////////////////////////////////////////////////////////////////////////
441void t_pppParlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
442 const ColumnVector& xx, double fixRatio) const {
443
444 string epoTimeStr = string(epoTime);
445 const t_pppStation* sta = PPP_CLIENT->staRover();
446
447 LOG << endl;
448
449 t_pppParam* parX = 0;
450 t_pppParam* parY = 0;
451 t_pppParam* parZ = 0;
452 for (unsigned ii = 0; ii < _params.size(); ii++) {
453 t_pppParam* par = _params[ii];
454 if (par->type() == t_pppParam::crdX) {
455 parX = par;
456 }
457 else if (par->type() == t_pppParam::crdY) {
458 parY = par;
459 }
460 else if (par->type() == t_pppParam::crdZ) {
461 parZ = par;
462 }
463 else {
464 int ind = par->indexNew();
465 double apr = (par->type() == t_pppParam::trp) ?
466 t_tropo::delay_saast(sta->xyzApr(), M_PI/2.0) : par->x0();
467 LOG << epoTimeStr << ' ' << par->toString() << ' '
468 << setw(10) << setprecision(4) << apr << ' '
469 << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
470 << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
471 if (par->type() == t_pppParam::amb) {
472 LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
473 << " epo = " << setw(4) << par->ambNumEpo();
474 }
475 LOG << endl;
476 }
477 }
478
479 if (parX && parY && parZ) {
480
481 ColumnVector xyz(3);
482 xyz[0] = xx[parX->indexNew()];
483 xyz[1] = xx[parY->indexNew()];
484 xyz[2] = xx[parZ->indexNew()];
485
486 ColumnVector neu(3);
487 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
488
489 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
490
491 SymmetricMatrix QQneu(3);
492 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
493
494 LOG << epoTimeStr << ' ' << sta->name()
495 << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
496 << setprecision(4) << sqrt(QQxyz[0][0])
497
498 << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
499 << setprecision(4) << sqrt(QQxyz[1][1])
500
501 << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
502 << setprecision(4) << sqrt(QQxyz[2][2])
503
504 << " dN = " << setprecision(4) << neu[0] << " +- "
505 << setprecision(4) << sqrt(QQneu[0][0])
506
507 << " dE = " << setprecision(4) << neu[1] << " +- "
508 << setprecision(4) << sqrt(QQneu[1][1])
509
510 << " dU = " << setprecision(4) << neu[2] << " +- "
511 << setprecision(4) << sqrt(QQneu[2][2]);
512
513 if (fixRatio > 0.0) {
514 LOG << " fix " << int(100*fixRatio) << " %";
515 }
516 else {
517 LOG << " flt ";
518 }
519
520 LOG << endl;
521 }
522 return;
523}
524
525//
526////////////////////////////////////////////////////////////////////////////
527void t_pppParlist::printParams(const bncTime& epoTime) {
528
529 for (unsigned iPar = 0; iPar < _params.size(); iPar++) {
530 LOG << _params[iPar]->toString()
531 << "\t lastObsTime().valid() \t" << _params[iPar]->lastObsTime().valid()
532 << "\t epoTime - par->lastObsTime() \t" << (epoTime - _params[iPar]->lastObsTime())
533 << endl;
534 }
535}
536
Note: See TracBrowser for help on using the repository browser.