Index: trunk/BNC/src/PPP/pppClient.cpp =================================================================== --- trunk/BNC/src/PPP/pppClient.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppClient.cpp (revision 7237) @@ -0,0 +1,548 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppClient + * + * Purpose: PPP Client processing starts here + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include + +#include +#include +#include +#include +#include + +#include "pppClient.h" +#include "pppEphPool.h" +#include "pppObsPool.h" +#include "bncconst.h" +#include "bncutils.h" +#include "pppStation.h" +#include "bncantex.h" +#include "pppFilter.h" + +using namespace BNC_PPP; +using namespace std; + +// Global variable holding thread-specific pointers +////////////////////////////////////////////////////////////////////////////// +QThreadStorage CLIENTS; + +// Static function returning thread-specific pointer +////////////////////////////////////////////////////////////////////////////// +t_pppClient* t_pppClient::instance() { + return CLIENTS.localData(); +} + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_pppClient::t_pppClient(const t_pppOptions* opt) { + _output = 0; + _opt = new t_pppOptions(*opt); + _log = new ostringstream(); + _ephPool = new t_pppEphPool(); + _obsPool = new t_pppObsPool(); + _staRover = new t_pppStation(); + _filter = new t_pppFilter(); + _tides = new t_tides(); + _iono = new t_iono(); + + if (!_opt->_antexFileName.empty()) { + _antex = new bncAntex(_opt->_antexFileName.c_str()); + } + else { + _antex = 0; + } + + CLIENTS.setLocalData(this); // CLIENTS takes ownership over "this" +} + +// Destructor +////////////////////////////////////////////////////////////////////////////// +t_pppClient::~t_pppClient() { + delete _log; + delete _opt; + delete _ephPool; + delete _obsPool; + delete _staRover; + delete _antex; + delete _filter; + delete _tides; + clearObs(); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::putEphemeris(const t_eph* eph) { + const t_ephGPS* ephGPS = dynamic_cast(eph); + const t_ephGlo* ephGlo = dynamic_cast(eph); + const t_ephGal* ephGal = dynamic_cast(eph); + if (ephGPS) { + _ephPool->putEphemeris(new t_ephGPS(*ephGPS)); + } + else if (ephGlo) { + _ephPool->putEphemeris(new t_ephGlo(*ephGlo)); + } + else if (ephGal) { + _ephPool->putEphemeris(new t_ephGal(*ephGal)); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::putTec(const t_vTec* vTec) { + _iono->setTecData(new t_vTec(*vTec)); + //_staRover->putTec(new t_vTec(*vTec)); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::putOrbCorrections(const vector& corr) { + for (unsigned ii = 0; ii < corr.size(); ii++) { + _ephPool->putOrbCorrection(new t_orbCorr(*corr[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::putClkCorrections(const vector& corr) { + for (unsigned ii = 0; ii < corr.size(); ii++) { + _ephPool->putClkCorrection(new t_clkCorr(*corr[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::putCodeBiases(const vector& biases) { + for (unsigned ii = 0; ii < biases.size(); ii++) { + _obsPool->putCodeBias(new t_satCodeBias(*biases[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +t_irc t_pppClient::prepareObs(const vector& satObs, + vector& obsVector, bncTime& epoTime) { + // Default + // ------- + epoTime.reset(); + + // Create vector of valid observations + // ----------------------------------- + for (unsigned ii = 0; ii < satObs.size(); ii++) { + char system = satObs[ii]->_prn.system(); + if (OPT->useSystem(system)) { + t_pppSatObs* pppSatObs = new t_pppSatObs(*satObs[ii]); + if (pppSatObs->isValid()) { + obsVector.push_back(pppSatObs); + } + else { + delete pppSatObs; + } + } + } + + // 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_pppSatObs* satObs = obsVector.at(ii); + if (epoTime.undef()) { + epoTime = satObs->time(); + } + else { + double dt = satObs->time() - epoTime; + if (fabs(dt) > MAXSYNC) { + LOG << "t_pppClient::prepareObs asynchronous observations" << endl; + return failure; + } + meanDt += dt; + } + } + + if (obsVector.size() > 0) { + epoTime += meanDt / obsVector.size(); + } + + return success; +} + +// Compute the Bancroft position, check for blunders +////////////////////////////////////////////////////////////////////////////// +t_irc t_pppClient::cmpBancroft(const bncTime& epoTime, + vector& obsVector, + ColumnVector& xyzc, bool print) { + + t_lc::type tLC = t_lc::dummy; + + while (true) { + Matrix BB(obsVector.size(), 4); + int iObs = -1; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + const t_pppSatObs* satObs = obsVector.at(ii); + if (satObs->prn().system() == 'G') { + if (tLC == t_lc::dummy) { + tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1; + } + if ( satObs->isValid(tLC) && (!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_pppClient::cmpBancroft not enough observations" << endl; + return failure; + } + BB = BB.Rows(1,iObs+1); + bancroft(BB, xyzc); + + xyzc[3] /= t_CST::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_pppSatObs* 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_CST::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_CST::c << endl << endl; + } + break; + } + else { + t_pppSatObs* satObs = obsVector.at(maxResIndex); + LOG << "t_pppClient::cmpBancroft outlier " << satObs->prn().toString() + << " " << maxRes << endl; + delete satObs; + obsVector.erase(obsVector.begin() + maxResIndex); + } + } + + return success; +} + +// Compute A Priori GPS-Glonass Offset +////////////////////////////////////////////////////////////////////////////// +double t_pppClient::cmpOffGG(vector& obsVector) { + + t_lc::type tLC = t_lc::dummy; + double offGG = 0.0; + + if (OPT->useSystem('R')) { + + while (obsVector.size() > 0) { + offGG = 0.0; + double maxRes = 0.0; + int maxResIndex = -1; + t_prn maxResPrn; + unsigned nObs = 0; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + t_pppSatObs* satObs = obsVector.at(ii); + if (satObs->prn().system() == 'R') { + if (tLC == t_lc::dummy) { + tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1; + } + if (satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle)) { + double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC); + ++nObs; + offGG += ll; + if (fabs(ll) > fabs(maxRes)) { + maxRes = ll; + maxResIndex = ii; + maxResPrn = satObs->prn(); + } + } + } + } + + if (nObs > 0) { + offGG = offGG / nObs; + } + else { + offGG = 0.0; + } + + if (fabs(maxRes) > 1000.0) { + LOG << "t_pppClient::cmpOffGG outlier " << maxResPrn.toString() << " " << maxRes << endl; + obsVector.erase(obsVector.begin() + maxResIndex); + } + else { + break; + } + } + } + + return offGG; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::initOutput(t_output* output) { + _output = output; + _output->_numSat = 0; + _output->_pDop = 0.0; + _output->_error = false; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::clearObs() { + for (unsigned ii = 0; ii < _obsRover.size(); ii++) { + delete _obsRover.at(ii); + } + _obsRover.clear(); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::finish(t_irc irc) { + + clearObs(); + + _output->_epoTime = _epoTimeRover; + + if (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]; + + xyz2neu(_staRover->ellApr().data(), _filter->x().data(), _output->_neu); + + copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix); + + _output->_trp0 = t_tropo::delay_saast(_staRover->xyzApr(), M_PI/2.0); + _output->_trp = _filter->trp(); + _output->_trpStdev = _filter->trpStdev(); + + _output->_numSat = _filter->numSat(); + _output->_pDop = _filter->PDOP(); + _output->_error = false; + } + else { + _output->_error = true; + } + _output->_log = _log->str(); + delete _log; _log = new ostringstream(); +} + +// +////////////////////////////////////////////////////////////////////////////// +t_irc t_pppClient::cmpModel(t_pppStation* station, const ColumnVector& xyzc, + vector& obsVector) { + + bncTime time; + 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); + + // Receiver Clock + // -------------- + station->setDClk(xyzc[3]); + + // Tides + // ----- + station->setTideDspl( _tides->displacement(time, station->xyzApr()) ); + + + // Ionosphere + // ---------- + station->setVtec(_iono->vtec()); + + // Observation model + // ----------------- + vector::iterator it = obsVector.begin(); + while (it != obsVector.end()) { + t_pppSatObs* satObs = *it; + if (satObs->isValid()) { + satObs->cmpModel(station); + } + if (satObs->isValid() && satObs->eleSat() >= OPT->_minEle) { + ++it; + } + else { + delete satObs; + it = obsVector.erase(it); + } + } + + return success; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::processEpoch(const vector& satObs, t_output* output) { + + try { + initOutput(output); + + // Prepare Observations of the Rover + // --------------------------------- + if (prepareObs(satObs, _obsRover, _epoTimeRover) != success) { + return finish(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) != success) { + return finish(failure); + } + if (cmpModel(_staRover, xyzc, _obsRover) != success) { + return finish(failure); + } + } + + _offGG = cmpOffGG(_obsRover); + + // Store last epoch of data + // ------------------------ + _obsPool->putEpoch(_epoTimeRover, _obsRover); + + // Process Epoch in Filter + // ----------------------- + if (_filter->processEpoch(_obsPool) != success) { + return finish(failure); + } + } + catch (Exception& exc) { + LOG << exc.what() << endl; + return finish(failure); + } + catch (t_except msg) { + LOG << msg.what() << endl; + return finish(failure); + } + catch (const char* msg) { + LOG << msg << endl; + return finish(failure); + } + catch (...) { + LOG << "unknown exception" << endl; + return finish(failure); + } + + return finish(success); +} + +// +//////////////////////////////////////////////////////////////////////////// +double lorentz(const ColumnVector& aa, const ColumnVector& bb) { + return aa[0]*bb[0] + aa[1]*bb[1] + aa[2]*bb[2] - aa[3]*bb[3]; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) { + + if (pos.Nrows() != 4) { + pos.ReSize(4); + } + pos = 0.0; + + for (int iter = 1; iter <= 2; iter++) { + Matrix BB = BBpass; + int mm = BB.Nrows(); + for (int ii = 1; ii <= mm; ii++) { + double xx = BB(ii,1); + double yy = BB(ii,2); + double traveltime = 0.072; + if (iter > 1) { + double zz = BB(ii,3); + double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) + + (yy-pos(2)) * (yy-pos(2)) + + (zz-pos(3)) * (zz-pos(3)) ); + traveltime = rho / t_CST::c; + } + double angle = traveltime * t_CST::omega; + double cosa = cos(angle); + double sina = sin(angle); + BB(ii,1) = cosa * xx + sina * yy; + BB(ii,2) = -sina * xx + cosa * yy; + } + + Matrix BBB; + if (mm > 4) { + SymmetricMatrix hlp; hlp << BB.t() * BB; + BBB = hlp.i() * BB.t(); + } + else { + BBB = BB.i(); + } + ColumnVector ee(mm); ee = 1.0; + ColumnVector alpha(mm); alpha = 0.0; + for (int ii = 1; ii <= mm; ii++) { + alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0; + } + ColumnVector BBBe = BBB * ee; + ColumnVector BBBalpha = BBB * alpha; + double aa = lorentz(BBBe, BBBe); + double bb = lorentz(BBBe, BBBalpha)-1; + double cc = lorentz(BBBalpha, BBBalpha); + double root = sqrt(bb*bb-aa*cc); + + Matrix hlpPos(4,2); + hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha; + hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha; + + ColumnVector omc(2); + for (int pp = 1; pp <= 2; pp++) { + hlpPos(4,pp) = -hlpPos(4,pp); + omc(pp) = BB(1,4) - + sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) + + (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) + + (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) - + hlpPos(4,pp); + } + if ( fabs(omc(1)) > fabs(omc(2)) ) { + pos = hlpPos.Column(2); + } + else { + pos = hlpPos.Column(1); + } + } +} + Index: trunk/BNC/src/PPP/pppClient.h =================================================================== --- trunk/BNC/src/PPP/pppClient.h (revision 7237) +++ trunk/BNC/src/PPP/pppClient.h (revision 7237) @@ -0,0 +1,79 @@ +#ifndef PPPCLIENT_H +#define PPPCLIENT_H + +#include +#include +#include "pppInclude.h" +#include "ephemeris.h" +#include "pppOptions.h" +#include "pppModel.h" + +class bncAntex; + +namespace BNC_PPP { + +class t_pppEphPool; +class t_pppObsPool; +class t_pppSatObs; +class t_pppStation; +class t_pppFilter; + +class t_pppClient : public interface_pppClient { + public: + t_pppClient(const t_pppOptions* opt); + ~t_pppClient(); + + void putEphemeris(const t_eph* eph); + void putTec(const t_vTec* vTec); + void putOrbCorrections(const std::vector& corr); + void putClkCorrections(const std::vector& corr); + void putCodeBiases(const std::vector& satBias); + void processEpoch(const std::vector& satObs, t_output* output); + + const t_pppEphPool* ephPool() const {return _ephPool;} + const t_pppObsPool* obsPool() const {return _obsPool;} + const bncAntex* antex() const {return _antex;} + const t_pppStation* staRover() const {return _staRover;} + double offGG() const {return _offGG;} + + std::ostringstream& log() {return *_log;} + const t_pppOptions* opt() const {return _opt;} + + static void bancroft(const Matrix& BBpass, ColumnVector& pos); + + static t_pppClient* instance(); + + private: + void initOutput(t_output* output); + void finish(t_irc irc); + void clearObs(); + t_irc prepareObs(const std::vector& satObs, + std::vector& obsVector, bncTime& epoTime); + t_irc cmpModel(t_pppStation* station, const ColumnVector& xyzc, + std::vector& obsVector); + t_irc cmpBancroft(const bncTime& epoTime, std::vector& obsVector, + ColumnVector& xyzc, bool print); + double cmpOffGG(std::vector& obsVector); + + t_output* _output; + t_pppEphPool* _ephPool; + t_pppObsPool* _obsPool; + bncTime _epoTimeRover; + t_pppStation* _staRover; + bncAntex* _antex; + t_pppFilter* _filter; + double _offGG; + std::vector _obsRover; + std::ostringstream* _log; + t_pppOptions* _opt; + t_tides* _tides; + t_iono* _iono; +}; + +}; // namespace BNC_PPP + +#define PPP_CLIENT (BNC_PPP::t_pppClient::instance()) +#define LOG (BNC_PPP::t_pppClient::instance()->log()) +#define OPT (BNC_PPP::t_pppClient::instance()->opt()) + +#endif Index: trunk/BNC/src/PPP/pppEphPool.cpp =================================================================== --- trunk/BNC/src/PPP/pppEphPool.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppEphPool.cpp (revision 7237) @@ -0,0 +1,132 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppEphPool + * + * Purpose: Buffer with satellite ephemerides + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include "pppEphPool.h" +#include "pppInclude.h" +#include "pppClient.h" + +using namespace BNC_PPP; +using namespace std; + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppEphPool::putEphemeris(t_eph* eph) { + if (eph && eph->checkState() != t_eph::bad) { + _satEphPool[eph->prn().toInt()].putEphemeris(_maxQueueSize, eph); + } + else { + delete eph; + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppEphPool::putOrbCorrection(t_orbCorr* corr) { + if (corr) { + _satEphPool[corr->_prn.toInt()].putOrbCorrection(corr); + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppEphPool::putClkCorrection(t_clkCorr* corr) { + if (corr) { + _satEphPool[corr->_prn.toInt()].putClkCorrection(corr); + } +} + +// +///////////////////////////////////////////////////////////////////////////// +t_irc t_pppEphPool::getCrd(const t_prn& prn, const bncTime& tt, + ColumnVector& xc, ColumnVector& vv) const { + return _satEphPool[prn.toInt()].getCrd(tt, xc, vv); +} + +// +///////////////////////////////////////////////////////////////////////////// +int t_pppEphPool::getChannel(const t_prn& prn) const { + return _satEphPool[prn.toInt()].getChannel(); +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppEphPool::t_satEphPool::putEphemeris(unsigned maxQueueSize, t_eph* eph) { + if (_ephs.empty() || eph->isNewerThan(_ephs.front())) { + _ephs.push_front(eph); + if (maxQueueSize > 0 && _ephs.size() > maxQueueSize) { + delete _ephs.back(); + _ephs.pop_back(); + } + } + else { + delete eph; + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppEphPool::t_satEphPool::putOrbCorrection(t_orbCorr* corr) { + for (unsigned ii = 0; ii < _ephs.size(); ii++) { + t_eph* eph = _ephs[ii]; + if (eph->IOD() == corr->_iod) { + eph->setOrbCorr(corr); + return; + } + } + delete corr; +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppEphPool::t_satEphPool::putClkCorrection(t_clkCorr* corr) { + for (unsigned ii = 0; ii < _ephs.size(); ii++) { + t_eph* eph = _ephs[ii]; + if (eph->IOD() == corr->_iod) { + eph->setClkCorr(corr); + } + } + delete corr; +} + +// +///////////////////////////////////////////////////////////////////////////// +t_irc t_pppEphPool::t_satEphPool::getCrd(const bncTime& tt, ColumnVector& xc, + ColumnVector& vv) const { + for (unsigned ii = 0; ii < _ephs.size(); ii++) { + t_eph* eph = _ephs[ii]; + t_irc irc = eph->getCrd(tt, xc, vv, OPT->useOrbClkCorr()); + if (irc == success) { + if (eph->prn().system() == 'R') { + double age = tt - eph->TOC(); + if (fabs(age) > 3600.0) { + continue; + } + } + return irc; + } + } + return failure; +} + +// +///////////////////////////////////////////////////////////////////////////// +int t_pppEphPool::t_satEphPool::getChannel() const { + if (_ephs.size() > 0) { + return _ephs[0]->slotNum(); + } + return 0; +} Index: trunk/BNC/src/PPP/pppEphPool.h =================================================================== --- trunk/BNC/src/PPP/pppEphPool.h (revision 7237) +++ trunk/BNC/src/PPP/pppEphPool.h (revision 7237) @@ -0,0 +1,56 @@ +#ifndef EPHPOOL_H +#define EPHPOOL_H + +#include +#include "pppInclude.h" +#include "bnctime.h" +#include "ephemeris.h" + +namespace BNC_PPP { + +class t_pppEphPool { + public: + t_pppEphPool(unsigned maxQueueSize = 3) { + _maxQueueSize = maxQueueSize; + } + ~t_pppEphPool() {}; + + void putEphemeris(t_eph* eph); + void putOrbCorrection(t_orbCorr* corr); + void putClkCorrection(t_clkCorr* corr); + + t_irc getCrd(const t_prn& prn, const bncTime& tt, + ColumnVector& xc, ColumnVector& vv) const; + + int getChannel(const t_prn& prn) const; + + std::deque& ephs(t_prn prn) { + return _satEphPool[prn]._ephs; + } + + private: + + class t_satEphPool { + public: + t_satEphPool() {}; + ~t_satEphPool() { + for (unsigned ii = 0; ii < _ephs.size(); ii++) { + delete _ephs[ii]; + } + } + void putEphemeris(unsigned maxQueueSize, t_eph* eph); + void putOrbCorrection(t_orbCorr* corr); + void putClkCorrection(t_clkCorr* corr); + t_irc getCrd(const bncTime& tt, + ColumnVector& xc, ColumnVector& vv) const; + int getChannel() const; + std::deque _ephs; + }; + + t_satEphPool _satEphPool[t_prn::MAXPRN+1]; + unsigned _maxQueueSize; +}; + +} + +#endif Index: trunk/BNC/src/PPP/pppFilter.cpp =================================================================== --- trunk/BNC/src/PPP/pppFilter.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppFilter.cpp (revision 7237) @@ -0,0 +1,449 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppFilter + * + * Purpose: Filter Adjustment + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include + +#include "pppFilter.h" +#include "bncutils.h" +#include "pppParlist.h" +#include "pppObsPool.h" +#include "pppStation.h" +#include "pppClient.h" + +using namespace BNC_PPP; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_pppFilter::t_pppFilter() { + _parlist = 0; +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_pppFilter::~t_pppFilter() { + delete _parlist; +} + +// Process Single Epoch +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::processEpoch(t_pppObsPool* obsPool) { + + _numSat = 0; + + if (!_parlist) { + _parlist = new t_pppParlist(); + } + + // Vector of all Observations + // -------------------------- + t_pppObsPool::t_epoch* epoch = obsPool->lastEpoch(); + if (!epoch) { + return failure; + } + vector& allObs = epoch->obsVector(); + + // Time of the Epoch + // ----------------- + _epoTime = epoch->epoTime(); + + if (!_firstEpoTime.valid()) { + _firstEpoTime = _epoTime; + } + + // Set Parameters + // -------------- + _parlist->set(_epoTime, allObs); + const vector& params = _parlist->params(); + + // Status Vector, Variance-Covariance Matrix + // ----------------------------------------- + ColumnVector xFltOld = _xFlt; + SymmetricMatrix QFltOld = _QFlt; + + _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0; + _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0; + _x0.ReSize(_parlist->nPar()); _x0 = 0.0; + + for (unsigned ii = 0; ii < params.size(); ii++) { + const t_pppParam* par1 = params[ii]; + + _x0[ii] = par1->x0(); + + int iOld = par1->indexOld(); + if (iOld < 0) { + _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter + } + else { + _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise(); + _xFlt[ii] = xFltOld[iOld]; + for (unsigned jj = 0; jj < ii; jj++) { + const t_pppParam* par2 = params[jj]; + int jOld = par2->indexOld(); + if (jOld >= 0) { + _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1); + } + } + } + } + + predictCovCrdPart(QFltOld); + + // Process Satellite Systems separately + // ------------------------------------ + for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) { + char system = OPT->systems()[iSys]; + vector obsVector; + for (unsigned jj = 0; jj < allObs.size(); jj++) { + if (allObs[jj]->prn().system() == system) { + obsVector.push_back(allObs[jj]); + } + } + if ( processSystem(OPT->LCs(system), obsVector) != success ) { + return failure; + } + } + + cmpDOP(allObs); + + _parlist->printResult(_epoTime, _QFlt, _xFlt); + + return success; +} + +// Process Selected LCs +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::processSystem(const vector& LCs, + const vector& obsVector) { + + LOG.setf(ios::fixed); + + // Detect Cycle Slips + // ------------------ + if (detectCycleSlips(LCs, obsVector) != success) { + return failure; + } + + ColumnVector xSav = _xFlt; + SymmetricMatrix QSav = _QFlt; + string epoTimeStr = string(_epoTime); + const vector& params = _parlist->params(); + unsigned maxObs = obsVector.size() * LCs.size(); + + // Outlier Detection Loop + // ---------------------- + for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) { + + if (iOutlier > 0) { + _xFlt = xSav; + _QFlt = QSav; + } + + // First-Design Matrix, Terms Observed-Computed, Weight Matrix + // ----------------------------------------------------------- + Matrix AA(maxObs, _parlist->nPar()); + ColumnVector ll(maxObs); + DiagonalMatrix PP(maxObs); PP = 0.0; + + int iObs = -1; + vector usedObs; + vector usedTypes; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + t_pppSatObs* obs = obsVector[ii]; + if (!obs->outlier()) { + for (unsigned jj = 0; jj < LCs.size(); jj++) { + const t_lc::type tLC = LCs[jj]; + ++iObs; + usedObs.push_back(obs); + usedTypes.push_back(tLC); + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + const t_pppParam* par = params[iPar]; + AA[iObs][iPar] = par->partial(_epoTime, obs, tLC); + } + ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1)); + PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC)); + } + } + } + + // Check number of observations, truncate matrices + // ----------------------------------------------- + if (iObs == -1) { + return failure; + } + AA = AA.Rows(1, iObs+1); + ll = ll.Rows(1, iObs+1); + PP = PP.SymSubMatrix(1, iObs+1); + + // Kalman update step + // ------------------ + kalman(AA, ll, PP, _QFlt, _xFlt); + + // Check Residuals + // --------------- + ColumnVector vv = AA * _xFlt - ll; + double maxOutlier = 0.0; + int maxOutlierIndex = -1; + t_lc::type maxOutlierLC = t_lc::dummy; + for (unsigned ii = 0; ii < usedObs.size(); ii++) { + const t_lc::type tLC = usedTypes[ii]; + double res = fabs(vv[ii]); + if (res > usedObs[ii]->maxRes(tLC)) { + if (res > fabs(maxOutlier)) { + maxOutlier = vv[ii]; + maxOutlierIndex = ii; + maxOutlierLC = tLC; + } + } + } + + // Mark outlier or break outlier detection loop + // -------------------------------------------- + if (maxOutlierIndex > -1) { + t_pppSatObs* obs = usedObs[maxOutlierIndex]; + t_pppParam* par = 0; + LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' ' + << obs->prn().toString() << ' ' + << setw(8) << setprecision(4) << maxOutlier << endl; + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + t_pppParam* hlp = params[iPar]; + if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn() && + hlp->tLC() == usedTypes[maxOutlierIndex]) { + par = hlp; + } + } + if (par) { + if (par->ambResetCandidate()) { + resetAmb(par->prn(), obsVector, &QSav, &xSav); + } + else { + par->setAmbResetCandidate(); + obs->setOutlier(); + } + } + else { + obs->setOutlier(); + } + } + + // Print Residuals + // --------------- + else { + for (unsigned jj = 0; jj < LCs.size(); jj++) { + for (unsigned ii = 0; ii < usedObs.size(); ii++) { + const t_lc::type tLC = usedTypes[ii]; + t_pppSatObs* obs = usedObs[ii]; + if (tLC == LCs[jj]) { + obs->setRes(tLC, vv[ii]); + LOG << epoTimeStr << " RES " + << left << setw(3) << t_lc::toString(tLC) << right << ' ' + << obs->prn().toString().substr(0,3) << ' ' + << setw(8) << setprecision(4) << vv[ii] << endl; + } + } + } + break; + } + } + + return success; +} + +// Cycle-Slip Detection +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::detectCycleSlips(const vector& LCs, + const vector& obsVector) { + + const double SLIP = 20.0; // slip threshold + string epoTimeStr = string(_epoTime); + const vector& params = _parlist->params(); + + for (unsigned ii = 0; ii < LCs.size(); ii++) { + const t_lc::type& tLC = LCs[ii]; + if (t_lc::includesPhase(tLC)) { + for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) { + const t_pppSatObs* obs = obsVector[iObs]; + + // Check set Slips and Jump Counters + // --------------------------------- + bool slip = false; + + if (obs->slip()) { + LOG << "cycle slip set (obs)" << endl;; + slip = true; + } + + if (_slips[obs->prn()]._obsSlipCounter != -1 && + _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) { + LOG << "cycle slip set (obsSlipCounter)" << endl; + slip = true; + } + _slips[obs->prn()]._obsSlipCounter = obs->slipCounter(); + + if (_slips[obs->prn()]._biasJumpCounter != -1 && + _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) { + LOG << "cycle slip set (biasJumpCounter)" << endl; + slip = true; + } + _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter(); + + // Slip Set + // -------- + if (slip) { + resetAmb(obs->prn(), obsVector); + } + + // Check Pre-Fit Residuals + // ----------------------- + else { + ColumnVector AA(params.size()); + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + const t_pppParam* par = params[iPar]; + AA[iPar] = par->partial(_epoTime, obs, tLC); + } + + double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA); + double vv = DotProduct(AA, _xFlt) - ll; + + if (fabs(vv) > SLIP) { + LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' ' + << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl; + resetAmb(obs->prn(), obsVector); + } + } + } + } + } + + return success; +} + +// Reset Ambiguity Parameter (cycle slip) +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::resetAmb(t_prn prn, const vector& obsVector, + SymmetricMatrix* QSav, ColumnVector* xSav) { + t_irc irc = failure; + vector& params = _parlist->params(); + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + t_pppParam* par = params[iPar]; + if (par->type() == t_pppParam::amb && par->prn() == prn) { + int ind = par->indexNew(); + t_lc::type tLC = par->tLC(); + LOG << string(_epoTime) << " RESET " << par->toString() << endl; + delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector); + par->setIndex(ind); + params[iPar] = par; + for (unsigned ii = 1; ii <= params.size(); ii++) { + _QFlt(ii, ind+1) = 0.0; + if (QSav) { + (*QSav)(ii, ind+1) = 0.0; + } + } + _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0(); + if (QSav) { + (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1); + } + _xFlt[ind] = 0.0; + if (xSav) { + (*xSav)[ind] = _xFlt[ind]; + } + _x0[ind] = par->x0(); + irc = success; + } + } + + return irc; +} + +// Compute various DOP Values +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::cmpDOP(const vector& obsVector) { + + _dop.reset(); + + try { + const unsigned numPar = 4; + Matrix AA(obsVector.size(), numPar); + _numSat = 0; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + t_pppSatObs* obs = obsVector[ii]; + if (obs->isValid() && !obs->outlier()) { + ++_numSat; + for (unsigned iPar = 0; iPar < numPar; iPar++) { + const t_pppParam* par = _parlist->params()[iPar]; + AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1); + } + } + } + if (_numSat < 4) { + return; + } + AA = AA.Rows(1, _numSat); + SymmetricMatrix NN; NN << AA.t() * AA; + SymmetricMatrix QQ = NN.i(); + + _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3)); + _dop.T = sqrt(QQ(4,4)); + _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4)); + } + catch (...) { + } +} + +// Compute various DOP Values +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) { + + const vector& params = _parlist->params(); + if (params.size() < 3) { + return; + } + + bool first = (params[0]->indexOld() < 0); + + SymmetricMatrix Qneu(3); Qneu = 0.0; + for (unsigned ii = 0; ii < 3; ii++) { + const t_pppParam* par = params[ii]; + if (first) { + Qneu[ii][ii] = par->sigma0() * par->sigma0(); + } + else { + Qneu[ii][ii] = par->noise() * par->noise(); + } + } + + const t_pppStation* sta = PPP_CLIENT->staRover(); + SymmetricMatrix Qxyz(3); + covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz); + + if (first) { + _QFlt.SymSubMatrix(1,3) = Qxyz; + } + else { + double dt = _epoTime - _firstEpoTime; + if (dt < OPT->_seedingTime) { + _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3); + } + else { + _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz; + } + } +} Index: trunk/BNC/src/PPP/pppFilter.h =================================================================== --- trunk/BNC/src/PPP/pppFilter.h (revision 7237) +++ trunk/BNC/src/PPP/pppFilter.h (revision 7237) @@ -0,0 +1,97 @@ +#ifndef FILTER_H +#define FILTER_H + +#include +#include +#include "pppInclude.h" +#include "pppParlist.h" +#include "bnctime.h" +#include "t_prn.h" + +namespace BNC_PPP { + +class t_pppParlist; +class t_pppObsPool; +class t_pppSatObs; + +class t_pppFilter { + public: + t_pppFilter(); + ~t_pppFilter(); + + t_irc processEpoch(t_pppObsPool* obsPool); + + const ColumnVector& x() const {return _xFlt;} + const SymmetricMatrix& Q() const {return _QFlt;} + + int numSat() const {return _numSat;} + double PDOP() const {return _dop.P;} + double GDOP() const {return _dop.G;} + double trp() const { + const std::vector& par = _parlist->params(); + for (unsigned ii = 0; ii < par.size(); ++ii) { + if (par[ii]->type() == t_pppParam::trp) { + return x()[ii]; + } + } + return 0.0; + }; + double trpStdev() const { + const std::vector& par = _parlist->params(); + for (unsigned ii = 0; ii < par.size(); ++ii) { + if (par[ii]->type() == t_pppParam::trp) { + return sqrt(Q()[ii][ii]); + } + } + return 0.0; + }; + + private: + class t_slip { + public: + t_slip() { + _slip = false; + _obsSlipCounter = -1; + _biasJumpCounter = -1; + } + bool _slip; + int _obsSlipCounter; + int _biasJumpCounter; + }; + + class t_dop { + public: + t_dop() {reset();} + void reset() {P = T = G = 0.0;} + double P; + double T; + double G; + }; + + t_irc processSystem(const std::vector& LCs, + const std::vector& obsVector); + + t_irc detectCycleSlips(const std::vector& LCs, + const std::vector& obsVector); + + t_irc resetAmb(t_prn prn, const std::vector& obsVector, + SymmetricMatrix* QSav = 0, ColumnVector* xSav = 0); + + void cmpDOP(const std::vector& obsVector); + + void predictCovCrdPart(const SymmetricMatrix& QFltOld); + + bncTime _epoTime; + t_pppParlist* _parlist; + SymmetricMatrix _QFlt; + ColumnVector _xFlt; + ColumnVector _x0; + t_slip _slips[t_prn::MAXPRN+1]; + int _numSat; + t_dop _dop; + bncTime _firstEpoTime; +}; + +} + +#endif Index: trunk/BNC/src/PPP/pppObsPool.cpp =================================================================== --- trunk/BNC/src/PPP/pppObsPool.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppObsPool.cpp (revision 7237) @@ -0,0 +1,77 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppObsPool + * + * Purpose: Buffer with observations + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include "pppObsPool.h" + +using namespace BNC_PPP; +using namespace std; + +// Constructor +///////////////////////////////////////////////////////////////////////////// +t_pppObsPool::t_epoch::t_epoch(const bncTime& epoTime, vector& obsVector) { + _epoTime = epoTime; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + _obsVector.push_back(obsVector[ii]); + } + obsVector.clear(); +} + +// Destructor +///////////////////////////////////////////////////////////////////////////// +t_pppObsPool::t_epoch::~t_epoch() { + for (unsigned ii = 0; ii < _obsVector.size(); ii++) { + delete _obsVector[ii]; + } +} + +// Constructor +///////////////////////////////////////////////////////////////////////////// +t_pppObsPool::t_pppObsPool() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + _satCodeBiases[ii] = 0; + } +} + +// Destructor +///////////////////////////////////////////////////////////////////////////// +t_pppObsPool::~t_pppObsPool() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + delete _satCodeBiases[ii]; + } + while (_epochs.size() > 0) { + delete _epochs.front(); + _epochs.pop_front(); + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppObsPool::putCodeBias(t_satCodeBias* satCodeBias) { + int iPrn = satCodeBias->_prn.toInt(); + delete _satCodeBiases[iPrn]; + _satCodeBiases[iPrn] = satCodeBias; +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_pppObsPool::putEpoch(const bncTime& epoTime, vector& obsVector) { + const unsigned MAXSIZE = 2; + _epochs.push_back(new t_epoch(epoTime, obsVector)); + if (_epochs.size() > MAXSIZE) { + delete _epochs.front(); + _epochs.pop_front(); + } +} Index: trunk/BNC/src/PPP/pppObsPool.h =================================================================== --- trunk/BNC/src/PPP/pppObsPool.h (revision 7237) +++ trunk/BNC/src/PPP/pppObsPool.h (revision 7237) @@ -0,0 +1,52 @@ +#ifndef OBSPOOL_H +#define OBSPOOL_H + +#include +#include +#include "pppSatObs.h" +#include "bnctime.h" + +namespace BNC_PPP { + +class t_pppObsPool { + public: + + class t_epoch { + public: + t_epoch(const bncTime& epoTime, std::vector& obsVector); + ~t_epoch(); + std::vector& obsVector() {return _obsVector;} + const std::vector& obsVector() const {return _obsVector;} + const bncTime& epoTime() const {return _epoTime;} + private: + bncTime _epoTime; + std::vector _obsVector; + }; + + t_pppObsPool(); + ~t_pppObsPool(); + void putCodeBias(t_satCodeBias* satCodeBias); + + void putEpoch(const bncTime& epoTime, std::vector& obsVector); + + const t_satCodeBias* satCodeBias(const t_prn& prn) const { + return _satCodeBiases[prn.toInt()]; + } + + t_epoch* lastEpoch() { + if (_epochs.size()) { + return _epochs.back(); + } + else { + return 0; + } + } + + private: + t_satCodeBias* _satCodeBiases[t_prn::MAXPRN+1]; + std::deque _epochs; +}; + +} + +#endif Index: trunk/BNC/src/PPP/pppParlist.cpp =================================================================== --- trunk/BNC/src/PPP/pppParlist.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppParlist.cpp (revision 7237) @@ -0,0 +1,415 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppParlist + * + * Purpose: List of estimated parameters + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include + +#include "pppParlist.h" +#include "pppSatObs.h" +#include "pppStation.h" +#include "bncutils.h" +#include "bncconst.h" +#include "pppClient.h" + +using namespace BNC_PPP; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_pppParam::t_pppParam(e_type type, const t_prn& prn, t_lc::type tLC, + const vector* obsVector) { + + _type = type; + _prn = prn; + _tLC = tLC; + _x0 = 0.0; + _indexOld = -1; + _indexNew = -1; + _noise = 0.0; + _ambInfo = 0; + + switch (_type) { + case crdX: + _epoSpec = false; + _sigma0 = OPT->_aprSigCrd[0]; + _noise = OPT->_noiseCrd[0]; + break; + case crdY: + _epoSpec = false; + _sigma0 = OPT->_aprSigCrd[1]; + _noise = OPT->_noiseCrd[1]; + break; + case crdZ: + _epoSpec = false; + _sigma0 = OPT->_aprSigCrd[2]; + _noise = OPT->_noiseCrd[2]; + break; + case clkR: + _epoSpec = true; + _sigma0 = OPT->_noiseClk; + break; + case amb: + _epoSpec = false; + _sigma0 = OPT->_aprSigAmb; + _ambInfo = new t_ambInfo(); + if (obsVector) { + for (unsigned ii = 0; ii < obsVector->size(); ii++) { + const t_pppSatObs* obs = obsVector->at(ii); + if (obs->prn() == _prn) { + double offGG = 0; + if (_prn.system() == 'R' && tLC != t_lc::MW) { + offGG = PPP_CLIENT->offGG(); + } + _x0 = floor((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC) + 0.5); + break; + } + } + } + break; + case offGG: + _epoSpec = true; + _sigma0 = 1000.0; + _x0 = PPP_CLIENT->offGG(); + break; + case trp: + _epoSpec = false; + _sigma0 = OPT->_aprSigTrp; + _noise = OPT->_noiseTrp; + break; + } +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_pppParam::~t_pppParam() { + delete _ambInfo; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppParam::partial(const bncTime& /* epoTime */, const t_pppSatObs* obs, + const t_lc::type& tLC) const { + + // Special Case - Melbourne-Wuebbena + // --------------------------------- + if (tLC == t_lc::MW && _type != amb) { + return 0.0; + } + + const t_pppStation* sta = PPP_CLIENT->staRover(); + ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3); + + switch (_type) { + case crdX: + return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius(); + case crdY: + return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius(); + case crdZ: + return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius(); + case clkR: + return 1.0; + case offGG: + return (obs->prn().system() == 'R') ? 1.0 : 0.0; + case amb: + if (obs->prn() == _prn) { + if (tLC == _tLC) { + return (obs->lambda(tLC)); + } + else if (tLC == t_lc::lIF && _tLC == t_lc::MW) { + return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2); + } + else { + map codeCoeff; + map phaseCoeff; + obs->lcCoeff(tLC, codeCoeff, phaseCoeff); + if (_tLC == t_lc::l1) { + return obs->lambda(t_lc::l1) * phaseCoeff[t_lc::toFreq(obs->prn().system(),t_lc::l1)]; + } + else if (_tLC == t_lc::l2) { + return obs->lambda(t_lc::l2) * phaseCoeff[t_lc::toFreq(obs->prn().system(),t_lc::l2)]; + } + } + } + return 0.0; + case trp: + return 1.0 / sin(obs->eleSat()); + } + + return 0.0; +} + +// +//////////////////////////////////////////////////////////////////////////// +string t_pppParam::toString() const { + stringstream ss; + switch (_type) { + case crdX: + ss << "CRD_X"; + break; + case crdY: + ss << "CRD_Y"; + break; + case crdZ: + ss << "CRD_Z"; + break; + case clkR: + ss << "CLK "; + break; + case amb: + ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString(); + break; + case offGG: + ss << "OGG "; + break; + case trp: + ss << "TRP "; + break; + } + return ss.str(); +} + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_pppParlist::t_pppParlist() { +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_pppParlist::~t_pppParlist() { + for (unsigned ii = 0; ii < _params.size(); ii++) { + delete _params[ii]; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppParlist::set(const bncTime& epoTime, const std::vector& obsVector) { + + // Remove some Parameters + // ---------------------- + vector::iterator it = _params.begin(); + while (it != _params.end()) { + t_pppParam* par = *it; + + bool remove = false; + + if (par->epoSpec()) { + remove = true; + } + + else if (par->type() == t_pppParam::amb) { + if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) { + remove = true; + } + } + + else if (par->type() == t_pppParam::amb) { + if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) { + remove = true; + } + } + + if (remove) { + delete par; + it = _params.erase(it); + } + else { + ++it; + } + } + + // Check whether parameters have observations + // ------------------------------------------ + for (unsigned ii = 0; ii < _params.size(); ii++) { + t_pppParam* par = _params[ii]; + if (par->prn() == 0) { + par->setLastObsTime(epoTime); + if (par->firstObsTime().undef()) { + par->setFirstObsTime(epoTime); + } + } + else { + for (unsigned jj = 0; jj < obsVector.size(); jj++) { + const t_pppSatObs* satObs = obsVector[jj]; + if (satObs->prn() == par->prn()) { + par->setLastObsTime(epoTime); + if (par->firstObsTime().undef()) { + par->setFirstObsTime(epoTime); + } + break; + } + } + } + } + + // Required Set of Parameters + // -------------------------- + vector required; + + // Coordinates + // ----------- + required.push_back(new t_pppParam(t_pppParam::crdX, t_prn(), t_lc::dummy)); + required.push_back(new t_pppParam(t_pppParam::crdY, t_prn(), t_lc::dummy)); + required.push_back(new t_pppParam(t_pppParam::crdZ, t_prn(), t_lc::dummy)); + + // Receiver Clock + // -------------- + required.push_back(new t_pppParam(t_pppParam::clkR, t_prn(), t_lc::dummy)); + + // GPS-Glonass Clock Offset + // ------------------------ + if (OPT->useSystem('R')) { + required.push_back(new t_pppParam(t_pppParam::offGG, t_prn(), t_lc::dummy)); + } + + // Troposphere + // ----------- + if (OPT->estTrp()) { + required.push_back(new t_pppParam(t_pppParam::trp, t_prn(), t_lc::dummy)); + } + + // Ambiguities + // ----------- + for (unsigned jj = 0; jj < obsVector.size(); jj++) { + const t_pppSatObs* satObs = obsVector[jj]; + const vector& ambLCs = OPT->ambLCs(satObs->prn().system()); + for (unsigned ii = 0; ii < ambLCs.size(); ii++) { + required.push_back(new t_pppParam(t_pppParam::amb, satObs->prn(), ambLCs[ii], &obsVector)); + } + } + + // Check if all required parameters are present + // -------------------------------------------- + for (unsigned ii = 0; ii < required.size(); ii++) { + t_pppParam* parReq = required[ii]; + + bool found = false; + for (unsigned jj = 0; jj < _params.size(); jj++) { + t_pppParam* parOld = _params[jj]; + if (parOld->isEqual(parReq)) { + found = true; + break; + } + } + if (found) { + delete parReq; + } + else { + _params.push_back(parReq); + } + } + + // Set Parameter Indices + // --------------------- + sort(_params.begin(), _params.end(), t_pppParam::sortFunction); + + for (unsigned ii = 0; ii < _params.size(); ii++) { + t_pppParam* par = _params[ii]; + par->setIndex(ii); + for (unsigned jj = 0; jj < obsVector.size(); jj++) { + const t_pppSatObs* satObs = obsVector[jj]; + if (satObs->prn() == par->prn()) { + par->setAmbEleSat(satObs->eleSat()); + par->stepAmbNumEpo(); + } + } + } + + return success; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppParlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ, + const ColumnVector& xx) const { + + string epoTimeStr = string(epoTime); + const t_pppStation* sta = PPP_CLIENT->staRover(); + + LOG << endl; + + t_pppParam* parX = 0; + t_pppParam* parY = 0; + t_pppParam* parZ = 0; + for (unsigned ii = 0; ii < _params.size(); ii++) { + t_pppParam* par = _params[ii]; + if (par->type() == t_pppParam::crdX) { + parX = par; + } + else if (par->type() == t_pppParam::crdY) { + parY = par; + } + else if (par->type() == t_pppParam::crdZ) { + parZ = par; + } + else { + int ind = par->indexNew(); + double apr = (par->type() == t_pppParam::trp) ? + t_tropo::delay_saast(sta->xyzApr(), M_PI/2.0) : par->x0(); + LOG << epoTimeStr << ' ' << par->toString() << ' ' + << setw(10) << setprecision(4) << apr << ' ' + << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- " + << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]); + if (par->type() == t_pppParam::amb) { + LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI + << " epo = " << setw(4) << par->ambNumEpo(); + } + LOG << endl; + } + } + + if (parX && parY && parZ) { + + ColumnVector xyz(3); + xyz[0] = xx[parX->indexNew()]; + xyz[1] = xx[parY->indexNew()]; + xyz[2] = xx[parZ->indexNew()]; + + ColumnVector neu(3); + xyz2neu(sta->ellApr().data(), xyz.data(), neu.data()); + + SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3); + + SymmetricMatrix QQneu(3); + covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu); + + LOG << epoTimeStr << ' ' << sta->name() + << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- " + << setprecision(4) << sqrt(QQxyz[0][0]) + + << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- " + << setprecision(4) << sqrt(QQxyz[1][1]) + + << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- " + << setprecision(4) << sqrt(QQxyz[2][2]) + + << " dN = " << setprecision(4) << neu[0] << " +- " + << setprecision(4) << sqrt(QQneu[0][0]) + + << " dE = " << setprecision(4) << neu[1] << " +- " + << setprecision(4) << sqrt(QQneu[1][1]) + + << " dU = " << setprecision(4) << neu[2] << " +- " + << setprecision(4) << sqrt(QQneu[2][2]) + + << endl; + } +} + Index: trunk/BNC/src/PPP/pppParlist.h =================================================================== --- trunk/BNC/src/PPP/pppParlist.h (revision 7237) +++ trunk/BNC/src/PPP/pppParlist.h (revision 7237) @@ -0,0 +1,111 @@ +#ifndef PARLIST_H +#define PARLIST_H + +#include +#include +#include "pppInclude.h" +#include "t_prn.h" +#include "bnctime.h" + +namespace BNC_PPP { + +class t_pppSatObs; + +class t_pppParam { + public: + enum e_type {crdX, crdY, crdZ, clkR, amb, offGG, trp}; + + t_pppParam(e_type type, const t_prn& prn, t_lc::type tLC, + const std::vector* obsVector = 0); + + ~t_pppParam(); + e_type type() const {return _type;} + double x0() const {return _x0;} + double partial(const bncTime& epoTime, const t_pppSatObs* obs, + const t_lc::type& tLC) const; + bool epoSpec() const {return _epoSpec;} + bool isEqual(const t_pppParam* par2) const { + return (_type == par2->_type && _prn == par2->_prn && _tLC == par2->_tLC); + } + void setIndex(int indexNew) { + _indexOld = _indexNew; + _indexNew = indexNew; + } + int indexOld() const {return _indexOld;} + int indexNew() const {return _indexNew;} + double sigma0() const {return _sigma0;} + double noise() const {return _noise;} + t_lc::type tLC() const {return _tLC;} + t_prn prn() const {return _prn;} + std::string toString() const; + + const bncTime& lastObsTime() const {return _lastObsTime;} + void setLastObsTime(const bncTime& epoTime) {_lastObsTime = epoTime;} + const bncTime& firstObsTime() const {return _firstObsTime;} + void setFirstObsTime(const bncTime& epoTime) {_firstObsTime = epoTime;} + + bool ambResetCandidate() const {return _ambInfo && _ambInfo->_resetCandidate;} + void setAmbResetCandidate() {if (_ambInfo) _ambInfo->_resetCandidate = true;} + double ambEleSat() const {return _ambInfo ? _ambInfo->_eleSat : 0.0;} + void setAmbEleSat(double eleSat) {if (_ambInfo) _ambInfo->_eleSat = eleSat;} + unsigned ambNumEpo() const {return _ambInfo ? _ambInfo->_numEpo : 0;} + void stepAmbNumEpo() {if (_ambInfo) _ambInfo->_numEpo += 1;} + + static bool sortFunction(const t_pppParam* p1, const t_pppParam* p2) { + if (p1->_type != p2->_type) { + return p1->_type < p2->_type; + } + else if (p1->_tLC != p2->_tLC) { + return p1->_tLC < p2->_tLC; + } + else if (p1->_prn != p2->_prn) { + return p1->_prn < p2->_prn; + } + return false; + } + + private: + class t_ambInfo { + public: + t_ambInfo() { + _resetCandidate = false; + _eleSat = 0.0; + _numEpo = 0; + } + ~t_ambInfo() {} + bool _resetCandidate; + double _eleSat; + unsigned _numEpo; + }; + e_type _type; + t_prn _prn; + t_lc::type _tLC; + double _x0; + bool _epoSpec; + int _indexOld; + int _indexNew; + double _sigma0; + double _noise; + t_ambInfo* _ambInfo; + bncTime _lastObsTime; + bncTime _firstObsTime; +}; + +class t_pppParlist { + public: + t_pppParlist(); + ~t_pppParlist(); + + t_irc set(const bncTime& epoTime, const std::vector& obsVector); + unsigned nPar() const {return _params.size();} + const std::vector& params() const {return _params;} + std::vector& params() {return _params;} + void printResult(const bncTime& epoTime, const SymmetricMatrix& QQ, + const ColumnVector& xx) const; + private: + std::vector _params; +}; + +} + +#endif Index: trunk/BNC/src/PPP/pppSatObs.cpp =================================================================== --- trunk/BNC/src/PPP/pppSatObs.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppSatObs.cpp (revision 7237) @@ -0,0 +1,512 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppSatObs + * + * Purpose: Satellite observations + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + + +#include +#include +#include + +#include "pppSatObs.h" +#include "bncconst.h" +#include "pppEphPool.h" +#include "pppStation.h" +#include "bncutils.h" +#include "bncantex.h" +#include "pppObsPool.h" +#include "pppClient.h" + +using namespace BNC_PPP; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_pppSatObs::t_pppSatObs(const t_satObs& pppSatObs) { + _prn = pppSatObs._prn; + _time = pppSatObs._time; + _outlier = false; + _valid = true; + for (unsigned ii = 0; ii < t_frequency::max; ii++) { + _obs[ii] = 0; + } + prepareObs(pppSatObs); +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_pppSatObs::~t_pppSatObs() { + for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) { + delete _obs[iFreq]; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppSatObs::prepareObs(const t_satObs& pppSatObs) { + + _model.reset(); + + // Select pseudoranges and phase observations + // ------------------------------------------ + const string preferredAttrib = "CWPX_"; + + for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) { + string frqNum = t_frequency::toString(t_frequency::type(iFreq)).substr(1); + for (unsigned iPref = 0; iPref < preferredAttrib.length(); iPref++) { + string obsType = (preferredAttrib[iPref] == '_') ? frqNum : frqNum + preferredAttrib[iPref]; + if (_obs[iFreq] == 0) { + for (unsigned ii = 0; ii < pppSatObs._obs.size(); ii++) { + const t_frqObs* obs = pppSatObs._obs[ii]; + if (obs->_rnxType2ch == obsType && obs->_codeValid && obs->_phaseValid) { + _obs[iFreq] = new t_frqObs(*obs); + } + } + } + } + } + + // Used frequency types + // -------------------- + _fType1 = t_lc::toFreq(_prn.system(),t_lc::l1); + _fType2 = t_lc::toFreq(_prn.system(),t_lc::l2); + + // Check whether all required frequencies available + // ------------------------------------------------ + for (unsigned ii = 0; ii < OPT->LCs(_prn.system()).size(); ii++) { + t_lc::type tLC = OPT->LCs(_prn.system())[ii]; + if (!isValid(tLC)) { + _valid = false; + return; + } + } + + // Find Glonass Channel Number + // --------------------------- + if (_prn.system() == 'R') { + _channel = PPP_CLIENT->ephPool()->getChannel(_prn); + } + else { + _channel = 0; + } + + // Compute Satellite Coordinates at Time of Transmission + // ----------------------------------------------------- + _xcSat.ReSize(4); _xcSat = 0.0; + _vvSat.ReSize(4); _vvSat = 0.0; + bool totOK = false; + ColumnVector satPosOld(4); satPosOld = 0.0; + t_lc::type tLC = isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1; + double prange = obsValue(tLC); + for (int ii = 1; ii <= 10; ii++) { + bncTime ToT = _time - prange / t_CST::c - _xcSat[3]; + if (PPP_CLIENT->ephPool()->getCrd(_prn, ToT, _xcSat, _vvSat) != success) { + _valid = false; + return; + } + ColumnVector dx = _xcSat - satPosOld; + dx[3] *= t_CST::c; + if (dx.norm_Frobenius() < 1.e-4) { + totOK = true; + break; + } + satPosOld = _xcSat; + } + if (totOK) { + _model._satClkM = _xcSat[3] * t_CST::c; + } + else { + _valid = false; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppSatObs::lcCoeff(t_lc::type tLC, + map& codeCoeff, + map& phaseCoeff) const { + + codeCoeff.clear(); + phaseCoeff.clear(); + + double f1 = t_CST::freq(_fType1, _channel); + double f2 = t_CST::freq(_fType2, _channel); + + switch (tLC) { + case t_lc::l1: + phaseCoeff[_fType1] = 1.0; + return; + case t_lc::l2: + phaseCoeff[_fType2] = 1.0; + return; + case t_lc::lIF: + phaseCoeff[_fType1] = f1 * f1 / (f1 * f1 - f2 * f2); + phaseCoeff[_fType2] = -f2 * f2 / (f1 * f1 - f2 * f2); + return; + case t_lc::MW: + phaseCoeff[_fType1] = f1 / (f1 - f2); + phaseCoeff[_fType2] = -f2 / (f1 - f2); + codeCoeff[_fType1] = -f1 / (f1 + f2); + codeCoeff[_fType2] = -f2 / (f1 + f2); + return; + case t_lc::CL: + phaseCoeff[_fType1] = 0.5; + codeCoeff[_fType1] = 0.5; + return; + case t_lc::c1: + codeCoeff[_fType1] = 1.0; + return; + case t_lc::c2: + codeCoeff[_fType2] = 1.0; + return; + case t_lc::cIF: + codeCoeff[_fType1] = f1 * f1 / (f1 * f1 - f2 * f2); + codeCoeff[_fType2] = -f2 * f2 / (f1 * f1 - f2 * f2); + return; + case t_lc::dummy: + case t_lc::maxLc: + return; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +bool t_pppSatObs::isValid(t_lc::type tLC) const { + bool valid = true; + obsValue(tLC, &valid); + return valid; +} +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::obsValue(t_lc::type tLC, bool* valid) const { + + map codeCoeff; + map phaseCoeff; + lcCoeff(tLC, codeCoeff, phaseCoeff); + + double retVal = 0.0; + if (valid) *valid = true; + + map::const_iterator it; + for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { + t_frequency::type tFreq = it->first; + if (_obs[tFreq] == 0) { + if (valid) *valid = false; + return 0.0; + } + else { + retVal += it->second * _obs[tFreq]->_code; + } + } + for (it = phaseCoeff.begin(); it != phaseCoeff.end(); it++) { + t_frequency::type tFreq = it->first; + if (_obs[tFreq] == 0) { + if (valid) *valid = false; + return 0.0; + } + else { + retVal += it->second * _obs[tFreq]->_phase * t_CST::lambda(tFreq, _channel); + } + } + + return retVal; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::lambda(t_lc::type tLC) const { + + double f1 = t_CST::freq(_fType1, _channel); + double f2 = t_CST::freq(_fType2, _channel); + + if (tLC == t_lc::l1) { + return t_CST::c / f1; + } + else if (tLC == t_lc::l2) { + return t_CST::c / f2; + } + else if (tLC == t_lc::lIF) { + return t_CST::c / (f1 + f2); + } + else if (tLC == t_lc::MW) { + return t_CST::c / (f1 - f2); + } + else if (tLC == t_lc::CL) { + return t_CST::c / f1 / 2.0; + } + + return 0.0; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::sigma(t_lc::type tLC) const { + + map codeCoeff; + map phaseCoeff; + lcCoeff(tLC, codeCoeff, phaseCoeff); + + double retVal = 0.0; + + map::const_iterator it; + for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { + retVal += it->second * it->second * OPT->_sigmaC1 * OPT->_sigmaC1; + } + for (it = phaseCoeff.begin(); it != phaseCoeff.end(); it++) { + retVal += it->second * it->second * OPT->_sigmaL1 * OPT->_sigmaL1; + } + + retVal = sqrt(retVal); + + // De-Weight GLONASS + // ----------------- + if (_prn.system() == 'R') { + retVal *= 5.0; + } + + // Elevation-Dependent Weighting + // ----------------------------- + double cEle = 1.0; + if ( (OPT->_eleWgtCode && t_lc::includesCode(tLC)) || + (OPT->_eleWgtPhase && t_lc::includesPhase(tLC)) ) { + double eleD = eleSat()*180.0/M_PI; + double hlp = fabs(90.0 - eleD); + cEle = (1.0 + hlp*hlp*hlp*0.000004); + } + + return cEle * retVal; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::maxRes(t_lc::type tLC) const { + + map codeCoeff; + map phaseCoeff; + lcCoeff(tLC, codeCoeff, phaseCoeff); + + double retVal = 0.0; + + map::const_iterator it; + for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { + retVal += it->second * it->second * OPT->_maxResC1 * OPT->_maxResC1; + } + for (it = phaseCoeff.begin(); it != phaseCoeff.end(); it++) { + retVal += it->second * it->second * OPT->_maxResL1 * OPT->_maxResL1; + } + + return sqrt(retVal); +} + + +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppSatObs::cmpModel(const t_pppStation* station) { + + // Reset all model values + // ---------------------- + _model.reset(); + + // Topocentric Satellite Position + // ------------------------------ + ColumnVector rSat = _xcSat.Rows(1,3); + ColumnVector rhoV = rSat - station->xyzApr(); + _model._rho = rhoV.norm_Frobenius(); + + ColumnVector neu(3); + xyz2neu(station->ellApr().data(), rhoV.data(), neu.data()); + + _model._eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / _model._rho ); + if (neu[2] < 0) { + _model._eleSat *= -1.0; + } + _model._azSat = atan2(neu[1], neu[0]); + + // Satellite Clocks + // ---------------- + _model._satClkM = _xcSat[3] * t_CST::c; + + // Receiver Clocks + // --------------- + _model._recClkM = station->dClk() * t_CST::c; + + // Sagnac Effect (correction due to Earth rotation) + // ------------------------------------------------ + ColumnVector Omega(3); + Omega[0] = 0.0; + Omega[1] = 0.0; + Omega[2] = t_CST::omega / t_CST::c; + _model._sagnac = DotProduct(Omega, crossproduct(rSat, station->xyzApr())); + + // Antenna Eccentricity + // -------------------- + _model._antEcc = -DotProduct(station->xyzEcc(), rhoV) / _model._rho; + + // Antenna Phase Center Offsets and Variations + // ------------------------------------------- + if (PPP_CLIENT->antex()) { + for (unsigned ii = 0; ii < t_frequency::max; ii++) { + t_frequency::type frqType = static_cast(ii); + bool found; + _model._antPCO[ii] = PPP_CLIENT->antex()->rcvCorr(station->antName(), frqType, + _model._eleSat, _model._azSat, found); + } + } + + // Tropospheric Delay + // ------------------ + _model._tropo = t_tropo::delay_saast(station->xyzApr(), _model._eleSat); + + // Phase Wind-Up + // ------------- + _model._windUp = station->windUp(_time, _prn, rSat); + + // Code Biases + // ----------- + const t_satCodeBias* satCodeBias = PPP_CLIENT->obsPool()->satCodeBias(_prn); + if (satCodeBias) { + for (unsigned ii = 0; ii < satCodeBias->_bias.size(); ii++) { + const t_frqCodeBias& bias = satCodeBias->_bias[ii]; + for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) { + const t_frqObs* obs = _obs[iFreq]; + if (obs && obs->_rnxType2ch == bias._rnxType2ch) { + _model._codeBias[iFreq] = bias._value; + } + } + } + } + + // Tidal Correction + // ---------------- + _model._tide = -DotProduct(station->tideDspl(), rhoV) / _model._rho; + + // Ionospheric Delay + // ----------------- + if (station->vTec()) { + + for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) { + t_frequency::type frqType = static_cast(iFreq); + //_model._codeIono[iFreq] = + } + } + + + // Ocean Loading + // ------------- + // TODO + + // Set Model Set Flag + // ------------------ + _model._set = true; + + return success; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppSatObs::printModel() const { + LOG.setf(ios::fixed); + LOG << "MODEL for Satellite " << _prn.toString() << endl + << "RHO: " << setw(12) << setprecision(3) << _model._rho << endl + << "ELE: " << setw(12) << setprecision(3) << _model._eleSat * 180.0 / M_PI << endl + << "AZI: " << setw(12) << setprecision(3) << _model._azSat * 180.0 / M_PI << endl + << "SATCLK: " << setw(12) << setprecision(3) << _model._satClkM << endl + << "RECCLK: " << setw(12) << setprecision(3) << _model._recClkM << endl + << "SAGNAC: " << setw(12) << setprecision(3) << _model._sagnac << endl + << "ANTECC: " << setw(12) << setprecision(3) << _model._antEcc << endl + << "TROPO: " << setw(12) << setprecision(3) << _model._tropo << endl + << "WINDUP: " << setw(12) << setprecision(3) << _model._windUp << endl + << "TIDES: " << setw(12) << setprecision(3) << _model._tide << endl; + for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) { + if (_obs[iFreq]) { + LOG << "PCO: " << t_frequency::toString(t_frequency::type(iFreq)) << setw(12) << setprecision(3) << _model._antPCO[iFreq] << endl + << "BIAS CODE: " << t_frequency::toString(t_frequency::type(iFreq)) << setw(12) << setprecision(3) << _model._codeBias[iFreq] << endl + << "BIAS PHASE: " << t_frequency::toString(t_frequency::type(iFreq)) << setw(12) << setprecision(3) << _model._phaseBias[iFreq] << endl; + } + } + LOG << "OBS-CMP P3: " << _prn.toString() << " " + << setw(12) << setprecision(3) << obsValue(t_lc::cIF) << " " + << setw(12) << setprecision(3) << cmpValue(t_lc::cIF) << " " + << setw(12) << setprecision(3) << obsValue(t_lc::cIF) - cmpValue(t_lc::cIF) << endl; + + LOG << "OBS-CMP L3: " << _prn.toString() << " " + << setw(12) << setprecision(3) << obsValue(t_lc::lIF) << " " + << setw(12) << setprecision(3) << cmpValue(t_lc::lIF) << " " + << setw(12) << setprecision(3) << obsValue(t_lc::lIF) - cmpValue(t_lc::lIF) << endl; + + LOG << "OBS-CMP MW: " << _prn.toString() << " " + << setw(12) << setprecision(3) << obsValue(t_lc::MW) << " " + << setw(12) << setprecision(3) << cmpValue(t_lc::MW) << " " + << setw(12) << setprecision(3) << obsValue(t_lc::MW) - cmpValue(t_lc::MW) << endl; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::cmpValueForBanc(t_lc::type tLC) const { + return cmpValue(tLC) - _model._rho - _model._sagnac - _model._recClkM; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::cmpValue(t_lc::type tLC) const { + + if (!isValid(tLC)) { + return 0.0; + } + + // Non-Dispersive Part + // ------------------- + double nonDisp = _model._rho + _model._recClkM - _model._satClkM + + _model._sagnac + _model._antEcc + _model._tropo + + _model._tide; + + // Add Dispersive Part + // ------------------- + map codeCoeff; + map phaseCoeff; + lcCoeff(tLC, codeCoeff, phaseCoeff); + + double dispPart = 0.0; + + map::const_iterator it; + for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { + t_frequency::type tFreq = it->first; + dispPart += it->second * (_model._antPCO[tFreq] + _model._codeBias[tFreq]); + } + for (it = phaseCoeff.begin(); it != phaseCoeff.end(); it++) { + t_frequency::type tFreq = it->first; + dispPart += it->second * (_model._antPCO[tFreq] + _model._phaseBias[tFreq] + + _model._windUp * t_CST::lambda(tFreq, _channel)); + } + + return nonDisp + dispPart; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppSatObs::setRes(t_lc::type tLC, double res) { + _res[tLC] = res; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_pppSatObs::getRes(t_lc::type tLC) const { + map::const_iterator it = _res.find(tLC); + if (it != _res.end()) { + return it->second; + } + else { + return 0.0; + } +} Index: trunk/BNC/src/PPP/pppSatObs.h =================================================================== --- trunk/BNC/src/PPP/pppSatObs.h (revision 7237) +++ trunk/BNC/src/PPP/pppSatObs.h (revision 7237) @@ -0,0 +1,132 @@ +#ifndef PPPSATOBS_H +#define PPPSATOBS_H + +#include +#include +#include +#include "pppInclude.h" +#include "satObs.h" +#include "bnctime.h" + +namespace BNC_PPP { + +class t_pppStation; + +class t_pppSatObs { + public: + t_pppSatObs(const t_satObs& satObs); + ~t_pppSatObs(); + bool isValid() const {return _valid;}; + bool isValid(t_lc::type tLC) const; + const t_prn& prn() const {return _prn;} + const ColumnVector& xc() const {return _xcSat;} + const bncTime& time() const {return _time;} + t_irc cmpModel(const t_pppStation* station); + double obsValue(t_lc::type tLC, bool* valid = 0) const; + double cmpValue(t_lc::type tLC) const; + double cmpValueForBanc(t_lc::type tLC) const; + double rho() const {return _model._rho;} + double sagnac() const {return _model._sagnac;} + double eleSat() const {return _model._eleSat;} + bool modelSet() const {return _model._set;} + void printModel() const; + void lcCoeff(t_lc::type tLC, + std::map& codeCoeff, + std::map& phaseCoeff) const; + double lambda(t_lc::type tLC) const; + double sigma(t_lc::type tLC) const; + double maxRes(t_lc::type tLC) const; + bool outlier() const {return _outlier;} + void setOutlier() {_outlier = true;} + void setRes(t_lc::type tLC, double res); + double getRes(t_lc::type tLC) const; + + bool slip() const { + for (unsigned ii = 1; ii < t_frequency::max; ii++) { + if (_obs[ii] && _obs[ii]->_slip) { + return true; + } + } + return false; + } + + int slipCounter() const { + int cnt = -1; + for (unsigned ii = 1; ii < t_frequency::max; ii++) { + if (_obs[ii] && _obs[ii]->_slipCounter > cnt) { + cnt = _obs[ii]->_slipCounter; + } + } + return cnt; + } + + int biasJumpCounter() const { + int jmp = -1; + for (unsigned ii = 1; ii < t_frequency::max; ii++) { + if (_obs[ii] && _obs[ii]->_biasJumpCounter > jmp) { + jmp = _obs[ii]->_biasJumpCounter; + } + } + return jmp; + } + + private: + class t_model { + public: + t_model() {reset();} + ~t_model() {} + void reset() { + _set = false; + _rho = 0.0; + _eleSat = 0.0; + _azSat = 0.0; + _recClkM = 0.0; + _satClkM = 0.0; + _sagnac = 0.0; + _antEcc = 0.0; + _tropo = 0.0; + _tide = 0.0; + _windUp = 0.0; + for (unsigned ii = 0; ii < t_frequency::max; ii++) { + _antPCO[ii] = 0.0; + _codeBias[ii] = 0.0; + _phaseBias[ii] = 0.0; + _codeIono[ii] = 0.0; + } + } + bool _set; + double _rho; + double _eleSat; + double _azSat; + double _recClkM; + double _satClkM; + double _sagnac; + double _antEcc; + double _tropo; + double _tide; + double _windUp; + double _antPCO[t_frequency::max]; + double _codeBias[t_frequency::max]; + double _phaseBias[t_frequency::max]; + double _codeIono[t_frequency::max]; + }; + + void prepareObs(const t_satObs& satObs); + + bool _valid; + t_frequency::type _fType1; + t_frequency::type _fType2; + t_prn _prn; + bncTime _time; + int _channel; + t_frqObs* _obs[t_frequency::max]; + ColumnVector _xcSat; + ColumnVector _vvSat; + t_model _model; + bool _outlier; + std::map _res; +}; + +} + +#endif Index: trunk/BNC/src/PPP/pppStation.cpp =================================================================== --- trunk/BNC/src/PPP/pppStation.cpp (revision 7237) +++ trunk/BNC/src/PPP/pppStation.cpp (revision 7237) @@ -0,0 +1,58 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppStation + * + * Purpose: Processed station + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include "pppStation.h" +#include "bncutils.h" +#include "pppModel.h" + +using namespace BNC_PPP; +using namespace std; + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_pppStation::t_pppStation() { + _windUp = new t_windUp(); +} + +// Destructor +////////////////////////////////////////////////////////////////////////////// +t_pppStation::~t_pppStation() { + delete _windUp; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppStation::setXyzApr(const ColumnVector& xyzApr) { + _xyzApr = xyzApr; + _ellApr.ReSize(3); + xyz2ell(_xyzApr.data(), _ellApr.data()); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppStation::setNeuEcc(const ColumnVector& neuEcc) { + _neuEcc = neuEcc; + _xyzEcc.ReSize(3); + neu2xyz(_ellApr.data(), _neuEcc.data(), _xyzEcc.data()); +} + +// +////////////////////////////////////////////////////////////////////////////// +double t_pppStation::windUp(const bncTime& time, t_prn prn, + const ColumnVector& rSat) const { + return _windUp->value(time, _xyzApr, prn, rSat); +} + Index: trunk/BNC/src/PPP/pppStation.h =================================================================== --- trunk/BNC/src/PPP/pppStation.h (revision 7237) +++ trunk/BNC/src/PPP/pppStation.h (revision 7237) @@ -0,0 +1,52 @@ +#ifndef STATION_H +#define STATION_H + +#include +#include +#include "pppInclude.h" +#include "bnctime.h" + +namespace BNC_PPP { + +class t_windUp; + +class t_pppStation { + public: + t_pppStation(); + ~t_pppStation(); + void setName(std::string name) {_name = name;} + void setAntName(std::string antName) {_antName = antName;} + void setXyzApr(const ColumnVector& xyzApr); + void setNeuEcc(const ColumnVector& neuEcc); + void setDClk(double dClk) {_dClk = dClk;} + void setTideDspl(const ColumnVector& tideDspl) {_tideDspl = tideDspl;} + void setVtec(double vtec) {_vtec = vtec;} + const std::string& name() const {return _name;} + const std::string& antName() const {return _antName;} + const ColumnVector& xyzApr() const {return _xyzApr;} + const ColumnVector& ellApr() const {return _ellApr;} + const ColumnVector& neuEcc() const {return _neuEcc;} + const ColumnVector& xyzEcc() const {return _xyzEcc;} + const ColumnVector& tideDspl() const {return _tideDspl;} + double dClk() const {return _dClk;} + double windUp(const bncTime& time, t_prn prn, const ColumnVector& rSat) const; + double vTec() const {return _vtec;} + + private: + std::string _name; + std::string _antName; + ColumnVector _xyzApr; + ColumnVector _ellApr; + ColumnVector _neuEcc; + ColumnVector _xyzEcc; + ColumnVector _tideDspl; + double _dClk; + mutable t_windUp* _windUp; + bncTime _timeCheck; + ColumnVector _xyzCheck; + double _vtec; +}; + +} + +#endif