Index: trunk/BNC/src/PPP/pppClient.cpp =================================================================== --- trunk/BNC/src/PPP/pppClient.cpp (revision 7202) +++ trunk/BNC/src/PPP/pppClient.cpp (revision 7203) @@ -1,2 +1,26 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + /* ------------------------------------------------------------------------- * BKG NTRIP Client @@ -5,9 +29,9 @@ * Class: t_pppClient * - * Purpose: PPP Client processing starts here + * Purpose: Precise Point Positioning * * Author: L. Mervart * - * Created: 29-Jul-2014 + * Created: 21-Nov-2009 * * Changes: @@ -15,68 +39,304 @@ * -----------------------------------------------------------------------*/ -#include - -#include +#include #include -#include -#include -#include +#include #include "pppClient.h" -#include "pppEphPool.h" -#include "pppObsPool.h" -#include "bncconst.h" +#include "bncephuser.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 +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_pppClient::t_pppClient(const t_pppOptions* opt) { + + _opt = new t_pppOptions(*opt); + _filter = new t_pppFilter(this); + _epoData = new t_epoData(); + _log = new ostringstream(); + _ephUser = new bncEphUser(false); + + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + _satCodeBiases[ii] = 0; + } + +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_pppClient::~t_pppClient() { + + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + delete _satCodeBiases[ii]; + } + + delete _filter; + delete _epoData; + delete _opt; + delete _ephUser; + delete _log; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppClient::processEpoch(const vector& satObs, t_output* output) { + + // Convert and store observations + // ------------------------------ + _epoData->clear(); + for (unsigned ii = 0; ii < satObs.size(); ii++) { + const t_satObs* obs = satObs[ii]; + t_prn prn = obs->_prn; + if (prn.system() == 'E') {prn.setFlags(1);} // force I/NAV usage + t_satData* satData = new t_satData(); + + if (_epoData->tt.undef()) { + _epoData->tt = obs->_time; + } + + satData->tt = obs->_time; + satData->prn = QString(prn.toInternalString().c_str()); + satData->slipFlag = false; + satData->P1 = 0.0; + satData->P2 = 0.0; + satData->P5 = 0.0; + satData->P7 = 0.0; + satData->L1 = 0.0; + satData->L2 = 0.0; + satData->L5 = 0.0; + satData->L7 = 0.0; + for (unsigned ifrq = 0; ifrq < obs->_obs.size(); ifrq++) { + t_frqObs* frqObs = obs->_obs[ifrq]; + double cb = 0.0; + const t_satCodeBias* satCB = satCodeBias(prn); + if (satCB && satCB->_bias.size()) { + for (unsigned ii = 0; ii < satCB->_bias.size(); ii++) { + + const t_frqCodeBias& bias = satCB->_bias[ii]; + if (frqObs && frqObs->_rnxType2ch == bias._rnxType2ch) { + cb = bias._value; + } + } + } + if (frqObs->_rnxType2ch[0] == '1') { + if (frqObs->_codeValid) satData->P1 = frqObs->_code + cb; + if (frqObs->_phaseValid) satData->L1 = frqObs->_phase; + if (frqObs->_slip) satData->slipFlag = true; + } + else if (frqObs->_rnxType2ch[0] == '2') { + if (frqObs->_codeValid) satData->P2 = frqObs->_code + cb; + if (frqObs->_phaseValid) satData->L2 = frqObs->_phase; + if (frqObs->_slip) satData->slipFlag = true; + } + else if (frqObs->_rnxType2ch[0] == '5') { + if (frqObs->_codeValid) satData->P5 = frqObs->_code + cb; + if (frqObs->_phaseValid) satData->L5 = frqObs->_phase; + if (frqObs->_slip) satData->slipFlag = true; + } + else if (frqObs->_rnxType2ch[0] == '7') { + if (frqObs->_codeValid) satData->P7 = frqObs->_code + cb; + if (frqObs->_phaseValid) satData->L7 = frqObs->_phase; + if (frqObs->_slip) satData->slipFlag = true; + } + } + putNewObs(satData); + } + + // Data Pre-Processing + // ------------------- + QMutableMapIterator it(_epoData->satData); + while (it.hasNext()) { + it.next(); + QString prn = it.key(); + t_satData* satData = it.value(); + + if (cmpToT(satData) != success) { + delete satData; + it.remove(); + continue; + } + + } + + // Filter Solution + // --------------- + if (_filter->update(_epoData) == success) { + output->_error = false; + output->_epoTime = _filter->time(); + output->_xyzRover[0] = _filter->x(); + output->_xyzRover[1] = _filter->y(); + output->_xyzRover[2] = _filter->z(); + copy(&_filter->Q().data()[0], &_filter->Q().data()[6], output->_covMatrix); + output->_neu[0] = _filter->neu()[0]; + output->_neu[1] = _filter->neu()[1]; + output->_neu[2] = _filter->neu()[2]; + output->_numSat = _filter->numSat(); + output->_pDop = _filter->PDOP(); + output->_trp0 = _filter->trp0(); + output->_trp = _filter->trp(); + } + else { + output->_error = true; + } + + output->_log = _log->str(); + delete _log; _log = new ostringstream(); +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppClient::putNewObs(t_satData* satData) { + + // Set Observations GPS + // -------------------- + if (satData->system() == 'G') { + if (satData->P1 != 0.0 && satData->P2 != 0.0 && + satData->L1 != 0.0 && satData->L2 != 0.0 ) { + t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1); + t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2); + double f1 = t_CST::freq(fType1, 0); + double f2 = t_CST::freq(fType2, 0); + double a1 = f1 * f1 / (f1 * f1 - f2 * f2); + double a2 = - f2 * f2 / (f1 * f1 - f2 * f2); + satData->L1 = satData->L1 * t_CST::c / f1; + satData->L2 = satData->L2 * t_CST::c / f2; + satData->P3 = a1 * satData->P1 + a2 * satData->P2; + satData->L3 = a1 * satData->L1 + a2 * satData->L2; + satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2; + satData->lkA = a1; + satData->lkB = a2; + _epoData->satData[satData->prn] = satData; + } + else { + delete satData; + } + } + + // Set Observations Glonass + // ------------------------ + else if (satData->system() == 'R' && _opt->useSystem('R')) { + if (satData->P1 != 0.0 && satData->P2 != 0.0 && + satData->L1 != 0.0 && satData->L2 != 0.0 ) { + + int channel = 0; + if (satData->system() == 'R') { + const t_eph* eph = _ephUser->ephLast(satData->prn); + if (eph) { + channel = eph->slotNum(); + } + else { + delete satData; + return; + } + } + + t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1); + t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2); + double f1 = t_CST::freq(fType1, channel); + double f2 = t_CST::freq(fType2, channel); + double a1 = f1 * f1 / (f1 * f1 - f2 * f2); + double a2 = - f2 * f2 / (f1 * f1 - f2 * f2); + satData->L1 = satData->L1 * t_CST::c / f1; + satData->L2 = satData->L2 * t_CST::c / f2; + satData->P3 = a1 * satData->P1 + a2 * satData->P2; + satData->L3 = a1 * satData->L1 + a2 * satData->L2; + satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2; + satData->lkA = a1; + satData->lkB = a2; + _epoData->satData[satData->prn] = satData; + } + else { + delete satData; + } + } + + // Set Observations Galileo + // ------------------------ + else if (satData->system() == 'E' && _opt->useSystem('E')) { + if (satData->P1 != 0.0 && satData->P5 != 0.0 && + satData->L1 != 0.0 && satData->L5 != 0.0 ) { + double f1 = t_CST::freq(t_frequency::E1, 0); + double f5 = t_CST::freq(t_frequency::E5, 0); + double a1 = f1 * f1 / (f1 * f1 - f5 * f5); + double a5 = - f5 * f5 / (f1 * f1 - f5 * f5); + satData->L1 = satData->L1 * t_CST::c / f1; + satData->L5 = satData->L5 * t_CST::c / f5; + satData->P3 = a1 * satData->P1 + a5 * satData->P5; + satData->L3 = a1 * satData->L1 + a5 * satData->L5; + satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5; + satData->lkA = a1; + satData->lkB = a5; + _epoData->satData[satData->prn] = satData; + } + else { + delete satData; + } + } + + // Set Observations BDS + // --------------------- + else if (satData->system() == 'C' && _opt->useSystem('C')) { + if (satData->P2 != 0.0 && satData->P7 != 0.0 && + satData->L2 != 0.0 && satData->L7 != 0.0 ) { + double f2 = t_CST::freq(t_frequency::C2, 0); + double f7 = t_CST::freq(t_frequency::C7, 0); + double a2 = f2 * f2 / (f2 * f2 - f7 * f7); + double a7 = - f7 * f7 / (f2 * f2 - f7 * f7); + satData->L2 = satData->L2 * t_CST::c / f2; + satData->L7 = satData->L7 * t_CST::c / f7; + satData->P3 = a2 * satData->P2 + a7 * satData->P7; + satData->L3 = a2 * satData->L2 + a7 * satData->L7; + satData->lambda3 = a2 * t_CST::c / f2 + a7 * t_CST::c / f7; + satData->lkA = a2; + satData->lkB = a7; + _epoData->satData[satData->prn] = satData; + } + else { + delete satData; + } + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppClient::putOrbCorrections(const std::vector& corr) { + for (unsigned ii = 0; ii < corr.size(); ii++) { + QString prn = QString(corr[ii]->_prn.toInternalString().c_str()); + t_eph* eLast = _ephUser->ephLast(prn); + t_eph* ePrev = _ephUser->ephPrev(prn); + if (eLast && eLast->IOD() == corr[ii]->_iod) { + eLast->setOrbCorr(corr[ii]); + } + else if (ePrev && ePrev->IOD() == corr[ii]->_iod) { + ePrev->setOrbCorr(corr[ii]); + } + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppClient::putClkCorrections(const std::vector& corr) { + for (unsigned ii = 0; ii < corr.size(); ii++) { + QString prn = QString(corr[ii]->_prn.toInternalString().c_str()); + t_eph* eLast = _ephUser->ephLast(prn); + t_eph* ePrev = _ephUser->ephPrev(prn); + if (eLast && eLast->IOD() == corr[ii]->_iod) { + eLast->setClkCorr(corr[ii]); + } + else if (ePrev && ePrev->IOD() == corr[ii]->_iod) { + ePrev->setClkCorr(corr[ii]); + } + } +} + +// ////////////////////////////////////////////////////////////////////////////// -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(); - - 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::putCodeBiases(const std::vector& satCodeBias) { + for (unsigned ii = 0; ii < satCodeBias.size(); ii++) { + putCodeBias(new t_satCodeBias(*satCodeBias[ii])); + } } @@ -84,452 +344,76 @@ ////////////////////////////////////////////////////////////////////////////// void t_pppClient::putEphemeris(const t_eph* eph) { + bool check = _opt->_realTime; const t_ephGPS* ephGPS = dynamic_cast(eph); const t_ephGlo* ephGlo = dynamic_cast(eph); const t_ephGal* ephGal = dynamic_cast(eph); + const t_ephBDS* ephBDS = dynamic_cast(eph); if (ephGPS) { - _ephPool->putEphemeris(new t_ephGPS(*ephGPS)); + _ephUser->putNewEph(new t_ephGPS(*ephGPS), check); } else if (ephGlo) { - _ephPool->putEphemeris(new t_ephGlo(*ephGlo)); + _ephUser->putNewEph(new t_ephGlo(*ephGlo), check); } else if (ephGal) { - _ephPool->putEphemeris(new t_ephGal(*ephGal)); - } -} - -// -////////////////////////////////////////////////////////////////////////////// -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; + _ephUser->putNewEph(new t_ephGal(*ephGal), check); + } + else if (ephBDS) { + _ephUser->putNewEph(new t_ephBDS(*ephBDS), check); + } +} + +// Satellite Position +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppClient::getSatPos(const bncTime& tt, const QString& prn, + ColumnVector& xc, ColumnVector& vv) { + + t_eph* eLast = _ephUser->ephLast(prn); + t_eph* ePrev = _ephUser->ephPrev(prn); + if (eLast && eLast->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) { + return success; + } + else if (ePrev && ePrev->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) { + return success; + } + return failure; +} + +// Correct Time of Transmission +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppClient::cmpToT(t_satData* satData) { + + double prange = satData->P3; + if (prange == 0.0) { + return failure; + } + + double clkSat = 0.0; + for (int ii = 1; ii <= 10; ii++) { + + bncTime ToT = satData->tt - prange / t_CST::c - clkSat; + + ColumnVector xc(4); + ColumnVector vv(3); + if (getSatPos(ToT, satData->prn, xc, vv) != success) { 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()) ); - - // 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); - } - } -} - + + double clkSatOld = clkSat; + clkSat = xc(4); + + if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) { + satData->xx = xc.Rows(1,3); + satData->vv = vv; + satData->clk = clkSat * t_CST::c; + return success; + } + } + + return failure; +} + +void t_pppClient::putCodeBias(t_satCodeBias* satCodeBias) { + int iPrn = satCodeBias->_prn.toInt(); + delete _satCodeBiases[iPrn]; + _satCodeBiases[iPrn] = satCodeBias; +} Index: trunk/BNC/src/PPP/pppClient.h =================================================================== --- trunk/BNC/src/PPP/pppClient.h (revision 7202) +++ trunk/BNC/src/PPP/pppClient.h (revision 7203) @@ -1,78 +1,70 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + #ifndef PPPCLIENT_H #define PPPCLIENT_H -#include #include +#include + #include "pppInclude.h" -#include "ephemeris.h" #include "pppOptions.h" -#include "pppModel.h" -#include "satObs.h" +#include "pppFilter.h" -class bncAntex; +class bncEphUser; +class t_eph; 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(); + t_pppClient(const t_pppOptions* opt); + ~t_pppClient(); + void processEpoch(const std::vector& satObs, t_output* output); + void putEphemeris(const t_eph* eph); + void putOrbCorrections(const std::vector& corr); + void putClkCorrections(const std::vector& corr); + void putCodeBiases(const std::vector& satCodeBias); + std::ostringstream& log() {return *_log;} + const t_pppOptions* opt() const {return _opt;} + void putCodeBias(t_satCodeBias* satCodeBias); + const t_satCodeBias* satCodeBias(const t_prn& prn) const { + return _satCodeBiases[prn.toInt()]; + } + private: + t_irc getSatPos(const bncTime& tt, const QString& prn, ColumnVector& xc, ColumnVector& vv); + void putNewObs(t_satData* satData); + t_irc cmpToT(t_satData* satData); - void putEphemeris(const t_eph* eph); - 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_satCodeBias* _satCodeBiases[t_prn::MAXPRN+1]; + bncEphUser* _ephUser; + t_pppOptions* _opt; + t_epoData* _epoData; + t_pppFilter* _filter; + std::ostringstream* _log; }; -}; // 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()) +} // namespace #endif Index: trunk/BNC/src/PPP/pppFilter.cpp =================================================================== --- trunk/BNC/src/PPP/pppFilter.cpp (revision 7202) +++ trunk/BNC/src/PPP/pppFilter.cpp (revision 7203) @@ -1,13 +1,37 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + /* ------------------------------------------------------------------------- * BKG NTRIP Client * ------------------------------------------------------------------------- * - * Class: t_pppFilter + * Class: t_pppParam, t_pppFilter * - * Purpose: Filter Adjustment + * Purpose: Model for PPP * * Author: L. Mervart * - * Created: 29-Jul-2014 + * Created: 01-Dec-2009 * * Changes: @@ -15,435 +39,1287 @@ * -----------------------------------------------------------------------*/ -#include #include #include -#include +#include #include #include #include "pppFilter.h" +#include "pppClient.h" #include "bncutils.h" -#include "pppParlist.h" -#include "pppObsPool.h" -#include "pppStation.h" -#include "pppClient.h" +#include "bncantex.h" +#include "pppOptions.h" +#include "pppModel.h" using namespace BNC_PPP; using namespace std; +const double MAXRES_CODE = 2.98 * 3.0; +const double MAXRES_PHASE_GPS = 2.98 * 0.03; +const double MAXRES_PHASE_GLONASS = 2.98 * 0.03; +const double GLONASS_WEIGHT_FACTOR = 1.0; +const double BDS_WEIGHT_FACTOR = 2.0; + +#define LOG (_pppClient->log()) +#define OPT (_pppClient->opt()) + // Constructor //////////////////////////////////////////////////////////////////////////// -t_pppFilter::t_pppFilter() { - _parlist = 0; +t_pppParam::t_pppParam(t_pppParam::parType typeIn, int indexIn, + const QString& prnIn) { + type = typeIn; + index = indexIn; + prn = prnIn; + index_old = 0; + xx = 0.0; + numEpo = 0; } // Destructor //////////////////////////////////////////////////////////////////////////// +t_pppParam::~t_pppParam() { +} + +// Partial +//////////////////////////////////////////////////////////////////////////// +double t_pppParam::partial(t_satData* satData, bool phase) { + + Tracer tracer("t_pppParam::partial"); + + // Coordinates + // ----------- + if (type == CRD_X) { + return (xx - satData->xx(1)) / satData->rho; + } + else if (type == CRD_Y) { + return (xx - satData->xx(2)) / satData->rho; + } + else if (type == CRD_Z) { + return (xx - satData->xx(3)) / satData->rho; + } + + // Receiver Clocks + // --------------- + else if (type == RECCLK) { + return 1.0; + } + + // Troposphere + // ----------- + else if (type == TROPO) { + return 1.0 / sin(satData->eleSat); + } + + // Glonass Offset + // -------------- + else if (type == GLONASS_OFFSET) { + if (satData->prn[0] == 'R') { + return 1.0; + } + else { + return 0.0; + } + } + + // Galileo Offset + // -------------- + else if (type == GALILEO_OFFSET) { + if (satData->prn[0] == 'E') { + return 1.0; + } + else { + return 0.0; + } + } + + // BDS Offset + // ---------- + else if (type == BDS_OFFSET) { + if (satData->prn[0] == 'C') { + return 1.0; + } + else { + return 0.0; + } + } + + // Ambiguities + // ----------- + else if (type == AMB_L3) { + if (phase && satData->prn == prn) { + return 1.0; + } + else { + return 0.0; + } + } + + // Default return + // -------------- + return 0.0; +} + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_pppFilter::t_pppFilter(t_pppClient* pppClient) { + + _pppClient = pppClient; + _tides = new t_tides(); + + // Antenna Name, ANTEX File + // ------------------------ + _antex = 0; + if (!OPT->_antexFileName.empty()) { + _antex = new bncAntex(OPT->_antexFileName.c_str()); + } + + // Bancroft Coordinates + // -------------------- + _xcBanc.ReSize(4); _xcBanc = 0.0; + _ellBanc.ReSize(3); _ellBanc = 0.0; + + // Save copy of data (used in outlier detection) + // --------------------------------------------- + _epoData_sav = new t_epoData(); + + // Some statistics + // --------------- + _neu.ReSize(3); _neu = 0.0; + _numSat = 0; + _pDop = 0.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) { + delete _tides; + delete _antex; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + delete _params[iPar-1]; + } + for (int iPar = 1; iPar <= _params_sav.size(); iPar++) { + delete _params_sav[iPar-1]; + } + delete _epoData_sav; +} + +// Reset Parameters and Variance-Covariance Matrix +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::reset() { + + Tracer tracer("t_pppFilter::reset"); + + double lastTrp = 0.0; + for (int ii = 0; ii < _params.size(); ii++) { + t_pppParam* pp = _params[ii]; + if (pp->type == t_pppParam::TROPO) { + lastTrp = pp->xx; + } + delete pp; + } + _params.clear(); + + int nextPar = 0; + _params.push_back(new t_pppParam(t_pppParam::CRD_X, ++nextPar, "")); + _params.push_back(new t_pppParam(t_pppParam::CRD_Y, ++nextPar, "")); + _params.push_back(new t_pppParam(t_pppParam::CRD_Z, ++nextPar, "")); + _params.push_back(new t_pppParam(t_pppParam::RECCLK, ++nextPar, "")); + if (OPT->estTrp()) { + _params.push_back(new t_pppParam(t_pppParam::TROPO, ++nextPar, "")); + } + if (OPT->useSystem('R')) { + _params.push_back(new t_pppParam(t_pppParam::GLONASS_OFFSET, ++nextPar, "")); + } + if (OPT->useSystem('E')) { + _params.push_back(new t_pppParam(t_pppParam::GALILEO_OFFSET, ++nextPar, "")); + } + if (OPT->useSystem('C')) { + _params.push_back(new t_pppParam(t_pppParam::BDS_OFFSET, ++nextPar, "")); + } + + _QQ.ReSize(_params.size()); + _QQ = 0.0; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + t_pppParam* pp = _params[iPar-1]; + pp->xx = 0.0; + if (pp->isCrd()) { + _QQ(iPar,iPar) = OPT->_aprSigCrd(1) * OPT->_aprSigCrd(1); + } + else if (pp->type == t_pppParam::RECCLK) { + _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk; + } + else if (pp->type == t_pppParam::TROPO) { + _QQ(iPar,iPar) = OPT->_aprSigTrp * OPT->_aprSigTrp; + pp->xx = lastTrp; + } + else if (pp->type == t_pppParam::GLONASS_OFFSET) { + _QQ(iPar,iPar) = 1000.0 * 1000.0; + } + else if (pp->type == t_pppParam::GALILEO_OFFSET) { + _QQ(iPar,iPar) = 1000.0 * 1000.0; + } + else if (pp->type == t_pppParam::BDS_OFFSET) { + _QQ(iPar,iPar) = 1000.0 * 1000.0; + } + } +} + +// Bancroft Solution +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::cmpBancroft(t_epoData* epoData) { + + Tracer tracer("t_pppFilter::cmpBancroft"); + + if (int(epoData->sizeSys('G')) < OPT->_minObs) { + LOG << "t_pppFilter::cmpBancroft: not enough data\n"; 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; + + Matrix BB(epoData->sizeSys('G'), 4); + + QMapIterator it(epoData->satData); + int iObsBanc = 0; + while (it.hasNext()) { + it.next(); + t_satData* satData = it.value(); + if (satData->system() == 'G') { + ++iObsBanc; + QString prn = it.key(); + BB(iObsBanc, 1) = satData->xx(1); + BB(iObsBanc, 2) = satData->xx(2); + BB(iObsBanc, 3) = satData->xx(3); + BB(iObsBanc, 4) = satData->P3 + satData->clk; + } + } + + bancroft(BB, _xcBanc); + + // Ellipsoidal Coordinates + // ------------------------ + xyz2ell(_xcBanc.data(), _ellBanc.data()); + + // Compute Satellite Elevations + // ---------------------------- + QMutableMapIterator im(epoData->satData); + while (im.hasNext()) { + im.next(); + t_satData* satData = im.value(); + cmpEle(satData); + if (satData->eleSat < OPT->_minEle) { + delete satData; + im.remove(); + } + } + + return success; +} + +// Computed Value +//////////////////////////////////////////////////////////////////////////// +double t_pppFilter::cmpValue(t_satData* satData, bool phase) { + + Tracer tracer("t_pppFilter::cmpValue"); + + ColumnVector xRec(3); + xRec(1) = x(); + xRec(2) = y(); + xRec(3) = z(); + + double rho0 = (satData->xx - xRec).norm_Frobenius(); + double dPhi = t_CST::omega * rho0 / t_CST::c; + + xRec(1) = x() * cos(dPhi) - y() * sin(dPhi); + xRec(2) = y() * cos(dPhi) + x() * sin(dPhi); + xRec(3) = z(); + + xRec += _tides->displacement(_time, xRec); + + satData->rho = (satData->xx - xRec).norm_Frobenius(); + + double tropDelay = delay_saast(satData->eleSat) + + trp() / sin(satData->eleSat); + + double wind = 0.0; + if (phase) { + wind = windUp(satData->prn, satData->xx, xRec) * satData->lambda3; + } + + double offset = 0.0; + t_frequency::type frqA = t_frequency::G1; + t_frequency::type frqB = t_frequency::G2; + if (satData->prn[0] == 'R') { + offset = Glonass_offset(); + frqA = t_frequency::R1; + frqB = t_frequency::R2; + } + else if (satData->prn[0] == 'E') { + offset = Galileo_offset(); + //frqA = t_frequency::E1; as soon as available + //frqB = t_frequency::E5; -"- + } + else if (satData->prn[0] == 'C') { + offset = Bds_offset(); + //frqA = t_frequency::C2; as soon as available + //frqB = t_frequency::C7; -"- + } + double phaseCenter = 0.0; + if (_antex) { + bool found; + phaseCenter = satData->lkA * _antex->rcvCorr(OPT->_antNameRover, frqA, + satData->eleSat, satData->azSat, + found) + + satData->lkB * _antex->rcvCorr(OPT->_antNameRover, frqB, + satData->eleSat, satData->azSat, + found); + if (!found) { + LOG << "ANTEX: antenna >" << OPT->_antNameRover << "< not found\n"; + } + } + + double antennaOffset = 0.0; + double cosa = cos(satData->azSat); + double sina = sin(satData->azSat); + double cose = cos(satData->eleSat); + double sine = sin(satData->eleSat); + antennaOffset = -OPT->_neuEccRover(1) * cosa*cose + -OPT->_neuEccRover(2) * sina*cose + -OPT->_neuEccRover(3) * sine; + + return satData->rho + phaseCenter + antennaOffset + clk() + + offset - satData->clk + tropDelay + wind; +} + +// Tropospheric Model (Saastamoinen) +//////////////////////////////////////////////////////////////////////////// +double t_pppFilter::delay_saast(double Ele) { + + Tracer tracer("t_pppFilter::delay_saast"); + + double xyz[3]; + xyz[0] = x(); + xyz[1] = y(); + xyz[2] = z(); + double ell[3]; + xyz2ell(xyz, ell); + double height = ell[2]; + + double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225); + double TT = 18.0 - height * 0.0065 + 273.15; + double hh = 50.0 * exp(-6.396e-4 * height); + double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT); + + double h_km = height / 1000.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); + if (h_km < 0.0) h_km = 0.0; + if (h_km > 5.0) h_km = 5.0; + int ii = int(h_km + 1); + double href = ii - 1; + + double bCor[6]; + bCor[0] = 1.156; + bCor[1] = 1.006; + bCor[2] = 0.874; + bCor[3] = 0.757; + bCor[4] = 0.654; + bCor[5] = 0.563; + + double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href); + + double zen = M_PI/2.0 - Ele; + + return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen))); +} + +// Prediction Step of the Filter +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::predict(int iPhase, t_epoData* epoData) { + + Tracer tracer("t_pppFilter::predict"); + + if (iPhase == 0) { + + const double maxSolGap = 60.0; + + bool firstCrd = false; + if (!_lastTimeOK.valid() || (maxSolGap > 0.0 && _time - _lastTimeOK > maxSolGap)) { + firstCrd = true; + _startTime = epoData->tt; + reset(); + } + + // Use different white noise for Quick-Start mode + // ---------------------------------------------- + double sigCrdP_used = OPT->_noiseCrd(1); + if ( OPT->_seedingTime > 0.0 && OPT->_seedingTime > (epoData->tt - _startTime) ) { + sigCrdP_used = 0.0; + } + + // Predict Parameter values, add white noise + // ----------------------------------------- + for (int iPar = 1; iPar <= _params.size(); iPar++) { + t_pppParam* pp = _params[iPar-1]; + + // Coordinates + // ----------- + if (pp->type == t_pppParam::CRD_X) { + if (firstCrd) { + if (OPT->xyzAprRoverSet()) { + pp->xx = OPT->_xyzAprRover[0]; + } + else { + pp->xx = _xcBanc(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(); - + _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used; + } + else if (pp->type == t_pppParam::CRD_Y) { + if (firstCrd) { + if (OPT->xyzAprRoverSet()) { + pp->xx = OPT->_xyzAprRover[1]; + } + else { + pp->xx = _xcBanc(2); + } + } + _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used; + } + else if (pp->type == t_pppParam::CRD_Z) { + if (firstCrd) { + if (OPT->xyzAprRoverSet()) { + pp->xx = OPT->_xyzAprRover[2]; + } + else { + pp->xx = _xcBanc(3); + } + } + _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used; + } + + // Receiver Clocks + // --------------- + else if (pp->type == t_pppParam::RECCLK) { + pp->xx = _xcBanc(4); + for (int jj = 1; jj <= _params.size(); jj++) { + _QQ(iPar, jj) = 0.0; + } + _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk; + } + + // Tropospheric Delay + // ------------------ + else if (pp->type == t_pppParam::TROPO) { + _QQ(iPar,iPar) += OPT->_noiseTrp * OPT->_noiseTrp; + } + + // Glonass Offset + // -------------- + else if (pp->type == t_pppParam::GLONASS_OFFSET) { + pp->xx = 0.0; + for (int jj = 1; jj <= _params.size(); jj++) { + _QQ(iPar, jj) = 0.0; + } + _QQ(iPar,iPar) = 1000.0 * 1000.0; + } + + // Galileo Offset + // -------------- + else if (pp->type == t_pppParam::GALILEO_OFFSET) { + _QQ(iPar,iPar) += 0.1 * 0.1; + } + + // BDS Offset + // ---------- + else if (pp->type == t_pppParam::BDS_OFFSET) { + _QQ(iPar,iPar) = 1000.0 * 1000.0; //TODO: TEST + } + } + } + + // Add New Ambiguities if necessary + // -------------------------------- + if (OPT->ambLCs('G').size() || OPT->ambLCs('R').size() || + OPT->ambLCs('E').size() || OPT->ambLCs('C').size()) { + + // Make a copy of QQ and xx, set parameter indices + // ----------------------------------------------- + SymmetricMatrix QQ_old = _QQ; + + for (int iPar = 1; iPar <= _params.size(); iPar++) { + _params[iPar-1]->index_old = _params[iPar-1]->index; + _params[iPar-1]->index = 0; + } + + // Remove Ambiguity Parameters without observations + // ------------------------------------------------ + int iPar = 0; + QMutableVectorIterator im(_params); + while (im.hasNext()) { + t_pppParam* par = im.next(); + bool removed = false; + if (par->type == t_pppParam::AMB_L3) { + if (epoData->satData.find(par->prn) == epoData->satData.end()) { + removed = true; + delete par; + im.remove(); + } + } + if (! removed) { + ++iPar; + par->index = iPar; + } + } + + // Add new ambiguity parameters + // ---------------------------- + QMapIterator it(epoData->satData); + while (it.hasNext()) { + it.next(); + t_satData* satData = it.value(); + addAmb(satData); + } + + int nPar = _params.size(); + _QQ.ReSize(nPar); _QQ = 0.0; + for (int i1 = 1; i1 <= nPar; i1++) { + t_pppParam* p1 = _params[i1-1]; + if (p1->index_old != 0) { + _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old); + for (int i2 = 1; i2 <= nPar; i2++) { + t_pppParam* p2 = _params[i2-1]; + if (p2->index_old != 0) { + _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old); + } + } + } + } + + for (int ii = 1; ii <= nPar; ii++) { + t_pppParam* par = _params[ii-1]; + if (par->index_old == 0) { + _QQ(par->index, par->index) = OPT->_aprSigAmb * OPT->_aprSigAmb; + } + par->index_old = par->index; + } + } +} + +// Update Step of the Filter (currently just a single-epoch solution) +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::update(t_epoData* epoData) { + + Tracer tracer("t_pppFilter::update"); + + _time = epoData->tt; // current epoch time + + if (OPT->useOrbClkCorr()) { + LOG << "Precise Point Positioning of Epoch " << _time.datestr() << "_" << _time.timestr(1) + << "\n---------------------------------------------------------------\n"; + } + else { + LOG << "Single Point Positioning of Epoch " << _time.datestr() << "_" << _time.timestr(1) + << "\n--------------------------------------------------------------\n"; + } + // 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); + if (update_p(epoData) != success) { + return failure; + } + + // Set Solution Vector + // ------------------- + LOG.setf(ios::fixed); + QVectorIterator itPar(_params); + while (itPar.hasNext()) { + t_pppParam* par = itPar.next(); + if (par->type == t_pppParam::RECCLK) { + LOG << "\n clk = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) + << sqrt(_QQ(par->index,par->index)); + } + else if (par->type == t_pppParam::AMB_L3) { + ++par->numEpo; + LOG << "\n amb " << par->prn.mid(0,3).toAscii().data() << " = " + << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) + << sqrt(_QQ(par->index,par->index)) + << " nEpo = " << par->numEpo; + } + else if (par->type == t_pppParam::TROPO) { + double aprTrp = delay_saast(M_PI/2.0); + LOG << "\n trp = " << par->prn.mid(0,3).toAscii().data() + << setw(7) << setprecision(3) << aprTrp << " " + << setw(6) << setprecision(3) << showpos << par->xx << noshowpos + << " +- " << setw(6) << setprecision(3) + << sqrt(_QQ(par->index,par->index)); + } + else if (par->type == t_pppParam::GLONASS_OFFSET) { + LOG << "\n offGlo = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) + << sqrt(_QQ(par->index,par->index)); + } + else if (par->type == t_pppParam::GALILEO_OFFSET) { + LOG << "\n offGal = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) + << sqrt(_QQ(par->index,par->index)); + } + else if (par->type == t_pppParam::BDS_OFFSET) { + LOG << "\n offBds = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) + << sqrt(_QQ(par->index,par->index)); + } + } + + LOG << endl << endl; + + // Compute dilution of precision + // ----------------------------- + cmpDOP(epoData); + + // Final Message (both log file and screen) + // ---------------------------------------- + LOG << OPT->_roverName << " PPP " + << epoData->tt.timestr(1) << " " << epoData->sizeAll() << " " + << setw(14) << setprecision(3) << x() << " +- " + << setw(6) << setprecision(3) << sqrt(_QQ(1,1)) << " " + << setw(14) << setprecision(3) << y() << " +- " + << setw(6) << setprecision(3) << sqrt(_QQ(2,2)) << " " + << setw(14) << setprecision(3) << z() << " +- " + << setw(6) << setprecision(3) << sqrt(_QQ(3,3)); + + // NEU Output + // ---------- + if (OPT->xyzAprRoverSet()) { + double xyz[3]; + xyz[0] = x() - OPT->_xyzAprRover[0]; + xyz[1] = y() - OPT->_xyzAprRover[1]; + xyz[2] = z() - OPT->_xyzAprRover[2]; + + double ellRef[3]; + xyz2ell(OPT->_xyzAprRover.data(), ellRef); + xyz2neu(ellRef, xyz, _neu.data()); + + LOG << " NEU " + << setw(8) << setprecision(3) << _neu[0] << " " + << setw(8) << setprecision(3) << _neu[1] << " " + << setw(8) << setprecision(3) << _neu[2] << endl << endl; + } + else { + LOG << endl << endl; + } + + _lastTimeOK = _time; // remember time of last successful update + return success; +} + +// Outlier Detection +//////////////////////////////////////////////////////////////////////////// +QString t_pppFilter::outlierDetection(int iPhase, const ColumnVector& vv, + QMap& satData) { + + Tracer tracer("t_pppFilter::outlierDetection"); + + QString prnGPS; + QString prnGlo; + double maxResGPS = 0.0; // all the other systems except GLONASS + double maxResGlo = 0.0; // GLONASS + findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo); + + if (iPhase == 1) { + if (maxResGlo > 2.98 * OPT->_maxResL1) { + LOG << "Outlier Phase " << prnGlo.mid(0,3).toAscii().data() << ' ' << maxResGlo << endl; + return prnGlo; + } + else if (maxResGPS > 2.98 * OPT->_maxResL1) { + LOG << "Outlier Phase " << prnGPS.mid(0,3).toAscii().data() << ' ' << maxResGPS << endl; + return prnGPS; + } + } + else if (iPhase == 0 && maxResGPS > 2.98 * OPT->_maxResC1) { + LOG << "Outlier Code " << prnGPS.mid(0,3).toAscii().data() << ' ' << maxResGPS << endl; + return prnGPS; + } + + return QString(); +} + +// Phase Wind-Up Correction +/////////////////////////////////////////////////////////////////////////// +double t_pppFilter::windUp(const QString& prn, const ColumnVector& rSat, + const ColumnVector& rRec) { + + Tracer tracer("t_pppFilter::windUp"); + + double Mjd = _time.mjd() + _time.daysec() / 86400.0; + + // First time - initialize to zero + // ------------------------------- + if (!_windUpTime.contains(prn)) { + _windUpSum[prn] = 0.0; + } + + // Compute the correction for new time + // ----------------------------------- + if (!_windUpTime.contains(prn) || _windUpTime[prn] != Mjd) { + _windUpTime[prn] = Mjd; + + // Unit Vector GPS Satellite --> Receiver + // -------------------------------------- + ColumnVector rho = rRec - rSat; + rho /= rho.norm_Frobenius(); + + // GPS Satellite unit Vectors sz, sy, sx + // ------------------------------------- + ColumnVector sz = -rSat / rSat.norm_Frobenius(); + + ColumnVector xSun = t_astro::Sun(Mjd); + xSun /= xSun.norm_Frobenius(); + + ColumnVector sy = crossproduct(sz, xSun); + ColumnVector sx = crossproduct(sy, sz); + + // Effective Dipole of the GPS Satellite Antenna + // --------------------------------------------- + ColumnVector dipSat = sx - rho * DotProduct(rho,sx) + - crossproduct(rho, sy); + + // Receiver unit Vectors rx, ry + // ---------------------------- + ColumnVector rx(3); + ColumnVector ry(3); + + double recEll[3]; xyz2ell(rRec.data(), recEll) ; + double neu[3]; + + neu[0] = 1.0; + neu[1] = 0.0; + neu[2] = 0.0; + neu2xyz(recEll, neu, rx.data()); + + neu[0] = 0.0; + neu[1] = -1.0; + neu[2] = 0.0; + neu2xyz(recEll, neu, ry.data()); + + // Effective Dipole of the Receiver Antenna + // ---------------------------------------- + ColumnVector dipRec = rx - rho * DotProduct(rho,rx) + + crossproduct(rho, ry); + + // Resulting Effect + // ---------------- + double alpha = DotProduct(dipSat,dipRec) / + (dipSat.norm_Frobenius() * dipRec.norm_Frobenius()); + + if (alpha > 1.0) alpha = 1.0; + if (alpha < -1.0) alpha = -1.0; + + double dphi = acos(alpha) / 2.0 / M_PI; // in cycles + + if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) { + dphi = -dphi; + } + + _windUpSum[prn] = floor(_windUpSum[prn] - dphi + 0.5) + dphi; + } + + return _windUpSum[prn]; +} + +// +/////////////////////////////////////////////////////////////////////////// +void t_pppFilter::cmpEle(t_satData* satData) { + Tracer tracer("t_pppFilter::cmpEle"); + ColumnVector rr = satData->xx - _xcBanc.Rows(1,3); + double rho = rr.norm_Frobenius(); + + double neu[3]; + xyz2neu(_ellBanc.data(), rr.data(), neu); + + satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho ); + if (neu[2] < 0) { + satData->eleSat *= -1.0; + } + satData->azSat = atan2(neu[1], neu[0]); +} + +// +/////////////////////////////////////////////////////////////////////////// +void t_pppFilter::addAmb(t_satData* satData) { + Tracer tracer("t_pppFilter::addAmb"); + if (!OPT->ambLCs(satData->system()).size()){ + return; + } + bool found = false; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + if (_params[iPar-1]->type == t_pppParam::AMB_L3 && + _params[iPar-1]->prn == satData->prn) { + found = true; + break; + } + } + if (!found) { + t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3, + _params.size()+1, satData->prn); + _params.push_back(par); + par->xx = satData->L3 - cmpValue(satData, true); + } +} + +// +/////////////////////////////////////////////////////////////////////////// +void t_pppFilter::addObs(int iPhase, unsigned& iObs, t_satData* satData, + Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP) { + + Tracer tracer("t_pppFilter::addObs"); + + const double ELEWGHT = 20.0; + double ellWgtCoef = 1.0; + double eleD = satData->eleSat * 180.0 / M_PI; + if (eleD < ELEWGHT) { + ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0); + } + + // Remember Observation Index + // -------------------------- + ++iObs; + satData->obsIndex = iObs; + + // Phase Observations + // ------------------ + + if (iPhase == 1) { + ll(iObs) = satData->L3 - cmpValue(satData, true); + double sigL3 = 2.98 * OPT->_sigmaL1; + if (satData->system() == 'R') { + sigL3 *= GLONASS_WEIGHT_FACTOR; + } + if (satData->system() == 'C') { + sigL3 *= BDS_WEIGHT_FACTOR; + } + PP(iObs,iObs) = 1.0 / (sigL3 * sigL3) / (ellWgtCoef * ellWgtCoef); + for (int iPar = 1; iPar <= _params.size(); iPar++) { + if (_params[iPar-1]->type == t_pppParam::AMB_L3 && + _params[iPar-1]->prn == satData->prn) { + ll(iObs) -= _params[iPar-1]->xx; + } + AA(iObs, iPar) = _params[iPar-1]->partial(satData, true); + } + } + + // Code Observations + // ----------------- + else { + double sigP3 = 2.98 * OPT->_sigmaC1; + if (satData->system() == 'C') { + sigP3 *= BDS_WEIGHT_FACTOR; + } + ll(iObs) = satData->P3 - cmpValue(satData, false); + PP(iObs,iObs) = 1.0 / (sigP3 * sigP3) / (ellWgtCoef * ellWgtCoef); + for (int iPar = 1; iPar <= _params.size(); iPar++) { + AA(iObs, iPar) = _params[iPar-1]->partial(satData, false); + } + } +} + +// +/////////////////////////////////////////////////////////////////////////// +QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv, + const QMap& satDataMap) { + + Tracer tracer("t_pppFilter::printRes"); + + ostringstream str; + str.setf(ios::fixed); + bool useObs; + QMapIterator it(satDataMap); + while (it.hasNext()) { + it.next(); + t_satData* satData = it.value(); + (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() : + useObs = OPT->ambLCs(satData->system()).size(); + if (satData->obsIndex != 0 && useObs) { + str << _time.timestr(1) + << " RES " << satData->prn.mid(0,3).toAscii().data() + << (iPhase ? " L3 " : " P3 ") + << setw(9) << setprecision(4) << vv(satData->obsIndex) << endl; + } + } + + return QByteArray(str.str().c_str()); +} + +// +/////////////////////////////////////////////////////////////////////////// +void t_pppFilter::findMaxRes(const ColumnVector& vv, + const QMap& satData, + QString& prnGPS, QString& prnGlo, + double& maxResGPS, double& maxResGlo) { + + Tracer tracer("t_pppFilter::findMaxRes"); + + maxResGPS = 0.0; + maxResGlo = 0.0; + + QMapIterator it(satData); + while (it.hasNext()) { + it.next(); + t_satData* satData = it.value(); + if (satData->obsIndex != 0) { + QString prn = satData->prn; + if (prn[0] == 'R') { + if (fabs(vv(satData->obsIndex)) > maxResGlo) { + maxResGlo = fabs(vv(satData->obsIndex)); + prnGlo = prn; + } + } + else { + if (fabs(vv(satData->obsIndex)) > maxResGPS) { + maxResGPS = fabs(vv(satData->obsIndex)); + prnGPS = prn; + } + } + } + } +} + +// Update Step (private - loop over outliers) +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::update_p(t_epoData* epoData) { + + Tracer tracer("t_pppFilter::update_p"); + + // Save Variance-Covariance Matrix, and Status Vector + // -------------------------------------------------- + rememberState(epoData); + + QString lastOutlierPrn; + + // Try with all satellites, then with all minus one, etc. + // ------------------------------------------------------ + while (selectSatellites(lastOutlierPrn, epoData->satData) == success) { + + QByteArray strResCode; + QByteArray strResPhase; + + // Bancroft Solution + // ----------------- + if (cmpBancroft(epoData) != success) { + break; + } + + // First update using code observations, then phase observations + // ------------------------------------------------------------- + bool usePhase = OPT->ambLCs('G').size() || OPT->ambLCs('R').size() || + OPT->ambLCs('E').size() || OPT->ambLCs('C').size() ; + + for (int iPhase = 0; iPhase <= (usePhase ? 1 : 0); iPhase++) { + + // Status Prediction + // ----------------- + predict(iPhase, epoData); + + // Create First-Design Matrix + // -------------------------- + unsigned nPar = _params.size(); + unsigned nObs = 0; + nObs = epoData->sizeAll(); + bool useObs = false; + char sys[] ={'G', 'R', 'E', 'C'}; + for (unsigned ii = 0; ii < sizeof(sys); ii++) { + const char s = sys[ii]; + (iPhase == 0) ? useObs = OPT->codeLCs(s).size() : useObs = OPT->ambLCs(s).size(); + if (!useObs) { + nObs -= epoData->sizeSys(s); + } + } + + // Prepare first-design Matrix, vector observed-computed + // ----------------------------------------------------- + Matrix AA(nObs, nPar); // first design matrix + ColumnVector ll(nObs); // tems observed-computed + DiagonalMatrix PP(nObs); PP = 0.0; + + unsigned iObs = 0; + QMapIterator it(epoData->satData); + while (it.hasNext()) { + it.next(); + t_satData* satData = it.value(); + QString prn = satData->prn; + (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() : + useObs = OPT->ambLCs(satData->system()).size(); + if (useObs) { + addObs(iPhase, iObs, satData, AA, ll, PP); + } else { + satData->obsIndex = 0; + } + } + + // Compute Filter Update + // --------------------- + ColumnVector dx(nPar); dx = 0.0; + kalman(AA, ll, PP, _QQ, dx); + ColumnVector vv = ll - AA * dx; + + // Print Residuals + // --------------- + if (iPhase == 0) { + strResCode = printRes(iPhase, vv, epoData->satData); + } + else { + strResPhase = printRes(iPhase, vv, epoData->satData); + } + + // Check the residuals + // ------------------- + lastOutlierPrn = outlierDetection(iPhase, vv, epoData->satData); + + // No Outlier Detected + // ------------------- + if (lastOutlierPrn.isEmpty()) { + + QVectorIterator itPar(_params); + while (itPar.hasNext()) { + t_pppParam* par = itPar.next(); + par->xx += dx(par->index); + } + + if (!usePhase || iPhase == 1) { + if (_outlierGPS.size() > 0 || _outlierGlo.size() > 0) { + LOG << "Neglected PRNs: "; + if (!_outlierGPS.isEmpty()) { + LOG << _outlierGPS.last().mid(0,3).toAscii().data() << ' '; + } + QStringListIterator itGlo(_outlierGlo); + while (itGlo.hasNext()) { + QString prn = itGlo.next(); + LOG << prn.mid(0,3).toAscii().data() << ' '; + } } - ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1)); - PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC)); + LOG << strResCode.data() << strResPhase.data(); + + return success; } } - } - - // 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(); - } - } + + // Outlier Found + // ------------- else { - obs->setOutlier(); - } - } - - // Print Residuals - // --------------- + restoreState(epoData); + break; + } + + } // for iPhase + + } // while selectSatellites + + restoreState(epoData); + return failure; +} + +// Remeber Original State Vector and Variance-Covariance Matrix +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::rememberState(t_epoData* epoData) { + + _QQ_sav = _QQ; + + QVectorIterator itSav(_params_sav); + while (itSav.hasNext()) { + t_pppParam* par = itSav.next(); + delete par; + } + _params_sav.clear(); + + QVectorIterator it(_params); + while (it.hasNext()) { + t_pppParam* par = it.next(); + _params_sav.push_back(new t_pppParam(*par)); + } + + _epoData_sav->deepCopy(epoData); +} + +// Restore Original State Vector and Variance-Covariance Matrix +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::restoreState(t_epoData* epoData) { + + _QQ = _QQ_sav; + + QVectorIterator it(_params); + while (it.hasNext()) { + t_pppParam* par = it.next(); + delete par; + } + _params.clear(); + + QVectorIterator itSav(_params_sav); + while (itSav.hasNext()) { + t_pppParam* par = itSav.next(); + _params.push_back(new t_pppParam(*par)); + } + + epoData->deepCopy(_epoData_sav); +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn, + QMap& satData) { + + // First Call + // ---------- + if (lastOutlierPrn.isEmpty()) { + _outlierGPS.clear(); + _outlierGlo.clear(); + return success; + } + + // Second and next trials + // ---------------------- + else { + + if (lastOutlierPrn[0] == 'R') { + _outlierGlo << lastOutlierPrn; + } + + // Remove all Glonass Outliers + // --------------------------- + QStringListIterator it(_outlierGlo); + while (it.hasNext()) { + QString prn = it.next(); + if (satData.contains(prn)) { + delete satData.take(prn); + } + } + + if (lastOutlierPrn[0] == 'R') { + _outlierGPS.clear(); + return success; + } + + // GPS Outlier appeared for the first time - try to delete it + // ---------------------------------------------------------- + if (_outlierGPS.indexOf(lastOutlierPrn) == -1) { + _outlierGPS << lastOutlierPrn; + if (satData.contains(lastOutlierPrn)) { + delete satData.take(lastOutlierPrn); + } + return success; + } + + } + + return failure; +} + +// +//////////////////////////////////////////////////////////////////////////// +double lorentz(const ColumnVector& aa, const ColumnVector& bb) { + return aa(1)*bb(1) + aa(2)*bb(2) + aa(3)*bb(3) - aa(4)*bb(4); +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::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 { - 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) { + 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); + } + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_pppFilter::cmpDOP(t_epoData* epoData) { + + Tracer tracer("t_pppFilter::cmpDOP"); + + _numSat = 0; + _pDop = 0.0; + + if (_params.size() < 4) { 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; - } - } -} + const unsigned numPar = 4; + Matrix AA(epoData->sizeAll(), numPar); + QMapIterator it(epoData->satData); + while (it.hasNext()) { + it.next(); + t_satData* satData = it.value(); + _numSat += 1; + for (unsigned iPar = 0; iPar < numPar; iPar++) { + AA[_numSat-1][iPar] = _params[iPar]->partial(satData, false); + } + } + if (_numSat < 4) { + return; + } + AA = AA.Rows(1, _numSat); + SymmetricMatrix NN; NN << AA.t() * AA; + SymmetricMatrix QQ = NN.i(); + + _pDop = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3)); +} Index: trunk/BNC/src/PPP/pppFilter.h =================================================================== --- trunk/BNC/src/PPP/pppFilter.h (revision 7202) +++ trunk/BNC/src/PPP/pppFilter.h (revision 7203) @@ -1,94 +1,265 @@ -#ifndef FILTER_H -#define FILTER_H - -#include +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +#ifndef PPPFILTER_H +#define PPPFILTER_H + +#include +#include #include -#include "pppInclude.h" -#include "pppParlist.h" + +#include "bncconst.h" #include "bnctime.h" -#include "t_prn.h" + +class bncAntex; namespace BNC_PPP { -class t_pppParlist; -class t_pppObsPool; -class t_pppSatObs; +class t_pppClient; +class t_pppOptions; +class t_epoData; +class t_satData; +class t_tides; + +class t_satData { + public: + t_satData() { + obsIndex = 0; + P1 = 0.0; + P2 = 0.0; + P5 = 0.0; + P3 = 0.0; + L1 = 0.0; + L2 = 0.0; + L5 = 0.0; + L3 = 0.0; + lkA = 0.0; + lkB = 0.0; + } + ~t_satData() {} + bncTime tt; + QString prn; + double P1; + double P2; + double P5; + double P7; + double P3; + double L1; + double L2; + double L5; + double L7; + double L3; + ColumnVector xx; + ColumnVector vv; + double clk; + double eleSat; + double azSat; + double rho; + bool slipFlag; + double lambda3; + double lkA; + double lkB; + unsigned obsIndex; + char system() const {return prn.toAscii()[0];} +}; + +class t_epoData { + public: + t_epoData() {} + + ~t_epoData() { + clear(); + } + + void clear() { + QMapIterator it(satData); + while (it.hasNext()) { + it.next(); + delete it.value(); + } + satData.clear(); + tt.reset(); + } + + void deepCopy(const t_epoData* from) { + clear(); + tt = from->tt; + QMapIterator it(from->satData); + while (it.hasNext()) { + it.next(); + satData[it.key()] = new t_satData(*it.value()); + } + } + + unsigned sizeSys(char system) const { + unsigned ans = 0; + QMapIterator it(satData); + while (it.hasNext()) { + it.next(); + if (it.value()->system() == system) { + ++ans; + } + } + return ans; + } + unsigned sizeAll() const {return satData.size();} + + bncTime tt; + QMap satData; +}; + +class t_pppParam { + public: + enum parType {CRD_X, CRD_Y, CRD_Z, RECCLK, TROPO, AMB_L3, + GLONASS_OFFSET, GALILEO_OFFSET, BDS_OFFSET}; + t_pppParam(parType typeIn, int indexIn, const QString& prn); + ~t_pppParam(); + double partial(t_satData* satData, bool phase); + bool isCrd() const { + return (type == CRD_X || type == CRD_Y || type == CRD_Z); + } + parType type; + double xx; + int index; + int index_old; + int numEpo; + QString prn; +}; class t_pppFilter { public: - t_pppFilter(); + t_pppFilter(t_pppClient* pppClient); ~t_pppFilter(); - - t_irc processEpoch(t_pppObsPool* obsPool); - - const ColumnVector& x() const {return _xFlt;} - const SymmetricMatrix& Q() const {return _QFlt;} - + t_irc update(t_epoData* epoData); + bncTime time() const {return _time;} + const SymmetricMatrix& Q() const {return _QQ;} + const ColumnVector& neu() const {return _neu;} int numSat() const {return _numSat;} - double PDOP() const {return _dop.P;} - double GDOP() const {return _dop.G;} + double PDOP() const {return _pDop;} + double x() const {return _params[0]->xx;} + double y() const {return _params[1]->xx;} + double z() const {return _params[2]->xx;} + double clk() const {return _params[3]->xx;} + double trp0() {return delay_saast(M_PI/2.0);} 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; - }; + for (int ii = 0; ii < _params.size(); ++ii) { + t_pppParam* pp = _params[ii]; + if (pp->type == t_pppParam::TROPO) { + return pp->xx; + } + } + 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) { + for (int ii = 0; ii < _params.size(); ++ii) { + t_pppParam* pp = _params[ii]; + if (pp->type == t_pppParam::TROPO) { return sqrt(Q()[ii][ii]); } } return 0.0; - }; - + } + double Glonass_offset() const { + for (int ii = 0; ii < _params.size(); ++ii) { + t_pppParam* pp = _params[ii]; + if (pp->type == t_pppParam::GLONASS_OFFSET) { + return pp->xx; + } + } + return 0.0; + } + double Galileo_offset() const { + for (int ii = 0; ii < _params.size(); ++ii) { + t_pppParam* pp = _params[ii]; + if (pp->type == t_pppParam::GALILEO_OFFSET) { + return pp->xx; + } + } + return 0.0; + } + double Bds_offset() const { + for (int ii = 0; ii < _params.size(); ++ii) { + t_pppParam* pp = _params[ii]; + if (pp->type == t_pppParam::BDS_OFFSET) { + return pp->xx; + } + } + 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; + void reset(); + t_irc cmpBancroft(t_epoData* epoData); + void cmpEle(t_satData* satData); + void addAmb(t_satData* satData); + void addObs(int iPhase, unsigned& iObs, t_satData* satData, + Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP); + QByteArray printRes(int iPhase, const ColumnVector& vv, + const QMap& satDataMap); + void findMaxRes(const ColumnVector& vv, + const QMap& satData, + QString& prnGPS, QString& prnGlo, + double& maxResGPS, double& maxResGlo); + double cmpValue(t_satData* satData, bool phase); + double delay_saast(double Ele); + void predict(int iPhase, t_epoData* epoData); + t_irc update_p(t_epoData* epoData); + QString outlierDetection(int iPhase, const ColumnVector& vv, + QMap& satData); + + double windUp(const QString& prn, const ColumnVector& rSat, + const ColumnVector& rRec); + + bncTime _startTime; + + void rememberState(t_epoData* epoData); + void restoreState(t_epoData* epoData); + + t_irc selectSatellites(const QString& lastOutlierPrn, + QMap& satData); + + void bancroft(const Matrix& BBpass, ColumnVector& pos); + + void cmpDOP(t_epoData* epoData); + + t_pppClient* _pppClient; + bncTime _time; + bncTime _lastTimeOK; + QVector _params; + SymmetricMatrix _QQ; + QVector _params_sav; + SymmetricMatrix _QQ_sav; + t_epoData* _epoData_sav; + ColumnVector _xcBanc; + ColumnVector _ellBanc; + QMap _windUpTime; + QMap _windUpSum; + QStringList _outlierGPS; + QStringList _outlierGlo; + bncAntex* _antex; + t_tides* _tides; + ColumnVector _neu; + int _numSat; + double _pDop; }; Index: trunk/BNC/src/PPP_AR/pppClient.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppClient.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppClient.cpp (revision 7203) @@ -0,0 +1,535 @@ +/* ------------------------------------------------------------------------- + * 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(); + + 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::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()) ); + + // 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_AR/pppClient.h =================================================================== --- trunk/BNC/src/PPP_AR/pppClient.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppClient.h (revision 7203) @@ -0,0 +1,78 @@ +#ifndef PPPCLIENT_H +#define PPPCLIENT_H + +#include +#include +#include "pppInclude.h" +#include "ephemeris.h" +#include "pppOptions.h" +#include "pppModel.h" +#include "satObs.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 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; +}; + +}; // 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_AR/pppEphPool.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppEphPool.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppEphPool.cpp (revision 7203) @@ -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_AR/pppEphPool.h =================================================================== --- trunk/BNC/src/PPP_AR/pppEphPool.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppEphPool.h (revision 7203) @@ -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_AR/pppFilter.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppFilter.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppFilter.cpp (revision 7203) @@ -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_AR/pppFilter.h =================================================================== --- trunk/BNC/src/PPP_AR/pppFilter.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppFilter.h (revision 7203) @@ -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_AR/pppObsPool.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppObsPool.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppObsPool.cpp (revision 7203) @@ -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_AR/pppObsPool.h =================================================================== --- trunk/BNC/src/PPP_AR/pppObsPool.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppObsPool.h (revision 7203) @@ -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_AR/pppParlist.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppParlist.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppParlist.cpp (revision 7203) @@ -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_AR/pppParlist.h =================================================================== --- trunk/BNC/src/PPP_AR/pppParlist.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppParlist.h (revision 7203) @@ -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_AR/pppSatObs.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppSatObs.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppSatObs.cpp (revision 7203) @@ -0,0 +1,505 @@ +/* ------------------------------------------------------------------------- + * 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 + // ----------------- + // TODO + + // 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_AR/pppSatObs.h =================================================================== --- trunk/BNC/src/PPP_AR/pppSatObs.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppSatObs.h (revision 7203) @@ -0,0 +1,130 @@ +#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; + } + } + 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]; + }; + + 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_AR/pppStation.cpp =================================================================== --- trunk/BNC/src/PPP_AR/pppStation.cpp (revision 7203) +++ trunk/BNC/src/PPP_AR/pppStation.cpp (revision 7203) @@ -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_AR/pppStation.h =================================================================== --- trunk/BNC/src/PPP_AR/pppStation.h (revision 7203) +++ trunk/BNC/src/PPP_AR/pppStation.h (revision 7203) @@ -0,0 +1,49 @@ +#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;} + 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; + + 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; +}; + +} + +#endif Index: trunk/BNC/src/src.pri =================================================================== --- trunk/BNC/src/src.pri (revision 7202) +++ trunk/BNC/src/src.pri (revision 7203) @@ -117,18 +117,19 @@ } -exists(PPP) { - INCLUDEPATH += PPP - HEADERS += PPP/pppClient.h PPP/pppObsPool.h PPP/pppEphPool.h \ - PPP/pppStation.h PPP/pppFilter.h PPP/pppParlist.h \ - PPP/pppSatObs.h - SOURCES += PPP/pppClient.cpp PPP/pppObsPool.cpp PPP/pppEphPool.cpp \ - PPP/pppStation.cpp PPP/pppFilter.cpp PPP/pppParlist.cpp \ - PPP/pppSatObs.cpp +exists(PPP_AR) { + INCLUDEPATH += PPP_AR + DEFINES += USE_PPP_AR + HEADERS += PPP_AR/pppClient.h PPP_AR/pppObsPool.h PPP_AR/pppEphPool.h \ + PPP_AR/pppStation.h PPP_AR/pppFilter.h PPP_AR/pppParlist.h \ + PPP_AR/pppSatObs.h + SOURCES += PPP_AR/pppClient.cpp PPP_AR/pppObsPool.cpp PPP_AR/pppEphPool.cpp \ + PPP_AR/pppStation.cpp PPP_AR/pppFilter.cpp PPP_AR/pppParlist.cpp \ + PPP_AR/pppSatObs.cpp } else { - INCLUDEPATH += PPP_SSR_I - DEFINES += USE_PPP_SSR_I - HEADERS += PPP_SSR_I/pppClient.h PPP_SSR_I/pppFilter.h - SOURCES += PPP_SSR_I/pppClient.cpp PPP_SSR_I/pppFilter.cpp + INCLUDEPATH += PPP + DEFINES += USE_PPP + HEADERS += PPP/pppClient.h PPP/pppFilter.h + SOURCES += PPP/pppClient.cpp PPP/pppFilter.cpp }