Index: /trunk/BNC/src/PPP/pppMain.cpp =================================================================== --- /trunk/BNC/src/PPP/pppMain.cpp (revision 5683) +++ /trunk/BNC/src/PPP/pppMain.cpp (revision 5683) @@ -0,0 +1,498 @@ + +#include +#include +#include +#include +#include + +#include "pppMain.h" +#include "ephpool.h" +#include "obspool.h" +#include "satbias.h" +#include "genconst.h" +#include "utils.h" +#include "station.h" +#include "tides.h" +#include "antex.h" +#include "filter.h" + +using namespace BNC; +using namespace std; + +// Global Variable +////////////////////////////////////////////////////////////////////////////// +t_pppMain* pppMain = 0; + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_pppMain::t_pppMain() { + _opt = 0; + _output = 0; + _antex = 0; + _log = new ostringstream(); + _ephPool = new t_ephPool(); + _obsPool = new t_obsPool(); + _staRover = new t_station(e_rover); + _staBase = new t_station(e_base); + _filter = new t_filter(); +} + +// Destructor +////////////////////////////////////////////////////////////////////////////// +t_pppMain::~t_pppMain() { + delete _log; + delete _opt; + delete _ephPool; + delete _obsPool; + delete _staRover; + delete _staBase; + delete _antex; + delete _filter; + clearObs(); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::setOptions(const t_pppOpt* opt) { + delete _opt; + _opt = new t_options(opt); + + delete _antex; _antex = 0; + if (!_opt->antexFileName().empty()) { + _antex = new t_antex(_opt->antexFileName()); + } + + _opt->print(); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::putGPSEphemeris(const t_ephGPS* eph) { + _ephPool->putEphemeris(new t_ephGPS(*eph)); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::putGloEphemeris(const t_ephGlo* eph) { + _ephPool->putEphemeris(new t_ephGlo(*eph)); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::putOrbCorrections(int numCorr, const t_pppOrbCorr* corr) { + for (int ii = 0; ii < numCorr; ii++) { + _ephPool->putOrbCorrection(new t_orbCorr(corr[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::putClkCorrections(int numCorr, const t_pppClkCorr* corr) { + for (int ii = 0; ii < numCorr; ii++) { + _ephPool->putClkCorrection(new t_clkCorr(corr[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::putBiases(int numBiases, const t_pppSatBiases* biases) { + for (int ii = 0; ii < numBiases; ii++) { + _obsPool->putBiases(new t_satBias(biases[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +t_irc::irc t_pppMain::prepareObs(int numSat, const t_pppSatObs* satObs, + vector& obsVector, t_time& epoTime) { + // Default + // ------- + epoTime.reset(); + + // Create vector of valid observations + // ----------------------------------- + int numValidGPS = 0; + for (int ii = 0; ii < numSat; ii++) { + char system = satObs[ii]._satellite._system; + if (system == 'G' || (system == 'R' && OPT->useGlonass())) { + t_satObs* satObs = new t_satObs(satObs[ii]); + if (satObs->isValid()) { + obsVector.push_back(satObs); + if (satObs->prn().system() == 'G') { + ++numValidGPS; + } + } + else { + delete satObs; + } + } + } + + // Check whether data are synchronized, compute epoTime + // ---------------------------------------------------- + const double MAXSYNC = 0.05; // synchronization limit + double meanDt = 0.0; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + const t_satObs* satObs = obsVector.at(ii); + if (epoTime.undef()) { + epoTime = satObs->time(); + } + else { + double dt = satObs->time() - epoTime; + if (fabs(dt) > MAXSYNC) { + LOG << "t_pppMain::prepareObs asynchronous observations" << endl; + return t_irc::failure; + } + meanDt += dt; + } + } + epoTime += meanDt / obsVector.size(); + + return t_irc::success; +} + +// Compute the Bancroft position, check for blunders +////////////////////////////////////////////////////////////////////////////// +t_irc::irc t_pppMain::cmpBancroft(const t_time& epoTime, + vector& obsVector, + ColumnVector& xyzc, bool print) { + + t_lc::type tLC = (OPT->dualFreqRequired() ? t_lc::cIF : t_lc::c1); + + while (true) { + Matrix BB(obsVector.size(), 4); + int iObs = -1; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + const t_satObs* satObs = obsVector.at(ii); + if ( satObs->isValid() && satObs->prn().system() == 'G' && + (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) { + ++iObs; + BB[iObs][0] = satObs->xc()[0]; + BB[iObs][1] = satObs->xc()[1]; + BB[iObs][2] = satObs->xc()[2]; + BB[iObs][3] = satObs->obsValue(tLC) - satObs->cmpValueForBanc(tLC); + } + } + if (iObs + 1 < OPT->minobs()) { + LOG << "t_pppMain::cmpBancroft not enough observations" << endl; + return t_irc::failure; + } + BB = BB.Rows(1,iObs+1); + bancroft(BB, xyzc); + + xyzc[3] /= t_genConst::c; + + // Check Blunders + // -------------- + const double BLUNDER = 100.0; + double maxRes = 0.0; + unsigned maxResIndex = 0; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + const t_satObs* satObs = obsVector.at(ii); + if ( satObs->isValid() && satObs->prn().system() == 'G' && + (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) { + ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3); + double res = rr.norm_Frobenius() - satObs->obsValue(tLC) + - (satObs->xc()[3] - xyzc[3]) * t_genConst::c; + if (fabs(res) > maxRes) { + maxRes = fabs(res); + maxResIndex = ii; + } + } + } + if (maxRes < BLUNDER) { + if (print) { + LOG.setf(ios::fixed); + LOG << string(epoTime) << " BANCROFT:" << ' ' + << setw(14) << setprecision(3) << xyzc[0] << ' ' + << setw(14) << setprecision(3) << xyzc[1] << ' ' + << setw(14) << setprecision(3) << xyzc[2] << ' ' + << setw(14) << setprecision(3) << xyzc[3] * t_genConst::c << endl << endl; + } + break; + } + else { + t_satObs* satObs = obsVector.at(maxResIndex); + LOG << "t_pppMain::cmpBancroft outlier " << satObs->prn().toString() + << " " << maxRes << endl; + delete satObs; + obsVector.erase(obsVector.begin() + maxResIndex); + } + } + + return t_irc::success; +} + +// Compute A Priori GPS-Glonass Offset +////////////////////////////////////////////////////////////////////////////// +double t_pppMain::cmpOffGG(vector& obsVector) { + + t_lc::type tLC = (OPT->dualFreqRequired() ? t_lc::cIF : t_lc::c1); + double offGG = 0.0; + + if (OPT->useGlonass()) { + while (true) { + offGG = 0.0; + bool outlierFound = false; + unsigned nObs = 0; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + t_satObs* satObs = obsVector.at(ii); + if ( !satObs->outlier() && satObs->isValid() && satObs->prn().system() == 'R' && + (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) { + ++nObs; + double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC); + if (fabs(ll) > 1000.0) { + satObs->setOutlier(); + outlierFound = true; + LOG << "t_pppMain::cmpOffGG outlier " << satObs->prn().toString() + << " " << ll << endl; + } + offGG += ll; + } + } + if (nObs > 0) { + offGG = offGG / nObs; + } + else { + offGG = 0.0; + } + if (!outlierFound) { + break; + } + } + } + + return offGG; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::initOutput(t_pppOutput* output) { + _output = output; + _output->_epoTime._mjd = 0; + _output->_epoTime._sec = 0.0; + _output->_numSat = 0; + _output->_pDop = 0.0; + _output->_ambFixRate = 0.0; + _output->_log = 0; + _output->_error = false; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::clearObs() { + for (unsigned ii = 0; ii < _obsRover.size(); ii++) { + delete _obsRover.at(ii); + } + _obsRover.clear(); + for (unsigned ii = 0; ii < _obsBase.size(); ii++) { + delete _obsBase.at(ii); + } + _obsBase.clear(); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::finish(t_irc::irc irc) { + + clearObs(); + + _output->_epoTime._mjd = _epoTimeRover.mjd(); + _output->_epoTime._sec = _epoTimeRover.daysec(); + + if (irc == t_irc::success) { + _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0]; + _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1]; + _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2]; + copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix); + _output->_numSat = _filter->numSat(); + _output->_pDop = _filter->PDOP(); + _output->_ambFixRate = _filter->ambFixRate(); + _output->_error = false; + } + else { + _output->_error = true; + } + if (OPT->logLevel() > 0) { + _output->_log = strdup(_log->str().c_str()); + } + delete _log; _log = new ostringstream(); +} + +// +////////////////////////////////////////////////////////////////////////////// +t_irc::irc t_pppMain::cmpModel(t_station* station, const ColumnVector& xyzc, + vector& obsVector) { + + t_time time; + if (station->roverBase() == e_rover) { + time = _epoTimeRover; + station->setName(OPT->roverName()); + station->setAntName(OPT->antNameRover()); + if (OPT->xyzAprRoverSet()) { + station->setXyzApr(OPT->xyzAprRover()); + } + else { + station->setXyzApr(xyzc.Rows(1,3)); + } + station->setNeuEcc(OPT->neuEccRover()); + } + else { + time = _epoTimeBase; + station->setName(OPT->baseName()); + station->setAntName(OPT->antNameBase()); + station->setXyzApr(OPT->xyzAprBase()); + station->setNeuEcc(OPT->neuEccBase()); + } + + // Receiver Clock + // -------------- + station->setDClk(xyzc[3]); + + // US Restriction + // -------------- + station->checkRestriction(time); + + // Tides + // ----- + station->setTideDspl(t_tides::displacement(time, station->xyzApr())); + + // Observation model + // ----------------- + vector::iterator it = obsVector.begin(); + while (it != obsVector.end()) { + t_satObs* satObs = *it; + satObs->cmpModel(station); + if (satObs->isValid() && satObs->eleSat() >= OPT->minEle()) { + ++it; + } + else { + delete satObs; + it = obsVector.erase(it); + } + } + + return t_irc::success; +} + +// +////////////////////////////////////////////////////////////////////////////// +t_irc::irc t_pppMain::createDifferences() { + + vector::iterator it = _obsRover.begin(); + while (it != _obsRover.end()) { + t_satObs* obsR = *it; + t_satObs* obsB = 0; + for (unsigned ii = 0; ii < _obsBase.size(); ii++) { + if (_obsBase[ii]->prn() == obsR->prn()) { + obsB = _obsBase[ii]; + break; + } + } + if (obsB) { + obsR->createDifference(obsB); + ++it; + } + else { + delete obsR; + it = _obsRover.erase(it); + } + } + + return t_irc::success; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppMain::processEpoch(int numSatRover, const t_pppSatObs* satObsRover, + t_pppOutput* output) { + + try { + initOutput(output); + + // Prepare Observations of the Rover + // --------------------------------- + if (prepareObs(numSatRover, satObsRover, _obsRover, + _epoTimeRover) != t_irc::success) { + return finish(t_irc::failure); + } + + LOG << "\nResults of Epoch "; + if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover); + LOG << "\n--------------------------------------\n"; + + for (int iter = 1; iter <= 2; iter++) { + ColumnVector xyzc(4); xyzc = 0.0; + bool print = (iter == 2); + if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != t_irc::success) { + return finish(t_irc::failure); + } + if (cmpModel(_staRover, xyzc, _obsRover) != t_irc::success) { + return finish(t_irc::failure); + } + } + + _offGG = cmpOffGG(_obsRover); + + // Prepare Observations of the Base + // -------------------------------- + if (OPT->isDifferential()) { + if (prepareObs(numSatBase, satObsBase, _obsBase, + _epoTimeBase) != t_irc::success) { + return finish(t_irc::failure); + } + ColumnVector xyzc(4); xyzc = 0.0; + if (cmpModel(_staBase, xyzc, _obsBase) != t_irc::success) { + return finish(t_irc::failure); + } + if (cmpBancroft(_epoTimeBase, _obsBase, xyzc, true) != t_irc::success) { + return finish(t_irc::failure); + } + _staBase->setDClk(xyzc[3]); + + _offGG -= cmpOffGG(_obsBase); + + // Create single-differences of observations + // ----------------------------------------- + if (createDifferences() != t_irc::success) { + return finish(t_irc::failure); + } + for (unsigned ii = 0; ii < _obsBase.size(); ii++) { + delete _obsBase[ii]; + } + _obsBase.clear(); + } + + // Store last epoch of data + // ------------------------ + _obsPool->putEpoch(_epoTimeRover, _obsRover); + + // Process Epoch in Filter + // ----------------------- + if (_filter->processEpoch(_obsPool) != t_irc::success) { + return finish(t_irc::failure); + } + } + catch (Exception& exc) { + LOG << exc.what() << endl; + return finish(t_irc::failure); + } + catch (string& msg) { + LOG << "Exception: " << msg << endl; + return finish(t_irc::failure); + } + catch (logic_error exc) { + LOG << exc.what() << endl; + return finish(t_irc::failure); + } + catch (const char* msg) { + LOG << msg << endl; + return finish(t_irc::failure); + } + catch (...) { + LOG << "unknown exception" << endl; + return finish(t_irc::failure); + } + + return finish(t_irc::success); +}