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

Last change on this file since 11208 was 11196, checked in by stuerze, 6 days ago

some PPP updates

  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 23.3 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 _running = true;
52 _output = 0;
53 _opt = new t_pppOptions(*opt);
54 _log = new ostringstream();
55 _ephPool = new t_pppEphPool();
56 _obsPool = new t_pppObsPool();
57 _staRover = new t_pppStation();
58 _filter = new t_pppFilter();
59 _tides = new t_tides();
60 _antex = 0;
61 _ssrFormat = ssrUnknown;
62 if (!_opt->_antexFileName.empty()) {
63 _antex = new bncAntex(_opt->_antexFileName.c_str());
64 *_log << _antex->info().toStdString() << endl;
65 }
66 if (!_opt->_blqFileName.empty()) {
67 if (_tides->readBlqFile(_opt->_blqFileName.c_str()) == success) {
68 //_tides->printAllBlqSets();
69 }
70 }
71 CLIENTS.setLocalData(this); // CLIENTS takes ownership over "this"
72}
73
74// Destructor
75//////////////////////////////////////////////////////////////////////////////
76t_pppClient::~t_pppClient() {
77 _running = false;
78 delete _log;
79 delete _opt;
80 delete _ephPool;
81 delete _obsPool;
82 delete _staRover;
83 if (_antex) {
84 delete _antex;
85 }
86 delete _filter;
87 delete _tides;
88 clearObs();
89}
90
91//
92//////////////////////////////////////////////////////////////////////////////
93void t_pppClient::putEphemeris(const t_eph* eph) {
94 const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
95 const t_ephGlo* ephGlo = dynamic_cast<const t_ephGlo*>(eph);
96 const t_ephGal* ephGal = dynamic_cast<const t_ephGal*>(eph);
97 const t_ephBDS* ephBDS = dynamic_cast<const t_ephBDS*>(eph);
98 if (ephGPS) {
99 _ephPool->putEphemeris(new t_ephGPS(*ephGPS));
100 }
101 else if (ephGlo) {
102 _ephPool->putEphemeris(new t_ephGlo(*ephGlo));
103 }
104 else if (ephGal) {
105 // SSR corrections of all formats refer to Galileo I/NAV. The pool matches
106 // corrections by IOD only and keeps one ephemeris per TOC, so an F/NAV set
107 // with the same IODnav (different clock parameters) must not get in.
108 if (ephGal->type() == t_eph::INAV) {
109 _ephPool->putEphemeris(new t_ephGal(*ephGal));
110 }
111 }
112 else if (ephBDS) {
113 _ephPool->putEphemeris(new t_ephBDS(*ephBDS));
114 }
115}
116
117//
118//////////////////////////////////////////////////////////////////////////////
119void t_pppClient::putTec(const t_vTec* vTec) {
120 _obsPool->putTec(new t_vTec(*vTec));
121}
122
123//
124//////////////////////////////////////////////////////////////////////////////
125void t_pppClient::putMetaData(const t_metaData* metaData) {
126 _obsPool->putMetaData(new t_metaData(*metaData));
127}
128
129//
130//////////////////////////////////////////////////////////////////////////////
131void t_pppClient::putOrbCorrections(const vector<t_orbCorr*>& corr) {
132 if (OPT->_logMode == t_pppOptions::normal && corr.size() > 0) {
133 LOG << "orbCorrections " << string(corr[0]->_time) << ' ' << corr.size() << endl;
134 }
135 for (unsigned ii = 0; ii < corr.size(); ii++) {
136 _ssrFormat = corr[ii]->_ssrFormat;
137 _ephPool->putOrbCorrection(new t_orbCorr(*corr[ii]));
138 }
139}
140
141//
142//////////////////////////////////////////////////////////////////////////////
143void t_pppClient::putClkCorrections(const vector<t_clkCorr*>& corr) {
144 if (OPT->_logMode == t_pppOptions::normal && corr.size() > 0) {
145 LOG << "clkCorrections " << string(corr[0]->_time) << ' ' << corr.size() << endl;
146 }
147 for (unsigned ii = 0; ii < corr.size(); ii++) {
148 _ephPool->putClkCorrection(new t_clkCorr(*corr[ii]));
149 }
150}
151
152//
153//////////////////////////////////////////////////////////////////////////////
154void t_pppClient::putCodeBiases(const vector<t_satCodeBias*>& biases) {
155 if (OPT->_logMode == t_pppOptions::normal && biases.size() > 0) {
156 LOG << "codeBiases " << string(biases[0]->_time) << ' ' << biases.size() << endl;
157 }
158 set<char> systems;
159 for (unsigned ii = 0; ii < biases.size(); ii++) {
160 t_satCodeBias* newBias = new t_satCodeBias(*biases[ii]);
161 char sys = newBias->_prn.system();
162 if (systems.find(sys) == systems.end()) {
163 _obsPool->clearCodeBiases(sys);
164 systems.insert(sys);
165 }
166 _obsPool->putCodeBias(newBias);
167 }
168}
169
170//
171//////////////////////////////////////////////////////////////////////////////
172void t_pppClient::putSatAntennas(const vector<t_satAntenna*>& satAntennas) {
173 if (OPT->_logMode == t_pppOptions::normal && satAntennas.size() > 0) {
174 LOG << "satAntennas " << string(satAntennas[0]->_time) << ' ' << satAntennas.size() << endl;
175 }
176 for (unsigned ii = 0; ii < satAntennas.size(); ii++) {
177 _obsPool->putSatAntenna(new t_satAntenna(*satAntennas[ii]));
178 }
179}
180
181//
182//////////////////////////////////////////////////////////////////////////////
183void t_pppClient::putPhaseBiases(const vector<t_satPhaseBias*>& biases) {
184 if (OPT->_logMode == t_pppOptions::normal && biases.size() > 0) {
185 LOG << "phaseBiases " << string(biases[0]->_time) << ' ' << biases.size() << endl;
186 }
187 set<char> systems;
188 for (unsigned ii = 0; ii < biases.size(); ii++) {
189 t_satPhaseBias* newBias = new t_satPhaseBias(*biases[ii]);
190 char sys = newBias->_prn.system();
191 if (systems.find(sys) == systems.end()) {
192 _obsPool->clearPhaseBiases(sys);
193 systems.insert(sys);
194 }
195 _obsPool->putPhaseBias(newBias);
196 }
197}
198
199//
200//////////////////////////////////////////////////////////////////////////////
201t_irc t_pppClient::prepareObs(const vector<t_satObs*>& satObs,
202 vector<t_pppSatObs*>& obsVector, bncTime& epoTime) {
203
204 // Default
205 // -------
206 epoTime.reset();
207 clearObs();
208
209 // Create vector of valid observations
210 // -----------------------------------
211 for (unsigned ii = 0; ii < satObs.size(); ii++) {
212 char sys = satObs[ii]->_prn.system();
213 if (_opt->useSystem(sys)) {
214 t_pppSatObs* pppSatObs = new t_pppSatObs(*satObs[ii]);
215 if (pppSatObs->isValid()) {
216 obsVector.push_back(pppSatObs);
217 }
218 else {
219 delete pppSatObs;
220 }
221 }
222 }
223
224 // Check whether data are synchronized, compute epoTime
225 // ----------------------------------------------------
226 const double MAXSYNC = 0.05; // synchronization limit
227 double meanDt = 0.0;
228 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
229 const t_pppSatObs* satObs = obsVector.at(ii);
230 if (epoTime.undef()) {
231 epoTime = satObs->time();
232 }
233 else {
234 double dt = satObs->time() - epoTime;
235 if (fabs(dt) > MAXSYNC) {
236 LOG << "t_pppClient::prepareObs asynchronous observations" << endl;
237 return failure;
238 }
239 meanDt += dt;
240 }
241 }
242
243 if (obsVector.size() > 0) {
244 epoTime += meanDt / obsVector.size();
245 }
246
247 return success;
248}
249
250//
251//////////////////////////////////////////////////////////////////////////////
252bool t_pppClient::preparePseudoObs(std::vector<t_pppSatObs*>& obsVector) {
253
254 bool pseudoObsIono = false;
255
256 if (_opt->_pseudoObsIono) {
257
258 // Select reference satellite per system: keep the current one if still
259 // visible; only switch to highest elevation when the current one disappears
260 // -------------------------------------------------------------------------
261 map<char, t_pppSatObs*> refSatMap;
262 map<char, t_pppSatObs*> bestEleMap;
263 for (t_pppSatObs* satObs : obsVector) {
264 satObs->resetReference();
265 double stec = satObs->getIonoCodeDelay(t_frequency::G1);
266 if (stec == 0.0) continue;
267 char sys = satObs->prn().system();
268 if (!bestEleMap.count(sys) || satObs->eleSat() > bestEleMap[sys]->eleSat()) {
269 bestEleMap[sys] = satObs;
270 }
271 if (_refPrnMap.count(sys) && satObs->prn() == _refPrnMap[sys]) {
272 refSatMap[sys] = satObs;
273 }
274 }
275 for (auto& kv : bestEleMap) {
276 if (!refSatMap.count(kv.first)) {
277 refSatMap[kv.first] = kv.second;
278 }
279 }
280 for (auto& kv : refSatMap) {
281 _refPrnMap[kv.first] = kv.second->prn();
282 kv.second->setAsReference();
283 }
284
285 if (bestEleMap.empty()) {
286 LOG << "GIM pseudo-obs unavailable: no valid STEC (vTec data missing?)" << endl;
287 }
288
289 // Set STEC pseudo-observations (single-differenced vs reference satellite)
290 // -------------------------------------------------------------------------
291 int nGIM = 0;
292 for (t_pppSatObs* satObs : obsVector) {
293 char sys = satObs->prn().system();
294 double stecRefSat = refSatMap.count(sys)
295 ? refSatMap[sys]->getIonoCodeDelay(t_frequency::G1)
296 : 0.0;
297 if (satObs->setPseudoObsIono(t_frequency::G1, stecRefSat)) {
298 pseudoObsIono = true;
299 nGIM++;
300 }
301 }
302 if (nGIM > 0) {
303 LOG << "GIM pseudo-obs: " << nGIM << " satellites, ref:";
304 for (auto& kv : refSatMap) {
305 LOG << ' ' << kv.second->prn().toString();
306 }
307 LOG << endl;
308 }
309 }
310
311 if (OPT->_logMode == t_pppOptions::all) {
312 vector<t_pppSatObs*>::iterator it = obsVector.begin();
313 while (it != obsVector.end()) {
314 t_pppSatObs* satObs = *it;
315 satObs->printObsMinusComputed();
316 it++;
317 }
318 }
319 return pseudoObsIono;
320}
321
322// Compute the Bancroft position, check for blunders
323//////////////////////////////////////////////////////////////////////////////
324t_irc t_pppClient::cmpBancroft(const bncTime& epoTime,
325 vector<t_pppSatObs*>& obsVector,
326 ColumnVector& xyzc, bool print) {
327
328 int numBancroft = obsVector.size();
329
330 while (_running) {
331 Matrix BB(numBancroft, 4);
332 int iObs = -1;
333 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
334 const t_pppSatObs* satObs = obsVector.at(ii);
335 t_lc LC = satObs->rangeLC();
336 if ( satObs->isValid(LC) &&
337 (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle) ) {
338 ++iObs;
339 BB[iObs][0] = satObs->xc()[0];
340 BB[iObs][1] = satObs->xc()[1];
341 BB[iObs][2] = satObs->xc()[2];
342 BB[iObs][3] = satObs->obsValue(LC) - satObs->cmpValueForBanc(LC);
343 }
344 }
345 if (iObs + 1 < _opt->_minObs) {
346 LOG << "t_pppClient::cmpBancroft not enough observations: " << iObs + 1 << endl;
347 return failure;
348 }
349 BB = BB.Rows(1,iObs+1);
350 if (bancroft(BB, xyzc) != success) {
351 return failure;
352 }
353
354 xyzc[3] /= t_CST::c;
355
356 // Check Blunders
357 // --------------
358 const double BLUNDER = 1500.0;
359 double maxRes = 0.0;
360 unsigned maxResIndex = 0;
361 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
362 const t_pppSatObs* satObs = obsVector.at(ii);
363 t_lc LC = satObs->rangeLC();
364 if (satObs->isValid(LC) &&
365 (!satObs->modelSet() || satObs->eleSat() >= _opt->_minEle) ) {
366 ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3);
367 double res = rr.NormFrobenius() - satObs->obsValue(LC)
368 - (satObs->xc()[3] - xyzc[3]) * t_CST::c;
369 if (fabs(res) > maxRes) {
370 maxRes = fabs(res);
371 maxResIndex = ii;
372 }
373 }
374 }
375 if (maxRes < BLUNDER) {
376 if (print) {
377 LOG.setf(ios::fixed);
378 LOG << "\nPPP of Epoch ";
379 if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
380 LOG << " using " << _opt->_corrMount;
381 LOG << "\n---------------------------------------------------------------\n";
382 LOG << string(epoTime) << " BANCROFT: "
383 << setw(14) << setprecision(3) << xyzc[0] << ' '
384 << setw(14) << setprecision(3) << xyzc[1] << ' '
385 << setw(14) << setprecision(3) << xyzc[2] << ' '
386 << setw(14) << setprecision(3) << xyzc[3] * t_CST::c << endl << endl;
387 }
388 break;
389 }
390 else {
391 t_pppSatObs* satObs = obsVector.at(maxResIndex);
392 LOG << "t_pppClient::cmpBancroft Outlier " << satObs->prn().toString()
393 << " " << maxRes << endl;
394 delete satObs;
395 obsVector.erase(obsVector.begin() + maxResIndex);
396 }
397 }
398
399 return success;
400}
401
402//
403//////////////////////////////////////////////////////////////////////////////
404void t_pppClient::initOutput(t_output* output) {
405 _output = output;
406 _output->_numSat = 0;
407 _output->_hDop = 0.0;
408 _output->_resRmsCode = 0.0;
409 _output->_resRmsPhase = 0.0;
410 _output->_error = false;
411}
412
413//
414//////////////////////////////////////////////////////////////////////////////
415void t_pppClient::clearObs() {
416 for (unsigned ii = 0; ii < _obsRover.size(); ii++) {
417 delete _obsRover.at(ii);
418 }
419 _obsRover.clear();
420}
421
422//
423//////////////////////////////////////////////////////////////////////////////
424void t_pppClient::finish(t_irc irc, int ind) {
425#ifdef BNC_DEBUG_PPP
426 LOG << "t_pppClient::finish(" << ind << "): " << irc << endl;
427#endif
428
429 clearObs();
430
431 _output->_epoTime = _epoTimeRover;
432
433 if (irc == success) {
434 _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0];
435 _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1];
436 _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2];
437
438 xyz2neu(_staRover->ellApr().data(), _filter->x().data(), _output->_neu);
439
440 copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix);
441
442 _output->_trp0 = t_tropo::delay_saast(_staRover->xyzApr(), M_PI/2.0);
443 _output->_trp = _filter->trp();
444 _output->_trpStdev = _filter->trpStdev();
445
446 _output->_fixRatio = _filter->fixRatio();
447
448 _output->_numSat = _filter->numSat();
449 _output->_hDop = _filter->HDOP();
450 _output->_resRmsCode = _filter->resRmsCode();
451 _output->_resRmsPhase = _filter->resRmsPhase();
452 _output->_error = false;
453 }
454 else {
455 _output->_error = true;
456 }
457
458 _output->_log = _log->str();
459 delete _log; _log = new ostringstream();
460
461 return;
462}
463
464//
465//////////////////////////////////////////////////////////////////////////////
466t_irc t_pppClient::cmpModel(t_pppStation* station, const ColumnVector& xyzc,
467 vector<t_pppSatObs*>& obsVector) {
468 bncTime time;
469 time = _epoTimeRover;
470 station->setName(_opt->_roverName);
471 station->setAntName(_opt->_antNameRover);
472 station->setEpochTime(time);
473
474 if (_opt->xyzAprRoverSet()) {
475 if (_opt->_refEpochRover != 0.0) {
476 double dt = decimalYear(time) - _opt->_refEpochRover;
477 station->setXyzApr(_opt->_xyzAprRover + dt * _opt->_velRover);
478 }
479 else {
480 station->setXyzApr(_opt->_xyzAprRover);
481 }
482 }
483 else {
484 station->setXyzApr(xyzc.Rows(1,3));
485 }
486 station->setNeuEcc(_opt->_neuEccRover);
487
488 // Receiver Clock
489 // --------------
490 station->setDClk(xyzc[3]);
491
492 // Tides
493 // -----
494 // RTCM-SSR Metadata (model correction type 3 = solid earth tides) tells us
495 // whether - and with which model - the correction stream's generation
496 // system already accounted for solid earth tides. To stay consistent with
497 // the orbit/clock corrections, mirror that choice here: skip our own
498 // correction if the provider applied none, apply our default model if the
499 // provider used its own default, and fall back to our default (with a
500 // warning) if the provider used some other, unsupported model - we have no
501 // way to reproduce an unknown model exactly. Absent any Metadata message
502 // (e.g. RTNET or older streams), keep today's unconditional behavior.
503 bool applyEarthTide = true;
504 const t_metaData* metaData = _obsPool->metaData();
505 if (metaData) {
506 for (unsigned ii = 0; ii < metaData->_entries.size(); ii++) {
507 const t_metaDataEntry& entry = metaData->_entries[ii];
508 if (entry._typeIndicator == 3) { // solid earth tides
509 if (!entry._applicationIndicator) {
510 applyEarthTide = false;
511 }
512 else if (entry._nonDefaultIndicator) {
513 LOG << "WARNING: SSR Metadata signals a non-default solid earth "
514 << "tide model (identifier " << entry._nonDefaultIdentifier
515 << "); applying BNC's default model anyway, results may be "
516 << "inconsistent" << endl;
517 }
518 break;
519 }
520 }
521 }
522 if (applyEarthTide) {
523 station->setTideDsplEarth(_tides->earth(time, station->xyzApr()));
524 }
525 else {
526 ColumnVector tideDsplEarth(3); tideDsplEarth = 0.0;
527 station->setTideDsplEarth(tideDsplEarth);
528 }
529 station->setTideDsplOcean(_tides->ocean(time, station->xyzApr(), station->name()));
530
531 // Observation model
532 // -----------------
533 vector<t_pppSatObs*>::iterator it = obsVector.begin();
534 while (it != obsVector.end()) {
535 t_pppSatObs* satObs = *it;
536 t_irc modelSetup;
537 if (satObs->isValid()) {
538 modelSetup = satObs->cmpModel(station);
539 }
540 if (satObs->isValid() &&
541 satObs->eleSat() >= _opt->_minEle &&
542 modelSetup == success) {
543 ++it;
544 }
545 else {
546 delete satObs;
547 it = obsVector.erase(it);
548 }
549 }
550
551 return success;
552}
553
554//
555//////////////////////////////////////////////////////////////////////////////
556void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
557
558 try {
559 initOutput(output);
560
561 // Prepare Observations of the Rover
562 // ---------------------------------
563 if (prepareObs(satObs, _obsRover, _epoTimeRover) != success) {
564 return finish(failure, 1);
565 }
566
567 // a changed ANTEX file is read and used from its GPS week on
568 if (_antex) {
569 QString msg = _antexReloader.update(_antex, _epoTimeRover);
570 if (!msg.isEmpty()) {
571 LOG << string(_epoTimeRover) << " " << msg.toStdString() << endl;
572 }
573 }
574
575 for (int iter = 1; iter <= 2; iter++) {
576 ColumnVector xyzc(4); xyzc = 0.0;
577 bool print = (iter == 2);
578 if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != success) {
579 return finish(failure, 2);
580 }
581 if (cmpModel(_staRover, xyzc, _obsRover) != success) {
582 return finish(failure, 3);
583 }
584 }
585
586 // Use observations only if satellite code biases are available
587 // ------------------------------------------------------------
588 if (_opt->ambRes()) {
589 useObsWithBiasesOnly(_obsRover);
590 }
591
592 if (int(_obsRover.size()) < _opt->_minObs) {
593 LOG << "t_pppClient::processEpoch not enough observations" << endl;
594 return finish(failure, 4);
595 }
596
597 // Prepare Pseudo Observations of the Rover
598 // ----------------------------------------
599 _pseudoObsIono = preparePseudoObs(_obsRover);
600
601 // Store last epoch of data
602 // ------------------------
603 _obsPool->putEpoch(_epoTimeRover, _obsRover, _pseudoObsIono);
604
605 // Process Epoch in Filter
606 // -----------------------
607 if (_filter->processEpoch(_obsPool) != success) {
608 LOG << "t_pppFilter::processEpoch() != success" << endl;
609 return finish(failure, 5);
610 }
611 }
612 catch (Exception& exc) {
613 LOG << exc.what() << endl;
614 // A failed Kalman update (e.g. non-positive-definite covariance) leaves
615 // the filter state unusable and would be carried into every subsequent
616 // epoch => restart the filter, keeping ephemerides and corrections
617 // ----------------------------------------------------------------------
618 LOG << string(_epoTimeRover) << " pppClient: filter reset after exception" << endl;
619 delete _filter;
620 _filter = new t_pppFilter();
621 return finish(failure, 6);
622 }
623 catch (t_except msg) {
624 LOG << msg.what() << endl;
625 return finish(failure, 7);
626 }
627 catch (const char* msg) {
628 LOG << msg << endl;
629 return finish(failure, 8);
630 }
631 catch (...) {
632 LOG << "unknown exception" << endl;
633 return finish(failure, 9);
634 }
635 return finish(success, 0);
636}
637
638//
639////////////////////////////////////////////////////////////////////////////
640double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
641 return aa[0]*bb[0] + aa[1]*bb[1] + aa[2]*bb[2] - aa[3]*bb[3];
642}
643
644//
645////////////////////////////////////////////////////////////////////////////
646t_irc t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) {
647
648 if (pos.Nrows() != 4) {
649 pos.ReSize(4);
650 }
651 pos = 0.0;
652
653 for (int iter = 1; iter <= 2; iter++) {
654 Matrix BB = BBpass;
655 int mm = BB.Nrows();
656 for (int ii = 1; ii <= mm; ii++) {
657 double xx = BB(ii,1);
658 double yy = BB(ii,2);
659 double traveltime = 0.072;
660 if (iter > 1) {
661 double zz = BB(ii,3);
662 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
663 (yy-pos(2)) * (yy-pos(2)) +
664 (zz-pos(3)) * (zz-pos(3)) );
665 traveltime = rho / t_CST::c;
666 }
667 double angle = traveltime * t_CST::omega;
668 double cosa = cos(angle);
669 double sina = sin(angle);
670 BB(ii,1) = cosa * xx + sina * yy;
671 BB(ii,2) = -sina * xx + cosa * yy;
672 }
673
674 Matrix BBB;
675 if (mm > 4) {
676 SymmetricMatrix hlp; hlp << BB.t() * BB;
677 BBB = hlp.i() * BB.t();
678 }
679 else {
680 BBB = BB.i();
681 }
682 ColumnVector ee(mm); ee = 1.0;
683 ColumnVector alpha(mm); alpha = 0.0;
684 for (int ii = 1; ii <= mm; ii++) {
685 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
686 }
687 ColumnVector BBBe = BBB * ee;
688 ColumnVector BBBalpha = BBB * alpha;
689 double aa = lorentz(BBBe, BBBe);
690 double bb = lorentz(BBBe, BBBalpha)-1;
691 double cc = lorentz(BBBalpha, BBBalpha);
692 double root = sqrt(bb*bb-aa*cc);
693
694 Matrix hlpPos(4,2);
695 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
696 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
697
698 ColumnVector omc(2);
699 for (int pp = 1; pp <= 2; pp++) {
700 hlpPos(4,pp) = -hlpPos(4,pp);
701 omc(pp) = BB(1,4) -
702 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
703 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
704 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
705 hlpPos(4,pp);
706 }
707 if ( fabs(omc(1)) > fabs(omc(2)) ) {
708 pos = hlpPos.Column(2);
709 }
710 else {
711 pos = hlpPos.Column(1);
712 }
713 }
714 if (pos.size() != 4 ||
715 std::isnan(pos(1)) ||
716 std::isnan(pos(2)) ||
717 std::isnan(pos(3)) ||
718 std::isnan(pos(4))) {
719 return failure;
720 }
721
722 return success;
723}
724
725//
726//////////////////////////////////////////////////////////////////////////////
727void t_pppClient::reset() {
728
729 LOG << "pppClient: reset" << endl;
730 _refPrnMap.clear();
731 // to delete old orbit and clock corrections
732 delete _ephPool;
733 _ephPool = new t_pppEphPool();
734
735 // to delete old biases
736 delete _obsPool;
737 _obsPool = new t_pppObsPool();
738
739 // to delete all parameters
740 delete _filter;
741 _filter = new t_pppFilter();
742
743}
744
745//
746//////////////////////////////////////////////////////////////////////////////
747void t_pppClient::useObsWithBiasesOnly(vector<t_pppSatObs*>& obsVector) const {
748 vector<t_pppSatObs*>::iterator it = obsVector.begin();
749 while (it != obsVector.end()) {
750 t_pppSatObs* pppSatObs = *it;
751 if (pppSatObs->hasBiases()) {
752 ++it;
753 }
754 else {
755 LOG << "t_pppClient::useObsWithBiasesOnly: no biases: " << pppSatObs->prn().toString().c_str() << endl;
756 it = obsVector.erase(it);
757 delete pppSatObs;
758 }
759 }
760}
Note: See TracBrowser for help on using the repository browser.