source: ntrip/trunk/BNC/src/PPP/pppClient.cpp@ 9432

Last change on this file since 9432 was 9432, checked in by stuerze, 3 years ago

update regarding PPP
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 27.2 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: t_pppClient
6 *
7 * Purpose: PPP Client processing starts here
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Jul-2014
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <QThreadStorage>
18
19#include <iostream>
20#include <iomanip>
21#include <cmath>
22#include <stdlib.h>
23#include <string.h>
24#include <stdexcept>
25
26#include "pppClient.h"
27#include "pppEphPool.h"
28#include "pppObsPool.h"
29#include "bncconst.h"
30#include "bncutils.h"
31#include "pppStation.h"
32#include "bncantex.h"
33#include "pppFilter.h"
34
35using namespace BNC_PPP;
36using namespace std;
37
38// Global variable holding thread-specific pointers
39//////////////////////////////////////////////////////////////////////////////
40QThreadStorage<t_pppClient*> CLIENTS;
41
42// Static function returning thread-specific pointer
43//////////////////////////////////////////////////////////////////////////////
44t_pppClient* t_pppClient::instance() {
45 return CLIENTS.localData();
46}
47
48// Constructor
49//////////////////////////////////////////////////////////////////////////////
50t_pppClient::t_pppClient(const t_pppOptions* opt) {
51 _output = 0;
52 _opt = new t_pppOptions(*opt);
53 _log = new ostringstream();
54 _ephPool = new t_pppEphPool();
55 _obsPool = new t_pppObsPool();
56 _staRover = new t_pppStation();
57 _filter = new t_pppFilter(_obsPool);
58 _tides = new t_tides();
59 _antex = 0;
60 if (!_opt->_antexFileName.empty()) {
61 _antex = new bncAntex(_opt->_antexFileName.c_str());
62 }
63 if (!_opt->_blqFileName.empty()) {
64 if (_tides->readBlqFile(_opt->_blqFileName.c_str()) == success) {
65 //_tides->printAllBlqSets();
66 }
67 }
68
69 if (_opt->_refSatRequired) {
70 for (unsigned iSys = 0; iSys < _opt->systems().size(); iSys++) {
71 char system = _opt->systems()[iSys];
72 _obsPool->initRefSatMapElement(system);
73 }
74 }
75
76 _offGR = 0.0;
77 _offGE = 0.0;
78 _offGC = 0.0;
79 CLIENTS.setLocalData(this); // CLIENTS takes ownership over "this"
80}
81
82// Destructor
83//////////////////////////////////////////////////////////////////////////////
84t_pppClient::~t_pppClient() {
85 delete _log;
86 delete _opt;
87 delete _filter;
88 delete _ephPool;
89 delete _obsPool;
90 delete _staRover;
91 if (_antex) {
92 delete _antex;
93 }
94 delete _tides;
95 clearObs();
96}
97
98//
99//////////////////////////////////////////////////////////////////////////////
100void t_pppClient::putEphemeris(const t_eph* eph) {
101 const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
102 const t_ephGlo* ephGlo = dynamic_cast<const t_ephGlo*>(eph);
103 const t_ephGal* ephGal = dynamic_cast<const t_ephGal*>(eph);
104 const t_ephBDS* ephBDS = dynamic_cast<const t_ephBDS*>(eph);
105 if (ephGPS) {
106 _ephPool->putEphemeris(new t_ephGPS(*ephGPS));
107 }
108 else if (ephGlo) {
109 _ephPool->putEphemeris(new t_ephGlo(*ephGlo));
110 }
111 else if (ephGal) {
112 _ephPool->putEphemeris(new t_ephGal(*ephGal));
113 }
114 else if (ephBDS) {
115 _ephPool->putEphemeris(new t_ephBDS(*ephBDS));
116 }
117}
118
119//
120//////////////////////////////////////////////////////////////////////////////
121void t_pppClient::putTec(const t_vTec* vTec) {
122 _obsPool->putTec(new t_vTec(*vTec));
123}
124
125//
126//////////////////////////////////////////////////////////////////////////////
127void t_pppClient::putOrbCorrections(const vector<t_orbCorr*>& corr) {
128 for (unsigned ii = 0; ii < corr.size(); ii++) {
129 _ephPool->putOrbCorrection(new t_orbCorr(*corr[ii]));
130 }
131}
132
133//
134//////////////////////////////////////////////////////////////////////////////
135void t_pppClient::putClkCorrections(const vector<t_clkCorr*>& corr) {
136 for (unsigned ii = 0; ii < corr.size(); ii++) {
137 _ephPool->putClkCorrection(new t_clkCorr(*corr[ii]));
138 }
139}
140
141//
142//////////////////////////////////////////////////////////////////////////////
143void t_pppClient::putCodeBiases(const vector<t_satCodeBias*>& biases) {
144 for (unsigned ii = 0; ii < biases.size(); ii++) {
145 _obsPool->putCodeBias(new t_satCodeBias(*biases[ii]));
146 }
147}
148
149//
150//////////////////////////////////////////////////////////////////////////////
151void t_pppClient::putPhaseBiases(const vector<t_satPhaseBias*>& biases) {
152 for (unsigned ii = 0; ii < biases.size(); ii++) {
153 _obsPool->putPhaseBias(new t_satPhaseBias(*biases[ii]));
154 }
155}
156
157//
158//////////////////////////////////////////////////////////////////////////////
159t_irc t_pppClient::prepareObs(const vector<t_satObs*>& satObs,
160 vector<t_pppSatObs*>& obsVector, bncTime& epoTime) {
161
162 // Default
163 // -------
164 epoTime.reset();
165 clearObs();
166
167 // Create vector of valid observations
168 // -----------------------------------
169 for (unsigned ii = 0; ii < satObs.size(); ii++) {
170 char system = satObs[ii]->_prn.system();
171 if (_opt->useSystem(system)) {
172 t_pppSatObs* pppSatObs = new t_pppSatObs(*satObs[ii]);
173 if (pppSatObs->isValid()) {
174 obsVector.push_back(pppSatObs);
175 }
176 else {
177 delete pppSatObs;
178 }
179 }
180 }
181
182 // Check whether data are synchronized, compute epoTime
183 // ----------------------------------------------------
184 const double MAXSYNC = 0.05; // synchronization limit
185 double meanDt = 0.0;
186 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
187 const t_pppSatObs* satObs = obsVector.at(ii);
188 if (epoTime.undef()) {
189 epoTime = satObs->time();
190 }
191 else {
192 double dt = satObs->time() - epoTime;
193 if (fabs(dt) > MAXSYNC) {
194 LOG << "t_pppClient::prepareObs asynchronous observations" << endl;
195 return failure;
196 }
197 meanDt += dt;
198 }
199 }
200
201 if (obsVector.size() > 0) {
202 epoTime += meanDt / obsVector.size();
203 }
204
205 return success;
206}
207
208//
209//////////////////////////////////////////////////////////////////////////////
210bool t_pppClient::preparePseudoObs(std::vector<t_pppSatObs*>& obsVector) {
211
212 bool pseudoObsIono = false;
213
214 if (_opt->_pseudoObsIono) {
215 vector<t_pppSatObs*>::iterator it = obsVector.begin();
216 while (it != obsVector.end()) {
217 t_pppSatObs* satObs = *it;
218 char system = satObs->prn().system();
219 t_pppRefSat* refSat = _obsPool->getRefSatMapElement(system);
220 double stecRef = refSat->stecValue();
221 if (stecRef && !satObs->isReference()) {
222 pseudoObsIono = true;
223 satObs->setPseudoObsIono(t_frequency::G1, stecRef);
224 }
225 it++;
226 }
227 }
228 if (_opt->_pseudoObsTropo) {
229 vector<t_pppSatObs*>::iterator it = obsVector.begin();
230 while (it != obsVector.end()) {
231 t_pppSatObs* satObs = *it;
232 if (satObs->isReference()) {
233 satObs->setPseudoObsTropo();
234 }
235 it++;
236 }
237 }
238/*
239 vector<t_pppSatObs*>::iterator it = obsVector.begin();
240 while (it != obsVector.end()) {
241 t_pppSatObs* satObs = *it;
242 satObs->printObsMinusComputed();
243 it++;
244 }
245*/
246 return pseudoObsIono;
247}
248
249// Compute the Bancroft position, check for blunders
250//////////////////////////////////////////////////////////////////////////////
251t_irc t_pppClient::cmpBancroft(const bncTime& epoTime,
252 vector<t_pppSatObs*>& obsVector,
253 ColumnVector& xyzc, bool print) {
254
255 t_lc::type tLC = t_lc::dummy;
256
257 while (true) {
258 Matrix BB(obsVector.size(), 4);
259 int iObs = -1;
260 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
261 const t_pppSatObs* satObs = obsVector.at(ii);
262 if (tLC == t_lc::dummy) {
263 if (satObs->isValid(t_lc::cIF)) {
264 tLC = t_lc::cIF;
265 }
266 else if (satObs->isValid(t_lc::c1)) {
267 tLC = t_lc::c1;
268 }
269 else if (satObs->isValid(t_lc::c2)) {
270 tLC = t_lc::c2;
271 }
272 }
273 if ( satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle) ) {
274 ++iObs;
275 BB[iObs][0] = satObs->xc()[0];
276 BB[iObs][1] = satObs->xc()[1];
277 BB[iObs][2] = satObs->xc()[2];
278 BB[iObs][3] = satObs->obsValue(tLC) - satObs->cmpValueForBanc(tLC);
279 }
280 }
281 if (iObs + 1 < _opt->_minObs) {
282 LOG << "t_pppClient::cmpBancroft not enough observations" << endl;
283 return failure;
284 }
285 BB = BB.Rows(1,iObs+1);
286 bancroft(BB, xyzc);
287
288 xyzc[3] /= t_CST::c;
289
290 // Check Blunders
291 // --------------
292 const double BLUNDER = 100.0;
293 double maxRes = 0.0;
294 unsigned maxResIndex = 0;
295 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
296 const t_pppSatObs* satObs = obsVector.at(ii);
297 if (satObs->isValid() &&
298 satObs->prn().system() == 'G' &&
299 (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle) ) {
300 ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3);
301 double res = rr.NormFrobenius() - satObs->obsValue(tLC)
302 - (satObs->xc()[3] - xyzc[3]) * t_CST::c;
303 if (std::isnan(res) || fabs(res) > maxRes) {
304 std::isnan(res) ?
305 maxRes = res :
306 maxRes = fabs(res);
307 maxResIndex = ii;
308 }
309 }
310 }
311 if (!std::isnan(maxRes) && maxRes < BLUNDER) {
312 if (print && _numEpoProcessing == 1) {
313 LOG.setf(ios::fixed);
314 LOG << string(epoTime) << " BANCROFT:" << ' '
315 << setw(14) << setprecision(3) << xyzc[0] << ' '
316 << setw(14) << setprecision(3) << xyzc[1] << ' '
317 << setw(14) << setprecision(3) << xyzc[2] << ' '
318 << setw(14) << setprecision(3) << xyzc[3] * t_CST::c << endl << endl;
319 }
320 break;
321 }
322 else {
323 t_pppSatObs* satObs = obsVector.at(maxResIndex);
324 LOG << "t_pppClient::cmpBancroft outlier " << satObs->prn().toString()
325 << " " << maxRes << endl;
326 delete satObs;
327 obsVector.erase(obsVector.begin() + maxResIndex);
328 }
329 }
330
331 return success;
332}
333
334// Compute A Priori GPS-Glonass Offset
335//////////////////////////////////////////////////////////////////////////////
336double t_pppClient::cmpOffGR(vector<t_pppSatObs*>& obsVector) {
337
338 t_lc::type tLC = t_lc::dummy;
339 double offGR = 0.0;
340
341 if (_opt->useSystem('R')) {
342 while (obsVector.size() > 0) {
343 offGR = 0.0;
344 double maxRes = 0.0;
345 int maxResIndex = -1;
346 t_prn maxResPrn;
347 unsigned nObs = 0;
348 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
349 const t_pppSatObs* satObs = obsVector.at(ii);
350 if (satObs->prn().system() == 'R') {
351 if (tLC == t_lc::dummy) {
352 tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
353 }
354 if (satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle)) {
355 double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
356 ++nObs;
357 offGR += ll;
358 if (fabs(ll) > fabs(maxRes)) {
359 maxRes = ll;
360 maxResIndex = ii;
361 maxResPrn = satObs->prn();
362 }
363 }
364 }
365 }
366
367 if (nObs > 0) {
368 offGR = offGR / nObs;
369 }
370 else {
371 offGR = 0.0;
372 }
373 if (fabs(maxRes) > OPT->_aprSigOGC) {
374 LOG << "t_pppClient::cmpOffGR outlier " << maxResPrn.toString() << " " << maxRes << endl;
375 delete obsVector.at(maxResIndex);
376 obsVector.erase(obsVector.begin() + maxResIndex);
377 }
378 else {
379 break;
380 }
381 }
382 }
383 return offGR;
384}
385
386// Compute A Priori GPS-Galileo Offset
387//////////////////////////////////////////////////////////////////////////////
388double t_pppClient::cmpOffGE(vector<t_pppSatObs*>& obsVector) {
389
390 t_lc::type tLC = t_lc::dummy;
391 double offGE = 0.0;
392
393 if (_opt->useSystem('E')) {
394 while (obsVector.size() > 0) {
395 offGE = 0.0;
396 double maxRes = 0.0;
397 int maxResIndex = -1;
398 t_prn maxResPrn;
399 unsigned nObs = 0;
400 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
401 const t_pppSatObs* satObs = obsVector.at(ii);
402 if (satObs->prn().system() == 'E') {
403 if (tLC == t_lc::dummy) {
404 tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
405 }
406 if (satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle)) {
407 double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
408 ++nObs;
409 offGE += ll;
410 if (fabs(ll) > fabs(maxRes)) {
411 maxRes = ll;
412 maxResIndex = ii;
413 maxResPrn = satObs->prn();
414 }
415 }
416 }
417 }
418
419 if (nObs > 0) {
420 offGE = offGE / nObs;
421 }
422 else {
423 offGE = 0.0;
424 }
425
426 if (fabs(maxRes) > OPT->_aprSigOGE) {
427 LOG << "t_pppClient::cmpOffGE outlier " << maxResPrn.toString() << " " << maxRes << endl;
428 delete obsVector.at(maxResIndex);
429 obsVector.erase(obsVector.begin() + maxResIndex);
430 }
431 else {
432 break;
433 }
434 }
435 }
436 return offGE;
437}
438
439// Compute A Priori GPS-BDS Offset
440//////////////////////////////////////////////////////////////////////////////
441double t_pppClient::cmpOffGC(vector<t_pppSatObs*>& obsVector) {
442
443 t_lc::type tLC = t_lc::dummy;
444 double offGC = 0.0;
445
446 if (_opt->useSystem('C')) {
447 while (obsVector.size() > 0) {
448 offGC = 0.0;
449 double maxRes = 0.0;
450 int maxResIndex = -1;
451 t_prn maxResPrn;
452 unsigned nObs = 0;
453 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
454 const t_pppSatObs* satObs = obsVector.at(ii);
455 if (satObs->prn().system() == 'C') {
456 if (tLC == t_lc::dummy) {
457 tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
458 }
459 if (satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle)) {
460 double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
461 ++nObs;
462 offGC += ll;
463 if (fabs(ll) > fabs(maxRes)) {
464 maxRes = ll;
465 maxResIndex = ii;
466 maxResPrn = satObs->prn();
467 }
468 }
469 }
470 }
471
472 if (nObs > 0) {
473 offGC = offGC / nObs;
474 }
475 else {
476 offGC = 0.0;
477 }
478
479 if (fabs(maxRes) > OPT->_aprSigOGC) {
480 LOG << "t_pppClient::cmpOffGC outlier " << maxResPrn.toString() << " " << maxRes << endl;
481 delete obsVector.at(maxResIndex);
482 obsVector.erase(obsVector.begin() + maxResIndex);
483 }
484 else {
485 break;
486 }
487 }
488 }
489 return offGC;
490}
491
492//
493//////////////////////////////////////////////////////////////////////////////
494void t_pppClient::initOutput(t_output* output) {
495 _output = output;
496 _output->_numSat = 0;
497 _output->_hDop = 0.0;
498 _output->_error = false;
499}
500
501//
502//////////////////////////////////////////////////////////////////////////////
503void t_pppClient::clearObs() {
504 for (unsigned ii = 0; ii < _obsRover.size(); ii++) {
505 delete _obsRover.at(ii);
506 }
507 _obsRover.clear();
508}
509
510//
511//////////////////////////////////////////////////////////////////////////////
512void t_pppClient::finish(t_irc irc) {
513
514 clearObs();
515
516 _output->_epoTime = _epoTimeRover;
517
518 if (irc == success) {
519 _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0];
520 _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1];
521 _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2];
522
523 xyz2neu(_staRover->ellApr().data(), _filter->x().data(), _output->_neu);
524
525 copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix);
526
527 _output->_trp0 = t_tropo::delay_saast(_staRover->xyzApr(), M_PI/2.0);
528 _output->_trp = _filter->trp();
529 _output->_trpStdev = _filter->trpStdev();
530
531 _output->_numSat = _filter->numSat();
532 _output->_hDop = _filter->HDOP();
533 _output->_error = false;
534 }
535 else {
536 _output->_error = true;
537 }
538 _output->_log = _log->str();
539 delete _log; _log = new ostringstream();
540}
541
542//
543//////////////////////////////////////////////////////////////////////////////
544t_irc t_pppClient::cmpModel(t_pppStation* station, const ColumnVector& xyzc,
545 vector<t_pppSatObs*>& obsVector) {
546 bncTime time;
547 time = _epoTimeRover;
548 station->setName(_opt->_roverName);
549 station->setAntName(_opt->_antNameRover);
550 station->setEpochTime(time);
551 if (_opt->xyzAprRoverSet()) {
552 station->setXyzApr(_opt->_xyzAprRover);
553 }
554 else {
555 station->setXyzApr(xyzc.Rows(1,3));
556 }
557 station->setNeuEcc(_opt->_neuEccRover);
558
559 // Receiver Clock
560 // --------------
561 station->setDClk(xyzc[3]);
562
563 // Tides
564 // -----
565 station->setTideDsplEarth(_tides->earth(time, station->xyzApr()));
566 station->setTideDsplOcean(_tides->ocean(time, station->xyzApr(), station->name()));
567
568 // Observation model
569 // -----------------
570 vector<t_pppSatObs*>::iterator it = obsVector.begin();
571 while (it != obsVector.end()) {
572 t_pppSatObs* satObs = *it;
573 t_irc modelSetup;
574 if (satObs->isValid()) {
575 modelSetup = satObs->cmpModel(station);
576 }
577 if (satObs->isValid() &&
578 satObs->eleSat() >= _opt->_minEle &&
579 modelSetup == success) {
580 ++it;
581 }
582 else {
583 delete satObs;
584 it = obsVector.erase(it);
585 }
586 }
587
588 return success;
589}
590
591//
592//////////////////////////////////////////////////////////////////////////////
593void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
594
595 try {
596 initOutput(output);
597 bool epochReProcessing = false;
598 _numEpoProcessing = 0;
599 _historicalRefSats.clear();
600
601 do {
602 _numEpoProcessing++;
603#ifdef BNC_DEBUG_PPP
604 LOG << "_numEpoProcessing " << _numEpoProcessing << endl;
605#endif
606 if (_obsPool->refSatChanged()) {
607 if(_filter->datumTransformation() != success) {
608 LOG << "pppFilter::datumTransformation() != success" << endl;
609 return finish(failure);
610 }
611 else {
612 LOG << "pppFilter::datumTransformation() == success" << endl;
613 if (!_obsPool->refSatChangeRequired()) {
614 _obsPool->saveLastEpoRefSats();
615 }
616 }
617 }
618 // Prepare Observations of the Rover
619 // ---------------------------------
620 if (prepareObs(satObs, _obsRover, _epoTimeRover) != success) {
621 return finish(failure);
622 }
623
624 if (_numEpoProcessing == 1) {
625 LOG << "\nPPP of Epoch ";
626 if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
627 LOG << "\n---------------------------------------------------------------\n";
628 }
629
630 for (int iter = 1; iter <= 2; iter++) {
631 ColumnVector xyzc(4); xyzc = 0.0;
632 bool print = (iter == 2);
633 if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != success) {
634 return finish(failure);
635 }
636 if (cmpModel(_staRover, xyzc, _obsRover) != success) {
637 return finish(failure);
638 }
639 }
640
641 if (_opt->_refSatRequired) {
642 if (handleRefSatellites(_obsRover) != success) {
643 return finish(failure);
644 }
645 if (_obsPool->refSatChanged() &&
646 (_opt->_obsModelType == OPT->DCMcodeBias ||
647 _opt->_obsModelType == OPT->DCMphaseBias ||
648 _opt->_pseudoObsIono)
649 ) {
650 LOG << "refSatChanged()" << endl;
651 epochReProcessing = true;
652 continue;
653 }
654 }
655
656 _offGR = cmpOffGR(_obsRover);
657 _offGE = cmpOffGE(_obsRover);
658 _offGC = cmpOffGC(_obsRover);
659
660 // Prepare Pseudo Observations of the Rover
661 // ----------------------------------------
662 _pseudoObsIono = preparePseudoObs(_obsRover);
663
664 if (int(_obsRover.size()) < _opt->_minObs) {
665 LOG << "t_pppClient::processEpoch not enough observations" << endl;
666 return finish(failure);
667 }
668
669 // Store last epoch of data
670 // ------------------------
671 _obsPool->putEpoch(_epoTimeRover, _obsRover, _pseudoObsIono);
672
673 // Process Epoch in Filter
674 // -----------------------
675 if (_filter->processEpoch() != success) {
676 return finish(failure);
677 }
678
679 // Epoch re-processing required?
680 // -----------------------------
681 if (_obsPool->refSatChangeRequired() && //SLIP
682 _opt->_obsModelType != t_pppOptions::UncombPPP) {
683 epochReProcessing = true;
684 _obsPool->deleteLastEpoch();
685 setHistoricalRefSats();
686 }
687 else {
688 epochReProcessing = false;
689 }
690
691 } while (epochReProcessing);
692
693 }
694 catch (Exception& exc) {
695 LOG << exc.what() << endl;
696 return finish(failure);
697 }
698 catch (t_except msg) {
699 LOG << msg.what() << endl;
700 return finish(failure);
701 }
702 catch (const char* msg) {
703 LOG << msg << endl;
704 return finish(failure);
705 }
706 catch (...) {
707 LOG << "unknown exception" << endl;
708 return finish(failure);
709 }
710
711 return finish(success);
712}
713
714//
715////////////////////////////////////////////////////////////////////////////
716double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
717 return aa[0]*bb[0] + aa[1]*bb[1] + aa[2]*bb[2] - aa[3]*bb[3];
718}
719
720//
721////////////////////////////////////////////////////////////////////////////
722void t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) {
723
724 if (pos.Nrows() != 4) {
725 pos.ReSize(4);
726 }
727 pos = 0.0;
728
729 for (int iter = 1; iter <= 2; iter++) {
730 Matrix BB = BBpass;
731 int mm = BB.Nrows();
732 for (int ii = 1; ii <= mm; ii++) {
733 double xx = BB(ii,1);
734 double yy = BB(ii,2);
735 double traveltime = 0.072;
736 if (iter > 1) {
737 double zz = BB(ii,3);
738 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
739 (yy-pos(2)) * (yy-pos(2)) +
740 (zz-pos(3)) * (zz-pos(3)) );
741 traveltime = rho / t_CST::c;
742 }
743 double angle = traveltime * t_CST::omega;
744 double cosa = cos(angle);
745 double sina = sin(angle);
746 BB(ii,1) = cosa * xx + sina * yy;
747 BB(ii,2) = -sina * xx + cosa * yy;
748 }
749
750 Matrix BBB;
751 if (mm > 4) {
752 SymmetricMatrix hlp; hlp << BB.t() * BB;
753 BBB = hlp.i() * BB.t();
754 }
755 else {
756 BBB = BB.i();
757 }
758 ColumnVector ee(mm); ee = 1.0;
759 ColumnVector alpha(mm); alpha = 0.0;
760 for (int ii = 1; ii <= mm; ii++) {
761 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
762 }
763 ColumnVector BBBe = BBB * ee;
764 ColumnVector BBBalpha = BBB * alpha;
765 double aa = lorentz(BBBe, BBBe);
766 double bb = lorentz(BBBe, BBBalpha)-1;
767 double cc = lorentz(BBBalpha, BBBalpha);
768 double root = sqrt(bb*bb-aa*cc);
769
770 Matrix hlpPos(4,2);
771 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
772 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
773
774 ColumnVector omc(2);
775 for (int pp = 1; pp <= 2; pp++) {
776 hlpPos(4,pp) = -hlpPos(4,pp);
777 omc(pp) = BB(1,4) -
778 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
779 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
780 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
781 hlpPos(4,pp);
782 }
783 if ( fabs(omc(1)) > fabs(omc(2)) ) {
784 pos = hlpPos.Column(2);
785 }
786 else {
787 pos = hlpPos.Column(1);
788 }
789 }
790}
791
792//
793//////////////////////////////////////////////////////////////////////////////
794void t_pppClient::setRefSatellites(std::vector<t_pppSatObs*>& obsVector) {
795 t_irc refSatReDefinition = success;
796 // reference satellite definition per system
797 for (unsigned iSys = 0; iSys < _opt->systems().size(); iSys++) {
798 char sys = _opt->systems()[iSys];
799 bool refSatDefined = false;
800 t_pppRefSat* refSat = _obsPool->getRefSatMapElement(sys);
801 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
802 t_pppSatObs* satObs = obsVector.at(ii);
803 if (satObs->eleSat() < _opt->_minEle) {
804 continue;
805 }
806 // reference satellite is unchanged
807 // ================================
808 if (!_obsPool->refSatChangeRequired(sys) && refSat->prn() == satObs->prn()) {
809 refSatDefined = true;
810 obsVector[ii]->setAsReference();
811 }
812 // reference satellite has changed
813 // ===============================
814 else if ( _obsPool->refSatChangeRequired(sys) && refSat->prn() != satObs->prn()) {
815 if (satObs->prn().system() == sys) {
816 if (!_historicalRefSats[sys].contains(satObs->prn())) {
817 refSatDefined = true;
818 obsVector[ii]->setAsReference();
819 refSat->setPrn(satObs->prn());
820 }
821 else if ( _historicalRefSats[sys].contains(satObs->prn())) {
822 refSatReDefinition = failure;
823 }
824 }
825 }
826 if (refSatDefined) {
827 if (OPT->_pseudoObsIono) {
828 refSat->setStecValue(satObs->getIonoCodeDelay(t_frequency::G1));
829 }
830 break;
831 }
832 }
833 // all available satellites were already tried to use
834 if ((!refSatDefined) && (refSatReDefinition == failure)) {
835 refSat->setPrn(t_prn(sys, 99));
836 _obsPool->setRefSatChangeRequired(sys, false);
837 return;
838 }
839
840 // reference satellite has to be initialized
841 // =========================================
842 if (!refSatDefined) {
843 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
844 t_pppSatObs* satObs = obsVector.at(ii);
845 if (satObs->eleSat() < _opt->_minEle) {
846 continue;
847 }
848 if (satObs->prn().system() == sys) {
849 obsVector[ii]->setAsReference();
850 refSat->setPrn(satObs->prn());
851 if (OPT->_pseudoObsIono) {
852 refSat->setStecValue(satObs->getIonoCodeDelay(t_frequency::G1));
853 }
854 refSatDefined = true;
855 break;
856 }
857 }
858 }
859
860 // no observations for that system
861 // ===============================
862 if (!refSatDefined) {
863 refSat->setPrn(t_prn());
864 }
865 _obsPool->setRefSatChangeRequired(sys, false); // done or impossible
866 }
867
868 return;
869}
870
871//
872//////////////////////////////////////////////////////////////////////////////
873t_irc t_pppClient::handleRefSatellites(std::vector<t_pppSatObs*>& obsVector) {
874
875 // set refSats in current epoch
876 // ============================
877 setRefSatellites(obsVector); // current epoch
878 LOG.setf(ios::fixed);
879 QMapIterator<char, t_pppRefSat*> it(_obsPool->getRefSatMap());
880 while (it.hasNext()) {
881 it.next();
882 char sys = it.key();
883 t_prn prn = it.value()->prn();
884 if (prn.number() == 0) { // no obs for that system available
885 continue;
886 }
887 else if (prn.number() == 99) { // refSat re-definition not possible
888#ifdef BNC_DEBUG_PPP
889 LOG << " => refSat re-definition impossible: " << sys << endl;
890#endif
891 return failure;
892 }
893 QString str;
894 if (prn == _obsPool->getRefSatMapElementLastEpoch(sys) ||
895 _obsPool->getRefSatMapElementLastEpoch(sys) == t_prn() ) {
896 _obsPool->setRefSatChanged(sys, false);
897 str = " SET ";
898 }
899 else {
900 _obsPool->setRefSatChanged(sys, true);
901 str = " RESET ";
902 }
903 LOG << string(_epoTimeRover) << str.toStdString() << " REFSAT " << sys << ": " << prn.toString() << endl;
904 }
905 return success;
906}
907
908void t_pppClient::setHistoricalRefSats() {
909 QMapIterator<char, t_pppRefSat*> it(_obsPool->getRefSatMap());
910 while (it.hasNext()) {
911 it.next();
912 t_prn prn = it.value()->prn();
913 char sys = prn.system();
914 if (_obsPool->refSatChangeRequired(sys)){
915 _historicalRefSats[sys].append(prn);
916 }
917 }}
918
919//
920//////////////////////////////////////////////////////////////////////////////
921void t_pppClient::reset() {
922
923 // to delete old orbit and clock corrections
924 delete _ephPool;
925 _ephPool = new t_pppEphPool();
926
927 // to delete old code biases
928 delete _obsPool;
929 _obsPool = new t_pppObsPool();
930
931 // to delete all parameters
932 delete _filter;
933 _filter = new t_pppFilter(_obsPool);
934
935}
Note: See TracBrowser for help on using the repository browser.