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

Last change on this file since 7048 was 6653, checked in by stuerze, 10 years ago

sinex tro file support added,
troposphere results in overall ppp logfile added

File size: 15.0 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 <stdlib.h>
22#include <string.h>
23#include <stdexcept>
24
25#include "pppClient.h"
26#include "pppEphPool.h"
27#include "pppObsPool.h"
28#include "bncconst.h"
29#include "bncutils.h"
30#include "pppStation.h"
31#include "bncantex.h"
32#include "pppFilter.h"
33
34using namespace BNC_PPP;
35using namespace std;
36
37// Global variable holding thread-specific pointers
38//////////////////////////////////////////////////////////////////////////////
39QThreadStorage<t_pppClient*> CLIENTS;
40
41// Static function returning thread-specific pointer
42//////////////////////////////////////////////////////////////////////////////
43t_pppClient* t_pppClient::instance() {
44 return CLIENTS.localData();
45}
46
47// Constructor
48//////////////////////////////////////////////////////////////////////////////
49t_pppClient::t_pppClient(const t_pppOptions* opt) {
50 _output = 0;
51 _opt = new t_pppOptions(*opt);
52 _log = new ostringstream();
53 _ephPool = new t_pppEphPool();
54 _obsPool = new t_pppObsPool();
55 _staRover = new t_pppStation();
56 _filter = new t_pppFilter();
57 _tides = new t_tides();
58
59 if (!_opt->_antexFileName.empty()) {
60 _antex = new bncAntex(_opt->_antexFileName.c_str());
61 }
62 else {
63 _antex = 0;
64 }
65
66 CLIENTS.setLocalData(this); // CLIENTS takes ownership over "this"
67}
68
69// Destructor
70//////////////////////////////////////////////////////////////////////////////
71t_pppClient::~t_pppClient() {
72 delete _log;
73 delete _opt;
74 delete _ephPool;
75 delete _obsPool;
76 delete _staRover;
77 delete _antex;
78 delete _filter;
79 delete _tides;
80 clearObs();
81}
82
83//
84//////////////////////////////////////////////////////////////////////////////
85void t_pppClient::putEphemeris(const t_eph* eph) {
86 const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
87 const t_ephGlo* ephGlo = dynamic_cast<const t_ephGlo*>(eph);
88 const t_ephGal* ephGal = dynamic_cast<const t_ephGal*>(eph);
89 if (ephGPS) {
90 _ephPool->putEphemeris(new t_ephGPS(*ephGPS));
91 }
92 else if (ephGlo) {
93 _ephPool->putEphemeris(new t_ephGlo(*ephGlo));
94 }
95 else if (ephGal) {
96 _ephPool->putEphemeris(new t_ephGal(*ephGal));
97 }
98}
99
100//
101//////////////////////////////////////////////////////////////////////////////
102void t_pppClient::putOrbCorrections(const vector<t_orbCorr*>& corr) {
103 for (unsigned ii = 0; ii < corr.size(); ii++) {
104 _ephPool->putOrbCorrection(new t_orbCorr(*corr[ii]));
105 }
106}
107
108//
109//////////////////////////////////////////////////////////////////////////////
110void t_pppClient::putClkCorrections(const vector<t_clkCorr*>& corr) {
111 for (unsigned ii = 0; ii < corr.size(); ii++) {
112 _ephPool->putClkCorrection(new t_clkCorr(*corr[ii]));
113 }
114}
115
116//
117//////////////////////////////////////////////////////////////////////////////
118void t_pppClient::putCodeBiases(const vector<t_satCodeBias*>& biases) {
119 for (unsigned ii = 0; ii < biases.size(); ii++) {
120 _obsPool->putCodeBias(new t_satCodeBias(*biases[ii]));
121 }
122}
123
124//
125//////////////////////////////////////////////////////////////////////////////
126t_irc t_pppClient::prepareObs(const vector<t_satObs*>& satObs,
127 vector<t_pppSatObs*>& obsVector, bncTime& epoTime) {
128 // Default
129 // -------
130 epoTime.reset();
131
132 // Create vector of valid observations
133 // -----------------------------------
134 for (unsigned ii = 0; ii < satObs.size(); ii++) {
135 char system = satObs[ii]->_prn.system();
136 if (OPT->useSystem(system)) {
137 t_pppSatObs* pppSatObs = new t_pppSatObs(*satObs[ii]);
138 if (pppSatObs->isValid()) {
139 obsVector.push_back(pppSatObs);
140 }
141 else {
142 delete pppSatObs;
143 }
144 }
145 }
146
147 // Check whether data are synchronized, compute epoTime
148 // ----------------------------------------------------
149 const double MAXSYNC = 0.05; // synchronization limit
150 double meanDt = 0.0;
151 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
152 const t_pppSatObs* satObs = obsVector.at(ii);
153 if (epoTime.undef()) {
154 epoTime = satObs->time();
155 }
156 else {
157 double dt = satObs->time() - epoTime;
158 if (fabs(dt) > MAXSYNC) {
159 LOG << "t_pppClient::prepareObs asynchronous observations" << endl;
160 return failure;
161 }
162 meanDt += dt;
163 }
164 }
165
166 if (obsVector.size() > 0) {
167 epoTime += meanDt / obsVector.size();
168 }
169
170 return success;
171}
172
173// Compute the Bancroft position, check for blunders
174//////////////////////////////////////////////////////////////////////////////
175t_irc t_pppClient::cmpBancroft(const bncTime& epoTime,
176 vector<t_pppSatObs*>& obsVector,
177 ColumnVector& xyzc, bool print) {
178
179 t_lc::type tLC = t_lc::dummy;
180
181 while (true) {
182 Matrix BB(obsVector.size(), 4);
183 int iObs = -1;
184 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
185 const t_pppSatObs* satObs = obsVector.at(ii);
186 if (satObs->prn().system() == 'G') {
187 if (tLC == t_lc::dummy) {
188 tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
189 }
190 if ( satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle) ) {
191 ++iObs;
192 BB[iObs][0] = satObs->xc()[0];
193 BB[iObs][1] = satObs->xc()[1];
194 BB[iObs][2] = satObs->xc()[2];
195 BB[iObs][3] = satObs->obsValue(tLC) - satObs->cmpValueForBanc(tLC);
196 }
197 }
198 }
199 if (iObs + 1 < OPT->_minObs) {
200 LOG << "t_pppClient::cmpBancroft not enough observations" << endl;
201 return failure;
202 }
203 BB = BB.Rows(1,iObs+1);
204 bancroft(BB, xyzc);
205
206 xyzc[3] /= t_CST::c;
207
208 // Check Blunders
209 // --------------
210 const double BLUNDER = 100.0;
211 double maxRes = 0.0;
212 unsigned maxResIndex = 0;
213 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
214 const t_pppSatObs* satObs = obsVector.at(ii);
215 if ( satObs->isValid() && satObs->prn().system() == 'G' &&
216 (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle) ) {
217 ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3);
218 double res = rr.norm_Frobenius() - satObs->obsValue(tLC)
219 - (satObs->xc()[3] - xyzc[3]) * t_CST::c;
220 if (fabs(res) > maxRes) {
221 maxRes = fabs(res);
222 maxResIndex = ii;
223 }
224 }
225 }
226 if (maxRes < BLUNDER) {
227 if (print) {
228 LOG.setf(ios::fixed);
229 LOG << string(epoTime) << " BANCROFT:" << ' '
230 << setw(14) << setprecision(3) << xyzc[0] << ' '
231 << setw(14) << setprecision(3) << xyzc[1] << ' '
232 << setw(14) << setprecision(3) << xyzc[2] << ' '
233 << setw(14) << setprecision(3) << xyzc[3] * t_CST::c << endl << endl;
234 }
235 break;
236 }
237 else {
238 t_pppSatObs* satObs = obsVector.at(maxResIndex);
239 LOG << "t_pppClient::cmpBancroft outlier " << satObs->prn().toString()
240 << " " << maxRes << endl;
241 delete satObs;
242 obsVector.erase(obsVector.begin() + maxResIndex);
243 }
244 }
245
246 return success;
247}
248
249// Compute A Priori GPS-Glonass Offset
250//////////////////////////////////////////////////////////////////////////////
251double t_pppClient::cmpOffGG(vector<t_pppSatObs*>& obsVector) {
252
253 t_lc::type tLC = t_lc::dummy;
254 double offGG = 0.0;
255
256 if (OPT->useSystem('R')) {
257
258 while (obsVector.size() > 0) {
259 offGG = 0.0;
260 double maxRes = 0.0;
261 int maxResIndex = -1;
262 t_prn maxResPrn;
263 unsigned nObs = 0;
264 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
265 t_pppSatObs* satObs = obsVector.at(ii);
266 if (satObs->prn().system() == 'R') {
267 if (tLC == t_lc::dummy) {
268 tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
269 }
270 if (satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle)) {
271 double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
272 ++nObs;
273 offGG += ll;
274 if (fabs(ll) > fabs(maxRes)) {
275 maxRes = ll;
276 maxResIndex = ii;
277 maxResPrn = satObs->prn();
278 }
279 }
280 }
281 }
282
283 if (nObs > 0) {
284 offGG = offGG / nObs;
285 }
286 else {
287 offGG = 0.0;
288 }
289
290 if (fabs(maxRes) > 1000.0) {
291 LOG << "t_pppClient::cmpOffGG outlier " << maxResPrn.toString() << " " << maxRes << endl;
292 obsVector.erase(obsVector.begin() + maxResIndex);
293 }
294 else {
295 break;
296 }
297 }
298 }
299
300 return offGG;
301}
302
303//
304//////////////////////////////////////////////////////////////////////////////
305void t_pppClient::initOutput(t_output* output) {
306 _output = output;
307 _output->_numSat = 0;
308 _output->_pDop = 0.0;
309 _output->_error = false;
310}
311
312//
313//////////////////////////////////////////////////////////////////////////////
314void t_pppClient::clearObs() {
315 for (unsigned ii = 0; ii < _obsRover.size(); ii++) {
316 delete _obsRover.at(ii);
317 }
318 _obsRover.clear();
319}
320
321//
322//////////////////////////////////////////////////////////////////////////////
323void t_pppClient::finish(t_irc irc) {
324
325 clearObs();
326
327 _output->_epoTime = _epoTimeRover;
328
329 if (irc == success) {
330 _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0];
331 _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1];
332 _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2];
333
334 xyz2neu(_staRover->ellApr().data(), _filter->x().data(), _output->_neu);
335
336 copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix);
337
338 _output->_trp0 = t_tropo::delay_saast(_staRover->xyzApr(), M_PI/2.0);
339 _output->_trp = _filter->trp();
340 _output->_trpStdev = _filter->trpStdev();
341
342 _output->_numSat = _filter->numSat();
343 _output->_pDop = _filter->PDOP();
344 _output->_error = false;
345 }
346 else {
347 _output->_error = true;
348 }
349 _output->_log = _log->str();
350 delete _log; _log = new ostringstream();
351}
352
353//
354//////////////////////////////////////////////////////////////////////////////
355t_irc t_pppClient::cmpModel(t_pppStation* station, const ColumnVector& xyzc,
356 vector<t_pppSatObs*>& obsVector) {
357
358 bncTime time;
359 time = _epoTimeRover;
360 station->setName(OPT->_roverName);
361 station->setAntName(OPT->_antNameRover);
362 if (OPT->xyzAprRoverSet()) {
363 station->setXyzApr(OPT->_xyzAprRover);
364 }
365 else {
366 station->setXyzApr(xyzc.Rows(1,3));
367 }
368 station->setNeuEcc(OPT->_neuEccRover);
369
370 // Receiver Clock
371 // --------------
372 station->setDClk(xyzc[3]);
373
374 // Tides
375 // -----
376 station->setTideDspl( _tides->displacement(time, station->xyzApr()) );
377
378 // Observation model
379 // -----------------
380 vector<t_pppSatObs*>::iterator it = obsVector.begin();
381 while (it != obsVector.end()) {
382 t_pppSatObs* satObs = *it;
383 if (satObs->isValid()) {
384 satObs->cmpModel(station);
385 }
386 if (satObs->isValid() && satObs->eleSat() >= OPT->_minEle) {
387 ++it;
388 }
389 else {
390 delete satObs;
391 it = obsVector.erase(it);
392 }
393 }
394
395 return success;
396}
397
398//
399//////////////////////////////////////////////////////////////////////////////
400void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
401
402 try {
403 initOutput(output);
404
405 // Prepare Observations of the Rover
406 // ---------------------------------
407 if (prepareObs(satObs, _obsRover, _epoTimeRover) != success) {
408 return finish(failure);
409 }
410
411 LOG << "\nResults of Epoch ";
412 if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
413 LOG << "\n--------------------------------------\n";
414
415 for (int iter = 1; iter <= 2; iter++) {
416 ColumnVector xyzc(4); xyzc = 0.0;
417 bool print = (iter == 2);
418 if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != success) {
419 return finish(failure);
420 }
421 if (cmpModel(_staRover, xyzc, _obsRover) != success) {
422 return finish(failure);
423 }
424 }
425
426 _offGG = cmpOffGG(_obsRover);
427
428 // Store last epoch of data
429 // ------------------------
430 _obsPool->putEpoch(_epoTimeRover, _obsRover);
431
432 // Process Epoch in Filter
433 // -----------------------
434 if (_filter->processEpoch(_obsPool) != success) {
435 return finish(failure);
436 }
437 }
438 catch (Exception& exc) {
439 LOG << exc.what() << endl;
440 return finish(failure);
441 }
442 catch (t_except msg) {
443 LOG << msg.what() << endl;
444 return finish(failure);
445 }
446 catch (const char* msg) {
447 LOG << msg << endl;
448 return finish(failure);
449 }
450 catch (...) {
451 LOG << "unknown exception" << endl;
452 return finish(failure);
453 }
454
455 return finish(success);
456}
457
458//
459////////////////////////////////////////////////////////////////////////////
460double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
461 return aa[0]*bb[0] + aa[1]*bb[1] + aa[2]*bb[2] - aa[3]*bb[3];
462}
463
464//
465////////////////////////////////////////////////////////////////////////////
466void t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) {
467
468 if (pos.Nrows() != 4) {
469 pos.ReSize(4);
470 }
471 pos = 0.0;
472
473 for (int iter = 1; iter <= 2; iter++) {
474 Matrix BB = BBpass;
475 int mm = BB.Nrows();
476 for (int ii = 1; ii <= mm; ii++) {
477 double xx = BB(ii,1);
478 double yy = BB(ii,2);
479 double traveltime = 0.072;
480 if (iter > 1) {
481 double zz = BB(ii,3);
482 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
483 (yy-pos(2)) * (yy-pos(2)) +
484 (zz-pos(3)) * (zz-pos(3)) );
485 traveltime = rho / t_CST::c;
486 }
487 double angle = traveltime * t_CST::omega;
488 double cosa = cos(angle);
489 double sina = sin(angle);
490 BB(ii,1) = cosa * xx + sina * yy;
491 BB(ii,2) = -sina * xx + cosa * yy;
492 }
493
494 Matrix BBB;
495 if (mm > 4) {
496 SymmetricMatrix hlp; hlp << BB.t() * BB;
497 BBB = hlp.i() * BB.t();
498 }
499 else {
500 BBB = BB.i();
501 }
502 ColumnVector ee(mm); ee = 1.0;
503 ColumnVector alpha(mm); alpha = 0.0;
504 for (int ii = 1; ii <= mm; ii++) {
505 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
506 }
507 ColumnVector BBBe = BBB * ee;
508 ColumnVector BBBalpha = BBB * alpha;
509 double aa = lorentz(BBBe, BBBe);
510 double bb = lorentz(BBBe, BBBalpha)-1;
511 double cc = lorentz(BBBalpha, BBBalpha);
512 double root = sqrt(bb*bb-aa*cc);
513
514 Matrix hlpPos(4,2);
515 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
516 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
517
518 ColumnVector omc(2);
519 for (int pp = 1; pp <= 2; pp++) {
520 hlpPos(4,pp) = -hlpPos(4,pp);
521 omc(pp) = BB(1,4) -
522 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
523 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
524 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
525 hlpPos(4,pp);
526 }
527 if ( fabs(omc(1)) > fabs(omc(2)) ) {
528 pos = hlpPos.Column(2);
529 }
530 else {
531 pos = hlpPos.Column(1);
532 }
533 }
534}
535
Note: See TracBrowser for help on using the repository browser.