Index: trunk/BNC/src/PPP/ephpool.cpp =================================================================== --- trunk/BNC/src/PPP/ephpool.cpp (revision 5743) +++ trunk/BNC/src/PPP/ephpool.cpp (revision 5743) @@ -0,0 +1,155 @@ +// 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_pppMain + * + * Purpose: Buffer with satellite ephemerides + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include "ephpool.h" +#include "ppp.h" + +using namespace BNC; +using namespace std; + +// +///////////////////////////////////////////////////////////////////////////// +void t_ephPool::putEphemeris(t_eph* eph) { + if (eph && eph->isOK()) { + _satEphPool[eph->prn().toInt()].putEphemeris(_maxQueueSize, eph); + } + else { + delete eph; + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_ephPool::putOrbCorrection(t_orbCorr* corr) { + if (corr) { + _satEphPool[corr->prn().toInt()].putOrbCorrection(corr); + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_ephPool::putClkCorrection(t_clkCorr* corr) { + if (corr) { + _satEphPool[corr->prn().toInt()].putClkCorrection(corr); + } +} + +// +///////////////////////////////////////////////////////////////////////////// +t_irc t_ephPool::getCrd(const t_prn& prn, const bncTime& tt, + ColumnVector& xc, ColumnVector& vv) const { + return _satEphPool[prn.toInt()].getCrd(tt, xc, vv); +} + +// +///////////////////////////////////////////////////////////////////////////// +int t_ephPool::getChannel(const t_prn& prn) const { + return _satEphPool[prn.toInt()].getChannel(); +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_ephPool::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_ephPool::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_ephPool::t_satEphPool::putClkCorrection(t_clkCorr* corr) { + for (unsigned ii = 0; ii < _ephs.size(); ii++) { + t_eph* eph = _ephs[ii]; + if (eph->IOD() == corr->IOD() || corr->absClk()) { + eph->setClkCorr(corr); + } + } + delete corr; +} + +// +///////////////////////////////////////////////////////////////////////////// +t_irc t_ephPool::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); + if (irc == t_irc::success) { + if (eph->prn().system() == 'R') { + double age = tt - eph->toc(); + if (fabs(age) > 3600.0) { + continue; + } + } + return irc; + } + } + return t_irc::failure; +} + +// +///////////////////////////////////////////////////////////////////////////// +int t_ephPool::t_satEphPool::getChannel() const { + if (_ephs.size() > 0) { + return _ephs[0]->getChannel(); + } + return 0; +} Index: trunk/BNC/src/PPP/ephpool.h =================================================================== --- trunk/BNC/src/PPP/ephpool.h (revision 5743) +++ trunk/BNC/src/PPP/ephpool.h (revision 5743) @@ -0,0 +1,56 @@ +#ifndef EPHPOOL_H +#define EPHPOOL_H + +#include +#include "ppp.h" +#include "bnctime.h" +#include "ephemeris.h" + +namespace BNC { + +class t_ephPool { + public: + t_ephPool(unsigned maxQueueSize = 3) { + _maxQueueSize = maxQueueSize; + } + ~t_ephPool() {}; + + 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/filter.cpp =================================================================== --- trunk/BNC/src/PPP/filter.cpp (revision 5743) +++ trunk/BNC/src/PPP/filter.cpp (revision 5743) @@ -0,0 +1,500 @@ +#include +#include +#include +#include +#include + +#include "filter.h" +#include "parlist.h" +#include "ambres.h" +#include "obspool.h" +#include "kalman.h" +#include "station.h" + +using namespace BNC; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_filter::t_filter() { + _parlist = 0; +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_filter::~t_filter() { + delete _parlist; +} + +// Process Single Epoch +//////////////////////////////////////////////////////////////////////////// +t_irc t_filter::processEpoch(t_obsPool* obsPool) { + + _ambFixRate = 0.0; + _numSat = 0; + + if (!_parlist) { + _parlist = new t_parlist(); + } + + // Vector of all Observations + // -------------------------- + t_obsPool::t_epoch* epoch = obsPool->lastEpoch(); + if (!epoch) { + return t_irc::failure; + } + vector& obsVector = epoch->obsVector(); + + // Time of the Epoch + // ----------------- + _epoTime = epoch->epoTime(); + + // Auxiliary vectors of processed linear combinations + // -------------------------------------------------- + vector LCsCode; + vector LCsPhase; + for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) { + const t_lc::type& tLC = OPT->LCs()[ii]; + if (t_lc::includesCode(tLC) && !t_lc::includesPhase(tLC)) { + LCsCode.push_back(tLC); + } + else { + LCsPhase.push_back(tLC); + } + } + vector ambLCs; + if (LCsPhase.size() == 1) { + ambLCs.push_back(LCsPhase[0]); + } + else if (LCsPhase.size() > 1) { + ambLCs.push_back(t_lc::l1); + ambLCs.push_back(t_lc::l2); + } + + // Set Parameters + // -------------- + _parlist->set(_epoTime, ambLCs, obsVector); + 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_param* 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_param* par2 = params[jj]; + int jOld = par2->indexOld(); + if (jOld >= 0) { + _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1); + } + } + } + } + + // Process LCs containing code separately + // -------------------------------------- + for (unsigned ipc = 0; ipc <= 1; ipc++) { + const vector& LCsHlp = (ipc == 0 ? LCsCode : LCsPhase); + if (LCsHlp.size() > 0) { + if ( processLC(LCsHlp, obsVector) != t_irc::success ) { + return t_irc::failure; + } + } + } + + // Copy Float Solution + // ------------------- + _QFix = _QFlt; + _xFix = _xFlt; + + _ambFixRate = 0.0; + + // Constrain ZD Ambiguities + // ------------------------ + if (LCsPhase.size() > 0) { + Matrix HH(LCsPhase.size(), params.size()); HH = 0.0; + ColumnVector hh(LCsPhase.size()); hh = 0.0; + DiagonalMatrix Sl(LCsPhase.size()); Sl = 1.e-4; + for (unsigned iLC = 0; iLC < LCsPhase.size(); iLC++) { + const t_param* ambPar = 0; + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + const t_param* par = params[iPar]; + if (par->type() == t_param::amb && par->tLC() == LCsPhase[iLC] && + par->indexOld() != -1 && !par->ambResetCandidate()) { + if (ambPar == 0 || ambPar->ambEleSat() < par->ambEleSat()) { + ambPar = par; + } + } + } + if (ambPar) { + HH[iLC][ambPar->indexNew()] = 1.0; + hh[iLC] = xFltOld[ambPar->indexOld()]; + } + } + kalman(_QFix, _xFix, HH, hh, Sl); + } + + // Ambiguity Resolution + // -------------------- + if (OPT->ambres()) { + if (ambLCs.size() > 1) { + t_ambres::ambres(_epoTime, ambLCs, _parlist, _QFix, _xFix, true); + } + _ambFixRate = t_ambres::ambres(_epoTime, ambLCs, _parlist, _QFix, _xFix, false); + + // Check/Print Fixed Residuals + // --------------------------- + bool checkResOK = true; + string epoTimeStr = string(_epoTime); + for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) { + const t_lc::type& tLC = OPT->LCs()[ii]; + for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) { + const t_satObs* obs = obsVector[iObs]; + if (obs->isValid() && !obs->outlier()) { + ColumnVector AA(params.size()); + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + const t_param* 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, _xFix) - ll; + + bool outlier = false; + if (fabs(vv) > (t_lc::includesCode(tLC) ? 1.5 * OPT->maxResCode() : 1.5 * OPT->maxResPhase())) { + outlier = true; + checkResOK = false; + } + + if (OPT->logLevel() > 1) { + LOG << epoTimeStr << " XRES " + << left << setw(3) << t_lc::toString(tLC) << right << ' ' + << obs->prn().toString() << ' ' + << setw(8) << setprecision(4) << vv; + if (outlier) { + LOG << " outlier AR"; + } + LOG << endl; + } + } + } + } + if (!checkResOK || _ambFixRate < OPT->ambresMinFixRate()) { + _ambFixRate = 0.0; + _xFix = _xFlt; + _QFix = _QFlt; + } + } + + _parlist->printResult(_epoTime, _QFix, _xFix, _ambFixRate); + + return t_irc::success; +} + +// Process Selected LCs +//////////////////////////////////////////////////////////////////////////// +t_irc t_filter::processLC(const vector& LCs, + vector& obsVector) { + + LOG.setf(ios::fixed); + + // Detect Cycle Slips + // ------------------ + if (detectCycleSlips(LCs, obsVector) != t_irc::success) { + return t_irc::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); + UpperTriangularMatrix Sl(maxObs); Sl = 0.0; + + int iObs = -1; + vector usedObs; + vector usedTypes; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + t_satObs* obs = obsVector[ii]; + if (!obs->outlier()) { + Matrix CC(LCs.size(), 4); + 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_param* 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)); + if (LCs.size() > 1) { + ColumnVector coeff(4); + obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff); + CC[jj][0] = coeff[0] * obs->sigma(t_lc::l1); + CC[jj][1] = coeff[1] * obs->sigma(t_lc::l2); + CC[jj][2] = coeff[2] * obs->sigma(t_lc::c1); + CC[jj][3] = coeff[3] * obs->sigma(t_lc::c2); + } + else { + Sl[iObs][iObs] = obs->sigma(tLC); + } + } + if (LCs.size() > 1) { + SymmetricMatrix QQ; QQ << CC * CC.t(); + Sl.SymSubMatrix(iObs-LCs.size()+2, iObs+1) = Cholesky(QQ).t(); + } + } + } + + // Check number of observations, truncate matrices + // ----------------------------------------------- + if (iObs+1 < OPT->minobs()) { + return t_irc::failure; + } + AA = AA.Rows(1, iObs+1); + ll = ll.Rows(1, iObs+1); + Sl = Sl.SymSubMatrix(1, iObs+1); + + // Kalman update step + // ------------------ + kalman(_QFlt, _xFlt, AA, ll, Sl); + + // 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 > + (t_lc::includesCode(tLC) ? OPT->maxResCode() : OPT->maxResPhase())) { + if (res > fabs(maxOutlier)) { + maxOutlier = vv[ii]; + maxOutlierIndex = ii; + maxOutlierLC = tLC; + } + } + } + + // Mark outlier or break outlier detection loop + // -------------------------------------------- + if (maxOutlierIndex > -1) { + t_satObs* obs = usedObs[maxOutlierIndex]; + t_param* 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_param* hlp = params[iPar]; + if (hlp->type() == t_param::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_satObs* obs = usedObs[ii]; + if (tLC == LCs[jj]) { + obs->setRes(tLC, vv[ii]); + if (OPT->logLevel() > 1) { + LOG << epoTimeStr << " RES " + << left << setw(3) << t_lc::toString(tLC) << right << ' ' + << obs->prn().toString() << ' ' + << setw(8) << setprecision(4) << vv[ii] << endl; + } + } + } + } + cmpDOP(LCs, AA); + break; + } + } + + return t_irc::success; +} + +// Cycle-Slip Detection +//////////////////////////////////////////////////////////////////////////// +t_irc t_filter::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_satObs* 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_param* 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 t_irc::success; +} + +// Reset Ambiguity Parameter (cycle slip) +//////////////////////////////////////////////////////////////////////////// +t_irc t_filter::resetAmb(t_prn prn, const vector& obsVector, + SymmetricMatrix* QSav, ColumnVector* xSav) { + t_irc irc = t_irc::failure; + vector& params = _parlist->params(); + for (unsigned iPar = 0; iPar < params.size(); iPar++) { + t_param* par = params[iPar]; + if (par->type() == t_param::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_param(t_param::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 = t_irc::success; + } + } + + return irc; +} + +// Compute various DOP Values +//////////////////////////////////////////////////////////////////////////// +void t_filter::cmpDOP(const std::vector& LCs, const Matrix& AA) { + + _dop.reset(); + _numSat = 0; + try { + _numSat = AA.Nrows() / LCs.size(); + + if (_numSat < 4) { + return; + } + + Matrix BB(_numSat, 4); + + for (int ii = 1; ii <= _numSat; ii++) { + BB.Row(ii) = AA.Row(ii*LCs.size()).columns(1,4); + } + + SymmetricMatrix NN; NN << BB.t() * BB; + 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 (...) { + } +} Index: trunk/BNC/src/PPP/filter.h =================================================================== --- trunk/BNC/src/PPP/filter.h (revision 5743) +++ trunk/BNC/src/PPP/filter.h (revision 5743) @@ -0,0 +1,74 @@ +#ifndef FILTER_H +#define FILTER_H + +#include +#include +#include "ppp.h" +#include "parlist.h" +#include "bnctime.h" +#include "t_prn.h" + +namespace BNC { + +class t_parlist; +class t_obsPool; +class t_satObs; + +class t_filter { + public: + t_filter(); + ~t_filter(); + + t_irc processEpoch(t_obsPool* 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;} + + 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 processLC(const std::vector& LCs, 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& LCs, const Matrix& AA); + + bncTime _epoTime; + t_parlist* _parlist; + SymmetricMatrix _QFlt; + ColumnVector _xFlt; + ColumnVector _x0; + t_slip _slips[t_prn::MAXPRN+1]; + int _numSat; + t_dop _dop; +}; + +} + +#endif Index: trunk/BNC/src/PPP/obspool.cpp =================================================================== --- trunk/BNC/src/PPP/obspool.cpp (revision 5743) +++ trunk/BNC/src/PPP/obspool.cpp (revision 5743) @@ -0,0 +1,101 @@ +// 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_pppMain + * + * Purpose: Buffer with observations + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include "obspool.h" + +using namespace BNC; +using namespace std; + +// Constructor +///////////////////////////////////////////////////////////////////////////// +t_obsPool::t_epoch::t_epoch(const t_time& epoTime, vector& obsVector) { + _epoTime = epoTime; + for (unsigned ii = 0; ii < obsVector.size(); ii++) { + _obsVector.push_back(obsVector[ii]); + } + obsVector.clear(); +} + +// Destructor +///////////////////////////////////////////////////////////////////////////// +t_obsPool::t_epoch::~t_epoch() { + for (unsigned ii = 0; ii < _obsVector.size(); ii++) { + delete _obsVector[ii]; + } +} + +// Constructor +///////////////////////////////////////////////////////////////////////////// +t_obsPool::t_obsPool() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + _satBiases[ii] = 0; + } +} + +// Destructor +///////////////////////////////////////////////////////////////////////////// +t_obsPool::~t_obsPool() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + delete _satBiases[ii]; + } + while (_epochs.size() > 0) { + delete _epochs.front(); + _epochs.pop_front(); + } +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_obsPool::putBiases(t_satBias* satBias) { + int iPrn = satBias->prn().toInt(); + delete _satBiases[iPrn]; + _satBiases[iPrn] = satBias; +} + +// +///////////////////////////////////////////////////////////////////////////// +void t_obsPool::putEpoch(const t_time& 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/obspool.h =================================================================== --- trunk/BNC/src/PPP/obspool.h (revision 5743) +++ trunk/BNC/src/PPP/obspool.h (revision 5743) @@ -0,0 +1,52 @@ +#ifndef OBSPOOL_H +#define OBSPOOL_H + +#include +#include +#include "satobs.h" +#include "satbias.h" +#include "bnctime.h" + +namespace BNC { + +class t_obsPool { + 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_obsPool(); + ~t_obsPool(); + void putBiases(t_satBias* satBias); + + void putEpoch(const bncTime& epoTime, std::vector& obsVector); + + const t_satBias* satBias(const t_prn& prn) const { + return _satBiases[prn.toInt()]; + } + t_epoch* lastEpoch() { + if (_epochs.size()) { + return _epochs.back(); + } + else { + return 0; + } + } + + private: + t_satBias* _satBiases[t_prn::MAXPRN+1]; + std::deque _epochs; +}; + +} + +#endif Index: trunk/BNC/src/PPP/parlist.cpp =================================================================== --- trunk/BNC/src/PPP/parlist.cpp (revision 5743) +++ trunk/BNC/src/PPP/parlist.cpp (revision 5743) @@ -0,0 +1,450 @@ +#include +#include +#include +#include +#include +#include +#include "parlist.h" +#include "satobs.h" +#include "station.h" +#include "tropo.h" +#include "iono.h" +#include "utils.h" +#include "genconst.h" + +using namespace BNC; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_param::t_param(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 = true; + _sigma0 = OPT->sigmaCrd(); + break; + case crdY: + _epoSpec = true; + _sigma0 = OPT->sigmaCrd(); + break; + case crdZ: + _epoSpec = true; + _sigma0 = OPT->sigmaCrd(); + break; + case clkR: + _epoSpec = true; + _sigma0 = 1000.0; + break; + case amb: + _ambInfo = new t_ambInfo(); + if (obsVector) { + for (unsigned ii = 0; ii < obsVector->size(); ii++) { + const t_satObs* obs = obsVector->at(ii); + if (obs->prn() == _prn) { + double offGG = 0; + if (_prn.system() == 'R' && tLC != t_lc::MW) { + offGG = PPP_CLIENT->offGG(); + } + _x0 = nint((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC)); + break; + } + } + } + _epoSpec = false; + _sigma0 = 100.0; + break; + case offGG: + _epoSpec = true; + _sigma0 = 1000.0; + _x0 = PPP_CLIENT->offGG(); + break; + case trp: + _epoSpec = false; + _sigma0 = OPT->sigmaTropo(); + _noise = OPT->noiseTropo(); + break; + case iono: + if (OPT->noiseIono() > 0.0) { + _epoSpec = false; + } + else { + _epoSpec = true; + } + _sigma0 = OPT->sigmaIono(); + _noise = OPT->noiseIono(); + break; + case bias: + _epoSpec = true; + _sigma0 = 10.0; + break; + } +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_param::~t_param() { + delete _ambInfo; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_param::partial(const bncTime& epoTime, const t_satObs* obs, + const t_lc::type& tLC) const { + + // Special Case - Melbourne-Wuebbena + // --------------------------------- + if (tLC == t_lc::MW && _type != amb && _type != bias) { + return 0.0; + } + + const t_station* 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 { + ColumnVector coeff(4); + obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff); + if (_tLC == t_lc::l1) { + return obs->lambda(t_lc::l1) * coeff(1); + } + else if (_tLC == t_lc::l2) { + return obs->lambda(t_lc::l2) * coeff(2); + } + } + } + return 0.0; + case trp: + return t_tropo::mf(OPT->tropoMF(), epoTime, sta->ellApr()[0], + sta->ellApr()[1], sta->ellApr()[2], obs->eleSat()); + case iono: + if (obs->prn() == _prn) { + return t_iono::dLCdI(tLC, obs->prn().system(), obs->channel()); + } + else { + return 0.0; + } + case bias: + if (tLC == _tLC && obs->prn().system() == _prn.system()) { + return 1.0; + } + else { + return 0.0; + } + } + + return 0.0; +} + +// +//////////////////////////////////////////////////////////////////////////// +string t_param::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; + case iono: + ss << "IONO " << _prn.toString(); + break; + case bias: + ss << "BIAS " << _prn.system() << ' ' << left << setw(3) << t_lc::toString(_tLC); + break; + } + return ss.str(); +} + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_parlist::t_parlist() { +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_parlist::~t_parlist() { + for (unsigned ii = 0; ii < _params.size(); ii++) { + delete _params[ii]; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_parlist::set(const bncTime& epoTime, const vector& ambLCs, + const vector& obsVector) { + + // Remove some Parameters + // ---------------------- + vector::iterator it = _params.begin(); + while (it != _params.end()) { + t_param* par = *it; + + bool remove = false; + + if (par->epoSpec()) { + remove = true; + } + + else if (par->type() == t_param::amb) { + if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) { + remove = true; + } + } + + else if (par->type() == t_param::amb || par->type() == t_param::iono) { + 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_param* 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_satObs* 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_param(t_param::crdX, t_prn(), t_lc::dummy)); + required.push_back(new t_param(t_param::crdY, t_prn(), t_lc::dummy)); + required.push_back(new t_param(t_param::crdZ, t_prn(), t_lc::dummy)); + + // Receiver Clock + // -------------- + required.push_back(new t_param(t_param::clkR, t_prn(), t_lc::dummy)); + + // GPS-Glonass Clock Offset + // ------------------------ + if (OPT->useGlonass()) { + required.push_back(new t_param(t_param::offGG, t_prn(), t_lc::dummy)); + } + + // Receiver Biases + // --------------- + for (unsigned ii = 0; ii < OPT->estBias().size(); ii++) { + const t_options::t_optBias& optBias = OPT->estBias()[ii]; + required.push_back(new t_param(t_param::bias, t_prn(optBias._system, 1), optBias._tLC)); + } + + // Troposphere + // ----------- + if (OPT->estTropo()) { + required.push_back(new t_param(t_param::trp, t_prn(), t_lc::dummy)); + } + + // Ambiguities + // ----------- + for (unsigned ii = 0; ii < ambLCs.size(); ii++) { + const t_lc::type& tLC = ambLCs[ii]; + for (unsigned jj = 0; jj < obsVector.size(); jj++) { + const t_satObs* satObs = obsVector[jj]; + required.push_back(new t_param(t_param::amb, satObs->prn(), tLC, &obsVector)); + } + } + + // Stochastic Ionosphere + //---------------------- + if (OPT->estIono()) { + for (unsigned jj = 0; jj < obsVector.size(); jj++) { + const t_satObs* satObs = obsVector[jj]; + required.push_back(new t_param(t_param::iono, satObs->prn(), t_lc::dummy)); + } + } + + // Check if all required parameters are present + // -------------------------------------------- + for (unsigned ii = 0; ii < required.size(); ii++) { + t_param* parReq = required[ii]; + + bool found = false; + for (unsigned jj = 0; jj < _params.size(); jj++) { + t_param* 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_param::sortFunction); + + for (unsigned ii = 0; ii < _params.size(); ii++) { + t_param* par = _params[ii]; + par->setIndex(ii); + for (unsigned jj = 0; jj < obsVector.size(); jj++) { + const t_satObs* satObs = obsVector[jj]; + if (satObs->prn() == par->prn()) { + par->setAmbEleSat(satObs->eleSat()); + par->stepAmbNumEpo(); + } + } + } + + return t_irc::success; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_parlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ, + const ColumnVector& xx, double ambFixRate) const { + + string epoTimeStr = string(epoTime); + + LOG << endl; + + t_param* parX = 0; + t_param* parY = 0; + t_param* parZ = 0; + for (unsigned ii = 0; ii < _params.size(); ii++) { + t_param* par = _params[ii]; + if (par->type() == t_param::crdX) { + parX = par; + } + else if (par->type() == t_param::crdY) { + parY = par; + } + else if (par->type() == t_param::crdZ) { + parZ = par; + } + else { + int ind = par->indexNew(); + LOG << epoTimeStr << ' ' << par->toString() << ' ' + << setw(10) << setprecision(4) << par->x0() << ' ' + << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- " + << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]); + if (par->type() == t_param::amb) { + LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * t_genConst::rho_deg + << " epo = " << setw(4) << par->ambNumEpo(); + } + LOG << endl; + } + } + + if (parX && parY && parZ) { + const t_station* sta = PPP_CLIENT->staRover(); + + 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 + << " 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]); + if (ambFixRate > 0.0) { + LOG << " fix "; + } + else { + LOG << " flt "; + } + LOG << int(100*ambFixRate) << " %\n"; + } +} + Index: trunk/BNC/src/PPP/parlist.h =================================================================== --- trunk/BNC/src/PPP/parlist.h (revision 5743) +++ trunk/BNC/src/PPP/parlist.h (revision 5743) @@ -0,0 +1,113 @@ +#ifndef PARLIST_H +#define PARLIST_H + +#include +#include +#include "ppp.h" +#include "t_prn.h" +#include "bnctime.h" + +namespace BNC { + +class t_satObs; + +class t_param { + public: + enum e_type {crdX, crdY, crdZ, clkR, amb, offGG, trp, iono, bias}; + + t_param(e_type type, const t_prn& prn, t_lc::type tLC, + const std::vector* obsVector = 0); + + ~t_param(); + e_type type() const {return _type;} + double x0() const {return _x0;} + double partial(const bncTime& epoTime, const t_satObs* obs, + const t_lc::type& tLC) const; + bool epoSpec() const {return _epoSpec;} + bool isEqual(const t_param* 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_param* p1, const t_param* 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_parlist { + public: + t_parlist(); + ~t_parlist(); + + t_irc set(const bncTime& epoTime, const std::vector& ambLCs, + 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, double ambFixRate) const; + private: + std::vector _params; +}; + +} + +#endif Index: trunk/BNC/src/PPP/ppp.h =================================================================== --- trunk/BNC/src/PPP/ppp.h (revision 5742) +++ trunk/BNC/src/PPP/ppp.h (revision 5743) @@ -17,5 +17,5 @@ }; -class t_pppOutput { +class t_output { public: bncTime _epoTime; @@ -44,5 +44,5 @@ }; -class t_pppSatObs { +class t_satObs { public: t_prn _prn; Index: trunk/BNC/src/PPP/pppClient.cpp =================================================================== --- trunk/BNC/src/PPP/pppClient.cpp (revision 5742) +++ trunk/BNC/src/PPP/pppClient.cpp (revision 5743) @@ -63,5 +63,5 @@ // Global Variable ////////////////////////////////////////////////////////////////////////////// -t_pppClient* pppClient = 0; +t_pppClient* PPP_CLIENT = 0; // Constructor Index: trunk/BNC/src/PPP/pppClient.h =================================================================== --- trunk/BNC/src/PPP/pppClient.h (revision 5742) +++ trunk/BNC/src/PPP/pppClient.h (revision 5743) @@ -68,11 +68,11 @@ /// Pointer to the main object -extern BNC::t_pppClient* pppClient; +extern BNC::t_pppClient* PPP_CLIENT; /// Log stream abbreviation -#define LOG (*pppClient->_log) +#define LOG (*PPP_CLIENT->_log) /// Options abbreviation -#define OPT (pppClient->_opt) +#define OPT (PPP_CLIENT->_opt) #endif Index: trunk/BNC/src/PPP/satbias.cpp =================================================================== --- trunk/BNC/src/PPP/satbias.cpp (revision 5743) +++ trunk/BNC/src/PPP/satbias.cpp (revision 5743) @@ -0,0 +1,58 @@ +// 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_pppMain + * + * Purpose: Satellite-specific biases + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include "satbias.h" + +using namespace BNC; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_satBias::t_satBias(const t_satBiases& satBiases) { + _prn.set(satBiases._satellite._system, satBiases._satellite._number); + _time.setmjd(satBiases._time._sec, satBiases._time._mjd); + _nx = satBiases._nx; + _jumpCount = satBiases._jumpCount; + for (int ii = 0; ii < satBiases._numBiases; ii++) { + const t_pppBias& bias = satBiases._biases[ii]; + t_biasType biasType = string(bias._rnxType3ch).substr(0,3); + _biases[biasType] = bias._value; + } +} Index: trunk/BNC/src/PPP/satbias.h =================================================================== --- trunk/BNC/src/PPP/satbias.h (revision 5743) +++ trunk/BNC/src/PPP/satbias.h (revision 5743) @@ -0,0 +1,30 @@ +#ifndef SATBIAS_H +#define SATBIAS_H + +#include +#include "ppp.h" +#include "bnctime.h" + +namespace BNC { + +typedef std::string t_biasType; + +class t_satBias { + public: + t_satBias(const t_satBiases& satBiases); + ~t_satBias() {} + const t_prn& prn() const {return _prn;} + const std::map& biases() const {return _biases;} + int nx() const {return _nx;} + int jumpCount() const {return _jumpCount;} + private: + t_prn _prn; + bncTime _time; + int _nx; + int _jumpCount; + std::map _biases; +}; + +} + +#endif Index: trunk/BNC/src/PPP/satobs.cpp =================================================================== --- trunk/BNC/src/PPP/satobs.cpp (revision 5743) +++ trunk/BNC/src/PPP/satobs.cpp (revision 5743) @@ -0,0 +1,575 @@ +// 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_satObs + * + * Purpose: Satellite observations + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + + +#include +#include +#include +#include "satobs.h" +#include "genconst.h" +#include "ephpool.h" +#include "station.h" +#include "utils.h" +#include "tropo.h" +#include "antex.h" +#include "obspool.h" + +using namespace BNC; +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_satObs::t_obs::t_obs(const t_obs& obs) { + _type = obs._rnxType2ch; + _code = obs._code; + _codeValid = obs._codeValid; + _phase = obs._phase; + _phaseValid = obs._phaseValid; + _doppler = obs._doppler; + _dopplerValid = obs._dopplerValid; + _snr = obs._snr; + _snrValid = obs._snrValid; + _slip = obs._slip; + _slipCounter = obs._slipCounter; + _biasJumpCounter = -1; +} + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_satObs::t_satObs(const t_satObs& satObs) { + _prn = satObs._prn; + _time = satObs._time; + _outlier = false; + for (int ii = 0; ii < satObs._numObs; ii++) { + const t_obs& obs = satObs._obs[ii]; + t_obsType obsType = string(obs._rnxType2ch).substr(0,2); + _allObs[obsType] = new t_obs(obs); + } + prepareObs(); +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_satObs::~t_satObs() { + map::const_iterator it; + for (it = _allObs.begin(); it != _allObs.end(); it++) { + delete it->second; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_satObs::prepareObs() { + _model.reset(); + _valid = true; + _validObs1 = 0; + _validObs2 = 0; + + bool dualFreq = OPT->dualFreqRequired(); + + // Select two pseudoranges and two phase observations + // -------------------------------------------------- + const string preferredAttrib = "CWP"; + for (unsigned iPref = 0; iPref < preferredAttrib.length(); iPref++) { + t_obsType obsType1 = "1?"; + obsType1[1] = preferredAttrib[iPref]; + if (_validObs1 == 0 && _allObs.find(obsType1) != _allObs.end()) { + t_obs* obs = _allObs[obsType1]; + if (obs->_codeValid && obs->_phaseValid) { + _validObs1 = obs; + } + } + if (dualFreq) { + t_obsType obsType2 = "2?"; + obsType2[1] = preferredAttrib[iPref]; + if (_validObs2 == 0 && _allObs.find(obsType2) != _allObs.end()) { + t_obs* obs = _allObs[obsType2]; + if (obs->_codeValid && obs->_phaseValid) { + _validObs2 = obs; + } + } + } + } + + if (_validObs1 == 0 || (dualFreq && _validObs2 == 0)) { + _valid = false; + return; + } + + // Find Glonass Channel Number + // --------------------------- + if (_prn.system() == 'R') { + _channel = PPP_CLIENT->ephPool()->getChannel(_prn); + } + else { + _channel = 0; + } + + // Copy raw observations + // --------------------- + _f1 = t_genConst::f1(_prn.system(), _channel); + _rawC1 = _validObs1->_code; + _rawL1 = _validObs1->_phase * t_genConst::c / _f1; + if (dualFreq) { + _f2 = t_genConst::f2(_prn.system(), _channel); + _rawC2 = _validObs2->_code; + _rawL2 = _validObs2->_phase * t_genConst::c / _f2; + } + else { + _f2 = 0.0; + _rawC2 = 0.0; + _rawL2 = 0.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 = (dualFreq ? t_lc::cIF : t_lc::c1); + double prange = obsValue(tLC); + for (int ii = 1; ii <= 10; ii++) { + bncTime ToT = _time - prange / t_genConst::c - _xcSat[3]; + if (PPP_CLIENT->ephPool()->getCrd(_prn, ToT, _xcSat, _vvSat) != t_irc::success) { + _valid = false; + return; + } + ColumnVector dx = _xcSat - satPosOld; + dx[3] *= t_genConst::c; + if (dx.norm_Frobenius() < 1.e-4) { + totOK = true; + break; + } + satPosOld = _xcSat; + } + if (totOK) { + _model._satClkM = _xcSat[3] * t_genConst::c; + } + else { + _valid = false; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_satObs::cmpModel(const t_station* 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_genConst::c; + + // Receiver Clocks + // --------------- + _model._recClkM = station->dClk() * t_genConst::c; + + // Sagnac Effect (correction due to Earth rotation) + // ------------------------------------------------ + ColumnVector Omega(3); + Omega[0] = 0.0; + Omega[1] = 0.0; + Omega[2] = t_genConst::omega / t_genConst::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()) { + t_frequency::type frq1 = t_frequency::G1; + t_frequency::type frq2 = t_frequency::G2; + if (_prn.system() == 'R') { + frq1 = t_frequency::R1; + frq2 = t_frequency::R2; + } + bool found; + _model._antPco1 = PPP_CLIENT->antex()->rcvCorr(frq1, station->antName(), + _model._eleSat, found); + _model._antPco2 = PPP_CLIENT->antex()->rcvCorr(frq2, station->antName(), + _model._eleSat, found); + } + + // Tropospheric Delay + // ------------------ + t_tropo::dtrop(_time, station->ellApr()[0], station->ellApr()[1], + station->ellApr()[2], _model._eleSat, OPT->tropoModel(), + OPT->tropoMF(), false, _model._tropo); + + // Phase Wind-Up + // ------------- + _model._windUp = station->windUp(_time, _prn, rSat); + + // Code (and Phase) Biases + // ----------------------- + bool biasC1flg = false; + bool biasC2flg = false; + bool biasL1flg = false; + bool biasL2flg = false; + int nsdfix = 0; + const t_satBias* satBias = PPP_CLIENT->obsPool()->satBias(_prn); + if (satBias) { + nsdfix = satBias->nx(); + map::const_iterator it; + for (it = satBias->biases().begin(); it != satBias->biases().end(); it++) { + const t_biasType& biasType = it->first; + if (_validObs1) { + _validObs1->_biasJumpCounter = satBias->jumpCount(); + if ("C" + _validObs1->_type == biasType) { + _model._biasC1 = it->second; + biasC1flg = true; + } + else if ("L" + _validObs1->_type == biasType) { + _model._biasL1 = it->second; + biasL1flg = true; + } + } + if (_validObs2) { + _validObs2->_biasJumpCounter = satBias->jumpCount(); + if ("C" + _validObs2->_type == biasType) { + _model._biasC2 = it->second; + biasC2flg = true; + } + else if ("L" + _validObs2->_type == biasType) { + _model._biasL2 = it->second; + biasL2flg = true; + } + } + } + } + if (_prn.system() == 'G' && OPT->biasRequired()) { + if (!biasC1flg || !biasC2flg || !biasL1flg || !biasL2flg) { + _valid = false; + } + if (nsdfix < OPT->minSDFix()) { + _valid = false; + } + } + + // Tidal Correction + // ---------------- + _model._tide = -DotProduct(station->tideDspl(), rhoV) / _model._rho; + + // Ionospheric Delay + // ----------------- + // TODO + + // Ocean Loading + // ------------- + // TODO + + // Set Model Set Flag + // ------------------ + _model._set = true; + + return t_irc::success; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_satObs::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 * t_genConst::rho_deg << endl + << "AZI: " << setw(12) << setprecision(3) << _model._azSat * t_genConst::rho_deg << 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 + << "PCO1: " << setw(12) << setprecision(3) << _model._antPco1 << endl + << "PCO2: " << setw(12) << setprecision(3) << _model._antPco2 << endl + << "TROPO: " << setw(12) << setprecision(3) << _model._tropo << endl + << "WINDUP: " << setw(12) << setprecision(3) << _model._windUp << endl + << "BIASC1: " << setw(12) << setprecision(3) << _model._biasC1 << endl + << "BIASC2: " << setw(12) << setprecision(3) << _model._biasC2 << endl + << "BIASL1: " << setw(12) << setprecision(3) << _model._biasL1 << endl + << "BIASL2: " << setw(12) << setprecision(3) << _model._biasL2 << endl + << "TIDES: " << setw(12) << setprecision(3) << _model._tide << endl; + + //// beg test + LOG << "PCO L3: " << setw(12) << setprecision(3) + << lc(t_lc::lIF, _model._antPco1, _model._antPco2, 0.0, 0.0) << endl; + + LOG << "WIND L3:" << setw(12) << setprecision(3) + << lc(t_lc::lIF, _model._windUp * t_genConst::c / _f1, + _model._windUp * t_genConst::c / _f2, 0.0, 0.0) << 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; + //// end test +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::obsValue(t_lc::type tLC) const { + + if (!_validObs2 && t_lc::need2ndFreq(tLC)) { + return 0.0; + } + + return this->lc(tLC, _rawL1, _rawL2, _rawC1, _rawC2); +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::cmpValueForBanc(t_lc::type tLC) const { + return cmpValue(tLC) - _model._rho - _model._sagnac - _model._recClkM; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::cmpValue(t_lc::type tLC) const { + + if (!_validObs2 && t_lc::need2ndFreq(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 + // ------------------- + double L1 = nonDisp + _model._antPco1 - _model._biasL1 + _model._windUp * t_genConst::c / _f1; + double L2 = nonDisp + _model._antPco2 - _model._biasL2 + _model._windUp * t_genConst::c / _f2; + double C1 = nonDisp + _model._antPco1 - _model._biasC1; + double C2 = nonDisp + _model._antPco2 - _model._biasC2; + + return this->lc(tLC, L1, L2, C1, C2); +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::lc(t_lc::type tLC, + double L1, double L2, double C1, double C2, + ColumnVector* coeff) const { + + if (coeff) { + coeff->ReSize(4); + (*coeff) = 0.0; + } + + if (tLC == t_lc::l1) { + if (coeff) (*coeff)(1) = 1.0; + return L1; + } + else if (tLC == t_lc::l2) { + if (coeff) (*coeff)(2) = 1.0; + return L2; + } + else if (tLC == t_lc::c1) { + if (coeff) (*coeff)(3) = 1.0; + return C1; + } + else if (tLC == t_lc::c2) { + if (coeff) (*coeff)(4) = 1.0; + return C2; + } + else if (tLC == t_lc::lIF || tLC == t_lc::cIF) { + double a1 = _f1 * _f1 / (_f1 * _f1 - _f2 * _f2); + double a2 = -_f2 * _f2 / (_f1 * _f1 - _f2 * _f2); + if (tLC == t_lc::lIF) { + if (coeff) { + (*coeff)(1) = a1; + (*coeff)(2) = a2; + } + return a1 * L1 + a2 * L2; + } + else { + if (coeff) { + (*coeff)(3) = a1; + (*coeff)(4) = a2; + } + return a1 * C1 + a2 * C2; + } + } + else if (tLC == t_lc::MW) { + double a1 = _f1 / (_f1 - _f2); + double a2 = -_f2 / (_f1 - _f2); + double a3 = -_f1 / (_f1 + _f2); + double a4 = -_f2 / (_f1 + _f2); + if (coeff) { + (*coeff)(1) = a1; + (*coeff)(2) = a2; + (*coeff)(3) = a3; + (*coeff)(4) = a4; + } + return a1 * L1 + a2 * L2 + a3 * C1 + a4 * C2; + } + else if (tLC == t_lc::CL) { + if (coeff) { + (*coeff)(1) = 0.5; + (*coeff)(3) = 0.5; + } + return (C1 + L1) / 2.0; + } + + return 0.0; +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_satObs::createDifference(const t_satObs* obsBase) { + + _rawC1 -= obsBase->_rawC1; + _rawC2 -= obsBase->_rawC2; + _rawL1 -= obsBase->_rawL1; + _rawL2 -= obsBase->_rawL2; + _model._rho -= obsBase->_model._rho; + _model._recClkM -= obsBase->_model._recClkM; + _model._satClkM -= obsBase->_model._satClkM; + _model._sagnac -= obsBase->_model._sagnac; + _model._antEcc -= obsBase->_model._antEcc; + _model._tropo -= obsBase->_model._tropo; + _model._tide -= obsBase->_model._tide; + _model._windUp -= obsBase->_model._windUp; + _model._antPco1 -= obsBase->_model._antPco1; + _model._antPco2 -= obsBase->_model._antPco2; + _model._biasC1 -= obsBase->_model._biasC1; + _model._biasC2 -= obsBase->_model._biasC2; + _model._biasL1 -= obsBase->_model._biasL1; + _model._biasL2 -= obsBase->_model._biasL2; + + return t_irc::success; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::lambda(t_lc::type tLC) const { + + if (tLC == t_lc::l1) { + return t_genConst::c / _f1; + } + else if (tLC == t_lc::l2) { + return t_genConst::c / _f2; + } + else if (tLC == t_lc::lIF) { + return t_genConst::c / (_f1 + _f2); + } + else if (tLC == t_lc::MW) { + return t_genConst::c / (_f1 - _f2); + } + else if (tLC == t_lc::CL) { + return t_genConst::c / _f1 / 2.0; + } + + return 0.0; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::sigma(t_lc::type tLC) const { + + ColumnVector sig(4); + sig(1) = OPT->sigmaPhase(); + sig(2) = OPT->sigmaPhase(); + sig(3) = OPT->sigmaCode(); + sig(4) = OPT->sigmaCode(); + + ColumnVector coeff(4); + lc(tLC, sig(1), sig(2), sig(3), sig(4), &coeff); + + ColumnVector sp = SP(sig, coeff); // Schur product + + // 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/t_genConst::pi; + double hlp = fabs(90.0 - eleD); + cEle = (1.0 + hlp*hlp*hlp*0.000004); + } + + return cEle * sp.norm_Frobenius(); +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_satObs::setRes(t_lc::type tLC, double res) { + _res[tLC] = res; +} + +// +//////////////////////////////////////////////////////////////////////////// +double t_satObs::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/satobs.h =================================================================== --- trunk/BNC/src/PPP/satobs.h (revision 5743) +++ trunk/BNC/src/PPP/satobs.h (revision 5743) @@ -0,0 +1,142 @@ +#ifndef SATOBS_H +#define SATOBS_H + +#include +#include +#include +#include "ppp.h" +#include "bnctime.h" + +namespace BNC { + +typedef std::string t_obsType; + +class t_station; + +class t_satObs { + public: + t_satObs(const t_pppSatObs& pppSatObs); + ~t_satObs(); + bool isValid() const {return _valid;} + void prepareObs(); + const t_prn& prn() const {return _prn;} + const ColumnVector& xc() const {return _xcSat;} + const bncTime& time() const {return _time;} + t_irc cmpModel(const t_station* station); + double obsValue(t_lc::type tLC) 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; + t_irc createDifference(const t_satObs* obsBase); + double lc(t_lc::type tLC, double L1, double L2, + double C1, double C2, ColumnVector* coeff = 0) const; + double lambda(t_lc::type tLC) const; + double sigma(t_lc::type tLC) const; + bool outlier() const {return _outlier;} + void setOutlier() {_outlier = true;} + int channel() const {return _channel;} + void setRes(t_lc::type tLC, double res); + double getRes(t_lc::type tLC) const; + + bool slip() const { + std::map::const_iterator it; + for (it = _allObs.begin(); it != _allObs.end(); it++) { + if (it->second->_slip) { + return true; + } + } + return false; + } + + int slipCounter() const { + int cnt = -1; + std::map::const_iterator it; + for (it = _allObs.begin(); it != _allObs.end(); it++) { + if (it->second->_slipCounter > cnt) { + cnt = it->second->_slipCounter; + } + } + return cnt; + } + + int biasJumpCounter() const { + int jmp = -1; + std::map::const_iterator it; + for (it = _allObs.begin(); it != _allObs.end(); it++) { + if (it->second->_biasJumpCounter > jmp) { + jmp = it->second->_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; + _antPco1 = 0.0; + _antPco2 = 0.0; + _biasC1 = 0.0; + _biasC2 = 0.0; + _biasL1 = 0.0; + _biasL2 = 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 _antPco1; + double _antPco2; + double _biasC1; + double _biasC2; + double _biasL1; + double _biasL2; + }; + + t_prn _prn; + bncTime _time; + int _channel; + std::map _allObs; + bool _valid; + t_obs* _validObs1; + t_obs* _validObs2; + double _f1; + double _f2; + double _rawC1; + double _rawC2; + double _rawL1; + double _rawL2; + ColumnVector _xcSat; + ColumnVector _vvSat; + t_model _model; + bool _outlier; + std::map _res; +}; + +} + +#endif Index: trunk/BNC/src/PPP/station.cpp =================================================================== --- trunk/BNC/src/PPP/station.cpp (revision 5743) +++ trunk/BNC/src/PPP/station.cpp (revision 5743) @@ -0,0 +1,107 @@ +// 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_station + * + * Purpose: Processed station + * + * Author: L. Mervart + * + * Created: 29-Jul-2014 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include "station.h" +#include "bncutils.h" +#include "windup.h" + +using namespace BNC; +using namespace std; + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_station::t_station() { + _windUp = new t_windUp(); +} + +// Destructor +////////////////////////////////////////////////////////////////////////////// +t_station::~t_station() { + delete _windUp; +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_station::setXyzApr(const ColumnVector& xyzApr) { + _xyzApr = xyzApr; + _ellApr.ReSize(3); + xyz2ell(_xyzApr.data(), _ellApr.data()); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_station::setNeuEcc(const ColumnVector& neuEcc) { + _neuEcc = neuEcc; + _xyzEcc.ReSize(3); + neu2xyz(_ellApr.data(), _neuEcc.data(), _xyzEcc.data()); +} + +// +////////////////////////////////////////////////////////////////////////////// +double t_station::windUp(const bncTime& time, t_prn prn, + const ColumnVector& rSat) const { + return _windUp->value(time, _xyzApr, prn, rSat); +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_station::checkRestriction(const bncTime& tt) { + + const double maxSpeed = 515.0; // [m/s^2] + const double maxHeight = 18200.0; // [m] + + if (_ellApr(3) > maxHeight) { + throw "t_station: height restriction"; + } + + if (tt.valid()) { + if (_timeCheck.valid()) { + double dt = tt - _timeCheck; + if (dt > 0.0) { + double vel = (_xyzApr - _xyzCheck).norm_Frobenius() / dt; + if (vel > maxSpeed) { + throw "t_station: speed restriction"; + } + } + } + _timeCheck = tt; + _xyzCheck = _xyzApr; + } +} Index: trunk/BNC/src/PPP/station.h =================================================================== --- trunk/BNC/src/PPP/station.h (revision 5743) +++ trunk/BNC/src/PPP/station.h (revision 5743) @@ -0,0 +1,50 @@ +#ifndef STATION_H +#define STATION_H + +#include +#include +#include "ppp.h" +#include "bnctime.h" + +namespace BNC { + +class t_windUp; + +class t_station { + public: + t_station(); + ~t_station(); + 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; + void checkRestriction(const bncTime& tt); + + 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/PPP/windup.cpp =================================================================== --- trunk/BNC/src/PPP/windup.cpp (revision 5743) +++ trunk/BNC/src/PPP/windup.cpp (revision 5743) @@ -0,0 +1,96 @@ + +#include + +#include "bncconst.h" +#include "windup.h" +#include "astro.h" +#include "bncutils.h" + +using namespace std; +using namespace BNC; + +// Constructor +/////////////////////////////////////////////////////////////////////////// +t_windUp::t_windUp() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + sumWind[ii] = 0.0; + lastEtime[ii] = 0.0; + } +} + +// Phase Wind-Up Correction +/////////////////////////////////////////////////////////////////////////// +double t_windUp::value(const bncTime& etime, const ColumnVector& rRec, + t_prn prn, const ColumnVector& rSat) { + + if (etime.mjddec() != lastEtime[prn.toInt()]) { + + // 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(etime); + 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 / t_genConst::pi; // in cycles + + if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) { + dphi = -dphi; + } + + if (lastEtime[prn.toInt()] == 0.0) { + sumWind[prn.toInt()] = dphi; + } + else { + sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi; + } + + lastEtime[prn.toInt()] = etime.mjddec(); + } + + return sumWind[prn.toInt()]; +} Index: trunk/BNC/src/PPP/windup.h =================================================================== --- trunk/BNC/src/PPP/windup.h (revision 5743) +++ trunk/BNC/src/PPP/windup.h (revision 5743) @@ -0,0 +1,27 @@ + +#ifndef WINDUP_H +#define WINDUP_H + +#include + +#include "ppp.h" +#include "bncTime.h" + +namespace BNC { + +class t_windUp { + public: + t_windUp(); + ~t_windUp() {}; + + double value(const bncTime& etime, const ColumnVector& rRec, t_prn prn, + const ColumnVector& rSat); + + private: + double lastEtime[t_prn::MAXPRN+1]; + double sumWind[t_prn::MAXPRN+1]; +}; + +} + +#endif