source: ntrip/trunk/BNC/src/PPP_RTK/pppClient.cpp@ 7232

Last change on this file since 7232 was 7231, checked in by stuerze, 10 years ago

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