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

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

updates regarding inter system biases
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 24.6 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) {
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
373 if (fabs(maxRes) > 1000.0) {
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) > 1000.0) {
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) > 1000.0) {
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 _historicalRefSats.clear();
596 try {
597 initOutput(output);
598 int num = 0;
599 bool epochReProcessing = false;
600
601 do {
602 num++;
603 if (num == 1) {
604 LOG << "\nPPP of Epoch ";
605 if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
606 LOG << "\n---------------------------------------------------------------\n";
607 }
608
609 // Prepare Observations of the Rover
610 // ---------------------------------
611 if (prepareObs(satObs, _obsRover, _epoTimeRover) != success) {
612 return finish(failure);
613 }
614
615 for (int iter = 1; iter <= 2; iter++) {
616 ColumnVector xyzc(4); xyzc = 0.0;
617 bool print = (iter == 2);
618 if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != success) {
619 return finish(failure);
620 }
621 if (cmpModel(_staRover, xyzc, _obsRover) != success) {
622 return finish(failure);
623 }
624 }
625
626 _offGR = cmpOffGR(_obsRover);
627 _offGE = cmpOffGE(_obsRover);
628 _offGC = cmpOffGC(_obsRover);
629
630 if (_opt->_refSatRequired) {
631 setRefSatellites(_obsRover);
632 LOG.setf(ios::fixed);
633 QMapIterator<char, t_pppRefSat*> it(_obsPool->getRefSatMap());
634 while (it.hasNext()) {
635 it.next();
636 char sys = it.key();
637 string prn = it.value()->prn().toString();
638 if (num == 1) {
639 LOG << "set ";
640 }
641 else {
642 LOG << "reset ";
643 }
644 LOG << string(_epoTimeRover) << " REFSAT " << sys << ": " << prn << endl;
645 }
646 }
647
648
649 // Prepare Pseudo Observations of the Rover
650 // ----------------------------------------
651 _pseudoObsIono = preparePseudoObs(_obsRover);
652
653 if (int(_obsRover.size()) < _opt->_minObs) {
654 LOG << "t_pppClient::processEpoch not enough observations" << endl;
655 return finish(failure);
656 }
657
658
659 // Store last epoch of data
660 // ------------------------
661 _obsPool->putEpoch(_epoTimeRover, _obsRover, _pseudoObsIono);
662 _obsPool->setHistoricalRefSatList(_historicalRefSats);
663
664 // Process Epoch in Filter
665 // -----------------------
666 if (_filter->processEpoch(num) != success) {
667 return finish(failure);
668 }
669
670 // Epoch re-processing required?
671 // -----------------------------
672 if (_obsPool->refSatChangeRequired()) {
673 epochReProcessing = true;
674 _obsPool->deleteLastEpoch();
675 QMapIterator<char, t_pppRefSat*> it(_obsPool->getRefSatMap());
676 while (it.hasNext()) {
677 it.next();
678 _historicalRefSats.append(it.value()->prn());
679 }
680 }
681 else {
682 epochReProcessing = false;
683
684 }
685 } while (epochReProcessing);
686 }
687 catch (Exception& exc) {
688 LOG << exc.what() << endl;
689 return finish(failure);
690 }
691 catch (t_except msg) {
692 LOG << msg.what() << endl;
693 return finish(failure);
694 }
695 catch (const char* msg) {
696 LOG << msg << endl;
697 return finish(failure);
698 }
699 catch (...) {
700 LOG << "unknown exception" << endl;
701 return finish(failure);
702 }
703
704 return finish(success);
705}
706
707//
708////////////////////////////////////////////////////////////////////////////
709double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
710 return aa[0]*bb[0] + aa[1]*bb[1] + aa[2]*bb[2] - aa[3]*bb[3];
711}
712
713//
714////////////////////////////////////////////////////////////////////////////
715void t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) {
716
717 if (pos.Nrows() != 4) {
718 pos.ReSize(4);
719 }
720 pos = 0.0;
721
722 for (int iter = 1; iter <= 2; iter++) {
723 Matrix BB = BBpass;
724 int mm = BB.Nrows();
725 for (int ii = 1; ii <= mm; ii++) {
726 double xx = BB(ii,1);
727 double yy = BB(ii,2);
728 double traveltime = 0.072;
729 if (iter > 1) {
730 double zz = BB(ii,3);
731 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
732 (yy-pos(2)) * (yy-pos(2)) +
733 (zz-pos(3)) * (zz-pos(3)) );
734 traveltime = rho / t_CST::c;
735 }
736 double angle = traveltime * t_CST::omega;
737 double cosa = cos(angle);
738 double sina = sin(angle);
739 BB(ii,1) = cosa * xx + sina * yy;
740 BB(ii,2) = -sina * xx + cosa * yy;
741 }
742
743 Matrix BBB;
744 if (mm > 4) {
745 SymmetricMatrix hlp; hlp << BB.t() * BB;
746 BBB = hlp.i() * BB.t();
747 }
748 else {
749 BBB = BB.i();
750 }
751 ColumnVector ee(mm); ee = 1.0;
752 ColumnVector alpha(mm); alpha = 0.0;
753 for (int ii = 1; ii <= mm; ii++) {
754 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
755 }
756 ColumnVector BBBe = BBB * ee;
757 ColumnVector BBBalpha = BBB * alpha;
758 double aa = lorentz(BBBe, BBBe);
759 double bb = lorentz(BBBe, BBBalpha)-1;
760 double cc = lorentz(BBBalpha, BBBalpha);
761 double root = sqrt(bb*bb-aa*cc);
762
763 Matrix hlpPos(4,2);
764 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
765 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
766
767 ColumnVector omc(2);
768 for (int pp = 1; pp <= 2; pp++) {
769 hlpPos(4,pp) = -hlpPos(4,pp);
770 omc(pp) = BB(1,4) -
771 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
772 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
773 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
774 hlpPos(4,pp);
775 }
776 if ( fabs(omc(1)) > fabs(omc(2)) ) {
777 pos = hlpPos.Column(2);
778 }
779 else {
780 pos = hlpPos.Column(1);
781 }
782 }
783}
784
785//
786//////////////////////////////////////////////////////////////////////////////
787void t_pppClient::setRefSatellites(std::vector<t_pppSatObs*>& obsVector) {
788
789 // reference satellite definition per system
790 for (unsigned iSys = 0; iSys < _opt->systems().size(); iSys++) {
791 char system = _opt->systems()[iSys];
792 bool refSatDefined = false;
793 t_pppRefSat* refSat = _obsPool->getRefSatMapElement(system);
794 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
795 t_pppSatObs* satObs = obsVector.at(ii);
796 if (satObs->eleSat() < _opt->_minEle) {continue;}
797 // reference satellite is unchanged
798 if (!_obsPool->refSatChangeRequired() && refSat->prn() == satObs->prn()) {
799 refSatDefined = true;
800 obsVector[ii]->setAsReference();
801 }
802 // reference satellite has changed
803 else if ( _obsPool->refSatChangeRequired() && refSat->prn() != satObs->prn() && !_historicalRefSats.contains(satObs->prn())) {
804 if (satObs->prn().system() == system) {
805 refSatDefined = true;
806 obsVector[ii]->setAsReference();
807 refSat->setPrn(satObs->prn());
808 }
809 }
810 if (refSatDefined) {
811 if (OPT->_pseudoObsIono) {
812 refSat->setStecValue(satObs->getIonoCodeDelay(t_frequency::G1));
813 }
814 break;
815 }
816 }
817 // reference satellite has to be initialized
818 if (!refSatDefined) {
819 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
820 t_pppSatObs* satObs = obsVector.at(ii);
821 if (satObs->eleSat() < _opt->_minEle) {
822 continue;
823 }
824 if (satObs->prn().system() == system) {
825 obsVector[ii]->setAsReference();
826 refSat->setPrn(satObs->prn());
827 if (OPT->_pseudoObsIono) {
828 refSat->setStecValue(satObs->getIonoCodeDelay(t_frequency::G1));
829 }
830 refSatDefined = true;
831 break;
832 }
833 }
834 }
835 }
836 _obsPool->setRefSatChangeRequired(false);
837
838}
839
840//
841//////////////////////////////////////////////////////////////////////////////
842void t_pppClient::reset() {
843
844 // to delete old orbit and clock corrections
845 delete _ephPool;
846 _ephPool = new t_pppEphPool();
847
848 // to delete old code biases
849 delete _obsPool;
850 _obsPool = new t_pppObsPool();
851
852 // to delete all parameters
853 delete _filter;
854 _filter = new t_pppFilter(_obsPool);
855
856}
Note: See TracBrowser for help on using the repository browser.