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

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