source: ntrip/trunk/BNC/src/PPP/ppp.cpp@ 5710

Last change on this file since 5710 was 5710, checked in by mervart, 10 years ago
File size: 14.0 KB
Line 
1
2#include <iostream>
3#include <iomanip>
4#include <stdlib.h>
5#include <string.h>
6#include <stdexcept>
7
8#include "pppMain.h"
9#include "ephpool.h"
10#include "obspool.h"
11#include "satbias.h"
12#include "genconst.h"
13#include "utils.h"
14#include "station.h"
15#include "tides.h"
16#include "antex.h"
17#include "filter.h"
18
19using namespace BNC;
20using namespace std;
21
22// Global Variable
23//////////////////////////////////////////////////////////////////////////////
24t_pppMain* pppMain = 0;
25
26// Constructor
27//////////////////////////////////////////////////////////////////////////////
28t_pppMain::t_pppMain() {
29 _opt = 0;
30 _output = 0;
31 _antex = 0;
32 _log = new ostringstream();
33 _ephPool = new t_ephPool();
34 _obsPool = new t_obsPool();
35 _staRover = new t_station(e_rover);
36 _staBase = new t_station(e_base);
37 _filter = new t_filter();
38}
39
40// Destructor
41//////////////////////////////////////////////////////////////////////////////
42t_pppMain::~t_pppMain() {
43 delete _log;
44 delete _opt;
45 delete _ephPool;
46 delete _obsPool;
47 delete _staRover;
48 delete _staBase;
49 delete _antex;
50 delete _filter;
51 clearObs();
52}
53
54//
55//////////////////////////////////////////////////////////////////////////////
56void t_pppMain::setOptions(const t_pppOpt* opt) {
57 delete _opt;
58 _opt = new t_options(opt);
59
60 delete _antex; _antex = 0;
61 if (!_opt->antexFileName().empty()) {
62 _antex = new t_antex(_opt->antexFileName());
63 }
64
65 _opt->print();
66}
67
68//
69//////////////////////////////////////////////////////////////////////////////
70void t_pppMain::putGPSEphemeris(const t_ephGPS* eph) {
71 _ephPool->putEphemeris(new t_ephGPS(*eph));
72}
73
74//
75//////////////////////////////////////////////////////////////////////////////
76void t_pppMain::putGloEphemeris(const t_ephGlo* eph) {
77 _ephPool->putEphemeris(new t_ephGlo(*eph));
78}
79
80//
81//////////////////////////////////////////////////////////////////////////////
82void t_pppMain::putOrbCorrections(int numCorr, const t_pppOrbCorr* corr) {
83 for (int ii = 0; ii < numCorr; ii++) {
84 _ephPool->putOrbCorrection(new t_orbCorr(corr[ii]));
85 }
86}
87
88//
89//////////////////////////////////////////////////////////////////////////////
90void t_pppMain::putClkCorrections(int numCorr, const t_pppClkCorr* corr) {
91 for (int ii = 0; ii < numCorr; ii++) {
92 _ephPool->putClkCorrection(new t_clkCorr(corr[ii]));
93 }
94}
95
96//
97//////////////////////////////////////////////////////////////////////////////
98void t_pppMain::putBiases(int numBiases, const t_pppSatBiases* biases) {
99 for (int ii = 0; ii < numBiases; ii++) {
100 _obsPool->putBiases(new t_satBias(biases[ii]));
101 }
102}
103
104//
105//////////////////////////////////////////////////////////////////////////////
106t_irc::irc t_pppMain::prepareObs(int numSat, const t_pppSatObs* satObs,
107 vector<t_satObs*>& obsVector, t_time& epoTime) {
108 // Default
109 // -------
110 epoTime.reset();
111
112 // Create vector of valid observations
113 // -----------------------------------
114 int numValidGPS = 0;
115 for (int ii = 0; ii < numSat; ii++) {
116 char system = satObs[ii]._satellite._system;
117 if (system == 'G' || (system == 'R' && OPT->useGlonass())) {
118 t_satObs* satObs = new t_satObs(satObs[ii]);
119 if (satObs->isValid()) {
120 obsVector.push_back(satObs);
121 if (satObs->prn().system() == 'G') {
122 ++numValidGPS;
123 }
124 }
125 else {
126 delete satObs;
127 }
128 }
129 }
130
131 // Check whether data are synchronized, compute epoTime
132 // ----------------------------------------------------
133 const double MAXSYNC = 0.05; // synchronization limit
134 double meanDt = 0.0;
135 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
136 const t_satObs* satObs = obsVector.at(ii);
137 if (epoTime.undef()) {
138 epoTime = satObs->time();
139 }
140 else {
141 double dt = satObs->time() - epoTime;
142 if (fabs(dt) > MAXSYNC) {
143 LOG << "t_pppMain::prepareObs asynchronous observations" << endl;
144 return t_irc::failure;
145 }
146 meanDt += dt;
147 }
148 }
149 epoTime += meanDt / obsVector.size();
150
151 return t_irc::success;
152}
153
154// Compute the Bancroft position, check for blunders
155//////////////////////////////////////////////////////////////////////////////
156t_irc::irc t_pppMain::cmpBancroft(const t_time& epoTime,
157 vector<t_satObs*>& obsVector,
158 ColumnVector& xyzc, bool print) {
159
160 t_lc::type tLC = (OPT->dualFreqRequired() ? t_lc::cIF : t_lc::c1);
161
162 while (true) {
163 Matrix BB(obsVector.size(), 4);
164 int iObs = -1;
165 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
166 const t_satObs* satObs = obsVector.at(ii);
167 if ( satObs->isValid() && satObs->prn().system() == 'G' &&
168 (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) {
169 ++iObs;
170 BB[iObs][0] = satObs->xc()[0];
171 BB[iObs][1] = satObs->xc()[1];
172 BB[iObs][2] = satObs->xc()[2];
173 BB[iObs][3] = satObs->obsValue(tLC) - satObs->cmpValueForBanc(tLC);
174 }
175 }
176 if (iObs + 1 < OPT->minobs()) {
177 LOG << "t_pppMain::cmpBancroft not enough observations" << endl;
178 return t_irc::failure;
179 }
180 BB = BB.Rows(1,iObs+1);
181 bancroft(BB, xyzc);
182
183 xyzc[3] /= t_genConst::c;
184
185 // Check Blunders
186 // --------------
187 const double BLUNDER = 100.0;
188 double maxRes = 0.0;
189 unsigned maxResIndex = 0;
190 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
191 const t_satObs* satObs = obsVector.at(ii);
192 if ( satObs->isValid() && satObs->prn().system() == 'G' &&
193 (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) {
194 ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3);
195 double res = rr.norm_Frobenius() - satObs->obsValue(tLC)
196 - (satObs->xc()[3] - xyzc[3]) * t_genConst::c;
197 if (fabs(res) > maxRes) {
198 maxRes = fabs(res);
199 maxResIndex = ii;
200 }
201 }
202 }
203 if (maxRes < BLUNDER) {
204 if (print) {
205 LOG.setf(ios::fixed);
206 LOG << string(epoTime) << " BANCROFT:" << ' '
207 << setw(14) << setprecision(3) << xyzc[0] << ' '
208 << setw(14) << setprecision(3) << xyzc[1] << ' '
209 << setw(14) << setprecision(3) << xyzc[2] << ' '
210 << setw(14) << setprecision(3) << xyzc[3] * t_genConst::c << endl << endl;
211 }
212 break;
213 }
214 else {
215 t_satObs* satObs = obsVector.at(maxResIndex);
216 LOG << "t_pppMain::cmpBancroft outlier " << satObs->prn().toString()
217 << " " << maxRes << endl;
218 delete satObs;
219 obsVector.erase(obsVector.begin() + maxResIndex);
220 }
221 }
222
223 return t_irc::success;
224}
225
226// Compute A Priori GPS-Glonass Offset
227//////////////////////////////////////////////////////////////////////////////
228double t_pppMain::cmpOffGG(vector<t_satObs*>& obsVector) {
229
230 t_lc::type tLC = (OPT->dualFreqRequired() ? t_lc::cIF : t_lc::c1);
231 double offGG = 0.0;
232
233 if (OPT->useGlonass()) {
234 while (true) {
235 offGG = 0.0;
236 bool outlierFound = false;
237 unsigned nObs = 0;
238 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
239 t_satObs* satObs = obsVector.at(ii);
240 if ( !satObs->outlier() && satObs->isValid() && satObs->prn().system() == 'R' &&
241 (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) {
242 ++nObs;
243 double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
244 if (fabs(ll) > 1000.0) {
245 satObs->setOutlier();
246 outlierFound = true;
247 LOG << "t_pppMain::cmpOffGG outlier " << satObs->prn().toString()
248 << " " << ll << endl;
249 }
250 offGG += ll;
251 }
252 }
253 if (nObs > 0) {
254 offGG = offGG / nObs;
255 }
256 else {
257 offGG = 0.0;
258 }
259 if (!outlierFound) {
260 break;
261 }
262 }
263 }
264
265 return offGG;
266}
267
268//
269//////////////////////////////////////////////////////////////////////////////
270void t_pppMain::initOutput(t_pppOutput* output) {
271 _output = output;
272 _output->_epoTime._mjd = 0;
273 _output->_epoTime._sec = 0.0;
274 _output->_numSat = 0;
275 _output->_pDop = 0.0;
276 _output->_ambFixRate = 0.0;
277 _output->_log = 0;
278 _output->_error = false;
279}
280
281//
282//////////////////////////////////////////////////////////////////////////////
283void t_pppMain::clearObs() {
284 for (unsigned ii = 0; ii < _obsRover.size(); ii++) {
285 delete _obsRover.at(ii);
286 }
287 _obsRover.clear();
288 for (unsigned ii = 0; ii < _obsBase.size(); ii++) {
289 delete _obsBase.at(ii);
290 }
291 _obsBase.clear();
292}
293
294//
295//////////////////////////////////////////////////////////////////////////////
296void t_pppMain::finish(t_irc::irc irc) {
297
298 clearObs();
299
300 _output->_epoTime._mjd = _epoTimeRover.mjd();
301 _output->_epoTime._sec = _epoTimeRover.daysec();
302
303 if (irc == t_irc::success) {
304 _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0];
305 _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1];
306 _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2];
307 copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix);
308 _output->_numSat = _filter->numSat();
309 _output->_pDop = _filter->PDOP();
310 _output->_ambFixRate = _filter->ambFixRate();
311 _output->_error = false;
312 }
313 else {
314 _output->_error = true;
315 }
316 if (OPT->logLevel() > 0) {
317 _output->_log = strdup(_log->str().c_str());
318 }
319 delete _log; _log = new ostringstream();
320}
321
322//
323//////////////////////////////////////////////////////////////////////////////
324t_irc::irc t_pppMain::cmpModel(t_station* station, const ColumnVector& xyzc,
325 vector<t_satObs*>& obsVector) {
326
327 t_time time;
328 if (station->roverBase() == e_rover) {
329 time = _epoTimeRover;
330 station->setName(OPT->roverName());
331 station->setAntName(OPT->antNameRover());
332 if (OPT->xyzAprRoverSet()) {
333 station->setXyzApr(OPT->xyzAprRover());
334 }
335 else {
336 station->setXyzApr(xyzc.Rows(1,3));
337 }
338 station->setNeuEcc(OPT->neuEccRover());
339 }
340 else {
341 time = _epoTimeBase;
342 station->setName(OPT->baseName());
343 station->setAntName(OPT->antNameBase());
344 station->setXyzApr(OPT->xyzAprBase());
345 station->setNeuEcc(OPT->neuEccBase());
346 }
347
348 // Receiver Clock
349 // --------------
350 station->setDClk(xyzc[3]);
351
352 // US Restriction
353 // --------------
354 station->checkRestriction(time);
355
356 // Tides
357 // -----
358 station->setTideDspl(t_tides::displacement(time, station->xyzApr()));
359
360 // Observation model
361 // -----------------
362 vector<t_satObs*>::iterator it = obsVector.begin();
363 while (it != obsVector.end()) {
364 t_satObs* satObs = *it;
365 satObs->cmpModel(station);
366 if (satObs->isValid() && satObs->eleSat() >= OPT->minEle()) {
367 ++it;
368 }
369 else {
370 delete satObs;
371 it = obsVector.erase(it);
372 }
373 }
374
375 return t_irc::success;
376}
377
378//
379//////////////////////////////////////////////////////////////////////////////
380t_irc::irc t_pppMain::createDifferences() {
381
382 vector<t_satObs*>::iterator it = _obsRover.begin();
383 while (it != _obsRover.end()) {
384 t_satObs* obsR = *it;
385 t_satObs* obsB = 0;
386 for (unsigned ii = 0; ii < _obsBase.size(); ii++) {
387 if (_obsBase[ii]->prn() == obsR->prn()) {
388 obsB = _obsBase[ii];
389 break;
390 }
391 }
392 if (obsB) {
393 obsR->createDifference(obsB);
394 ++it;
395 }
396 else {
397 delete obsR;
398 it = _obsRover.erase(it);
399 }
400 }
401
402 return t_irc::success;
403}
404
405//
406//////////////////////////////////////////////////////////////////////////////
407void t_pppMain::processEpoch(int numSatRover, const t_pppSatObs* satObsRover,
408 t_pppOutput* output) {
409
410 try {
411 initOutput(output);
412
413 // Prepare Observations of the Rover
414 // ---------------------------------
415 if (prepareObs(numSatRover, satObsRover, _obsRover,
416 _epoTimeRover) != t_irc::success) {
417 return finish(t_irc::failure);
418 }
419
420 LOG << "\nResults of Epoch ";
421 if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
422 LOG << "\n--------------------------------------\n";
423
424 for (int iter = 1; iter <= 2; iter++) {
425 ColumnVector xyzc(4); xyzc = 0.0;
426 bool print = (iter == 2);
427 if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != t_irc::success) {
428 return finish(t_irc::failure);
429 }
430 if (cmpModel(_staRover, xyzc, _obsRover) != t_irc::success) {
431 return finish(t_irc::failure);
432 }
433 }
434
435 _offGG = cmpOffGG(_obsRover);
436
437 // Prepare Observations of the Base
438 // --------------------------------
439 if (OPT->isDifferential()) {
440 if (prepareObs(numSatBase, satObsBase, _obsBase,
441 _epoTimeBase) != t_irc::success) {
442 return finish(t_irc::failure);
443 }
444 ColumnVector xyzc(4); xyzc = 0.0;
445 if (cmpModel(_staBase, xyzc, _obsBase) != t_irc::success) {
446 return finish(t_irc::failure);
447 }
448 if (cmpBancroft(_epoTimeBase, _obsBase, xyzc, true) != t_irc::success) {
449 return finish(t_irc::failure);
450 }
451 _staBase->setDClk(xyzc[3]);
452
453 _offGG -= cmpOffGG(_obsBase);
454
455 // Create single-differences of observations
456 // -----------------------------------------
457 if (createDifferences() != t_irc::success) {
458 return finish(t_irc::failure);
459 }
460 for (unsigned ii = 0; ii < _obsBase.size(); ii++) {
461 delete _obsBase[ii];
462 }
463 _obsBase.clear();
464 }
465
466 // Store last epoch of data
467 // ------------------------
468 _obsPool->putEpoch(_epoTimeRover, _obsRover);
469
470 // Process Epoch in Filter
471 // -----------------------
472 if (_filter->processEpoch(_obsPool) != t_irc::success) {
473 return finish(t_irc::failure);
474 }
475 }
476 catch (Exception& exc) {
477 LOG << exc.what() << endl;
478 return finish(t_irc::failure);
479 }
480 catch (string& msg) {
481 LOG << "Exception: " << msg << endl;
482 return finish(t_irc::failure);
483 }
484 catch (logic_error exc) {
485 LOG << exc.what() << endl;
486 return finish(t_irc::failure);
487 }
488 catch (const char* msg) {
489 LOG << msg << endl;
490 return finish(t_irc::failure);
491 }
492 catch (...) {
493 LOG << "unknown exception" << endl;
494 return finish(t_irc::failure);
495 }
496
497 return finish(t_irc::success);
498}
Note: See TracBrowser for help on using the repository browser.