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

Last change on this file since 9386 was 9386, 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: 26.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 _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 epochReProcessing = true;
638 continue;
639 }
640 }
641
642 _offGR = cmpOffGR(_obsRover);
643 _offGE = cmpOffGE(_obsRover);
644 _offGC = cmpOffGC(_obsRover);
645
646 // Prepare Pseudo Observations of the Rover
647 // ----------------------------------------
648 _pseudoObsIono = preparePseudoObs(_obsRover);
649
650 if (int(_obsRover.size()) < _opt->_minObs) {
651 LOG << "t_pppClient::processEpoch not enough observations" << endl;
652 return finish(failure);
653 }
654
655 // Store last epoch of data
656 // ------------------------
657 _obsPool->putEpoch(_epoTimeRover, _obsRover, _pseudoObsIono);
658
659 // Process Epoch in Filter
660 // -----------------------
661 if (_filter->processEpoch() != success) {
662 return finish(failure);
663 }
664
665 // Epoch re-processing required?
666 // -----------------------------
667 if (_obsPool->refSatChangeRequired()) {//SLIP
668 epochReProcessing = true;
669 _obsPool->deleteLastEpoch();
670 setHistoricalRefSats();
671 }
672 else {
673 epochReProcessing = false;
674 _historicalRefSats.clear();
675 }
676 } while (epochReProcessing);
677 }
678 catch (Exception& exc) {
679 LOG << exc.what() << endl;
680 return finish(failure);
681 }
682 catch (t_except msg) {
683 LOG << msg.what() << endl;
684 return finish(failure);
685 }
686 catch (const char* msg) {
687 LOG << msg << endl;
688 return finish(failure);
689 }
690 catch (...) {
691 LOG << "unknown exception" << endl;
692 return finish(failure);
693 }
694
695 return finish(success);
696}
697
698//
699////////////////////////////////////////////////////////////////////////////
700double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
701 return aa[0]*bb[0] + aa[1]*bb[1] + aa[2]*bb[2] - aa[3]*bb[3];
702}
703
704//
705////////////////////////////////////////////////////////////////////////////
706void t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) {
707
708 if (pos.Nrows() != 4) {
709 pos.ReSize(4);
710 }
711 pos = 0.0;
712
713 for (int iter = 1; iter <= 2; iter++) {
714 Matrix BB = BBpass;
715 int mm = BB.Nrows();
716 for (int ii = 1; ii <= mm; ii++) {
717 double xx = BB(ii,1);
718 double yy = BB(ii,2);
719 double traveltime = 0.072;
720 if (iter > 1) {
721 double zz = BB(ii,3);
722 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
723 (yy-pos(2)) * (yy-pos(2)) +
724 (zz-pos(3)) * (zz-pos(3)) );
725 traveltime = rho / t_CST::c;
726 }
727 double angle = traveltime * t_CST::omega;
728 double cosa = cos(angle);
729 double sina = sin(angle);
730 BB(ii,1) = cosa * xx + sina * yy;
731 BB(ii,2) = -sina * xx + cosa * yy;
732 }
733
734 Matrix BBB;
735 if (mm > 4) {
736 SymmetricMatrix hlp; hlp << BB.t() * BB;
737 BBB = hlp.i() * BB.t();
738 }
739 else {
740 BBB = BB.i();
741 }
742 ColumnVector ee(mm); ee = 1.0;
743 ColumnVector alpha(mm); alpha = 0.0;
744 for (int ii = 1; ii <= mm; ii++) {
745 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
746 }
747 ColumnVector BBBe = BBB * ee;
748 ColumnVector BBBalpha = BBB * alpha;
749 double aa = lorentz(BBBe, BBBe);
750 double bb = lorentz(BBBe, BBBalpha)-1;
751 double cc = lorentz(BBBalpha, BBBalpha);
752 double root = sqrt(bb*bb-aa*cc);
753
754 Matrix hlpPos(4,2);
755 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
756 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
757
758 ColumnVector omc(2);
759 for (int pp = 1; pp <= 2; pp++) {
760 hlpPos(4,pp) = -hlpPos(4,pp);
761 omc(pp) = BB(1,4) -
762 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
763 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
764 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
765 hlpPos(4,pp);
766 }
767 if ( fabs(omc(1)) > fabs(omc(2)) ) {
768 pos = hlpPos.Column(2);
769 }
770 else {
771 pos = hlpPos.Column(1);
772 }
773 }
774}
775
776//
777//////////////////////////////////////////////////////////////////////////////
778void t_pppClient::setRefSatellites(std::vector<t_pppSatObs*>& obsVector) {
779
780 // reference satellite definition per system
781 for (unsigned iSys = 0; iSys < _opt->systems().size(); iSys++) {
782 char sys = _opt->systems()[iSys];
783 bool refSatDefined = false;
784 t_pppRefSat* refSat = _obsPool->getRefSatMapElement(sys);
785 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
786 t_pppSatObs* satObs = obsVector.at(ii);
787 if (satObs->eleSat() < _opt->_minEle) {
788 continue;
789 }
790 // reference satellite is unchanged
791 if (!_obsPool->refSatChangeRequired(sys) && refSat->prn() == satObs->prn()) {
792 refSatDefined = true;
793 obsVector[ii]->setAsReference();
794#ifdef BNC_DEBUG_PPP
795 LOG << "=> unchanged refsatprn: " << satObs->prn().toString() << endl;
796#endif
797 }
798 // reference satellite has changed
799 else if ( _obsPool->refSatChangeRequired(sys) &&
800 refSat->prn() != satObs->prn() &&
801 !_historicalRefSats[sys].contains(satObs->prn())) {
802 if (satObs->prn().system() == sys) {
803 refSatDefined = true;
804 obsVector[ii]->setAsReference();
805 refSat->setPrn(satObs->prn());
806#ifdef BNC_DEBUG_PPP
807 LOG << "=> set refsatprn: " << satObs->prn().toString() << endl;
808#endif
809 }
810 }
811 if (refSatDefined) {
812 if (OPT->_pseudoObsIono) {
813 refSat->setStecValue(satObs->getIonoCodeDelay(t_frequency::G1));
814 }
815 break;
816 }
817 }
818 // reference satellite has to be initialized
819 if (!refSatDefined) {
820 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
821 t_pppSatObs* satObs = obsVector.at(ii);
822 if (satObs->eleSat() < _opt->_minEle) {
823 continue;
824 }
825 if (satObs->prn().system() == sys &&
826 !_historicalRefSats[sys].contains(satObs->prn())) {
827 obsVector[ii]->setAsReference();
828 refSat->setPrn(satObs->prn());
829#ifdef BNC_DEBUG_PPP
830 LOG << " => set refsatprn: " << satObs->prn().toString() << endl;
831#endif
832 if (OPT->_pseudoObsIono) {
833 refSat->setStecValue(satObs->getIonoCodeDelay(t_frequency::G1));
834 }
835 refSatDefined = true;
836 break;
837 }
838 }
839 }
840 if (!refSatDefined) {
841 refSat->setPrn(t_prn());
842 }
843 _obsPool->setRefSatChangeRequired(sys, false);
844 }
845}
846
847//
848//////////////////////////////////////////////////////////////////////////////
849t_irc t_pppClient::handleRefSatellites(std::vector<t_pppSatObs*>& obsVector) {
850
851 // set refSats in current epoch
852 // ============================
853 setRefSatellites(obsVector); // current epoch
854 LOG.setf(ios::fixed);
855 QMapIterator<char, t_pppRefSat*> it(_obsPool->getRefSatMap());
856 while (it.hasNext()) {
857 it.next();
858 char sys = it.key();
859 t_prn prn = it.value()->prn();
860 if (prn.number() == 0) {
861 return failure;
862 }
863 QString str;
864 if (prn == _obsPool->getRefSatMapElementLastEpoch(sys) ||
865 _obsPool->getRefSatMapElementLastEpoch(sys) == t_prn() ) {
866 _obsPool->setRefSatChanged(sys, false);
867 str = " SET ";
868 }
869 else {
870 _obsPool->setRefSatChanged(sys, true);
871 str = " RESET ";
872 }
873 LOG << string(_epoTimeRover) << str.toStdString() << " REFSAT " << sys << ": " << prn.toString() << endl;
874 }
875 return success;
876}
877
878void t_pppClient::setHistoricalRefSats() {
879 QMapIterator<char, t_pppRefSat*> it(_obsPool->getRefSatMap());
880 while (it.hasNext()) {
881 it.next();
882 t_prn prn = it.value()->prn();
883 char sys = prn.system();
884 if (_obsPool->refSatChangeRequired(sys)){
885 _historicalRefSats[sys].append(prn);
886 }
887 }
888}
889
890//
891//////////////////////////////////////////////////////////////////////////////
892void t_pppClient::reset() {
893
894 // to delete old orbit and clock corrections
895 delete _ephPool;
896 _ephPool = new t_pppEphPool();
897
898 // to delete old code biases
899 delete _obsPool;
900 _obsPool = new t_pppObsPool();
901
902 // to delete all parameters
903 delete _filter;
904 _filter = new t_pppFilter(_obsPool);
905
906}
Note: See TracBrowser for help on using the repository browser.