Index: /trunk/BNC/src/PPP_SSR_I/pppClient.cpp =================================================================== --- /trunk/BNC/src/PPP_SSR_I/pppClient.cpp (revision 7235) +++ /trunk/BNC/src/PPP_SSR_I/pppClient.cpp (revision 7235) @@ -0,0 +1,405 @@ +// 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_pppClient + * + * Purpose: Precise Point Positioning + * + * Author: L. Mervart + * + * Created: 21-Nov-2009 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include + +#include "pppClient.h" +#include "pppUtils.h" +#include "bncephuser.h" +#include "bncutils.h" + +using namespace BNC_PPP; +using namespace std; + +// 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); + _pppUtils = new t_pppUtils(); +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_pppClient::~t_pppClient() { + delete _filter; + delete _epoData; + delete _opt; + delete _ephUser; + delete _log; + delete _pppUtils; +} + +// +//////////////////////////////////////////////////////////////////////////// +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 = _pppUtils->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]); + } + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppClient::putCodeBiases(const std::vector& satCodeBias) { + for (unsigned ii = 0; ii < satCodeBias.size(); ii++) { + _pppUtils->putCodeBias(new t_satCodeBias(*satCodeBias[ii])); + } +} + +// +////////////////////////////////////////////////////////////////////////////// +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) { + _ephUser->putNewEph(new t_ephGPS(*ephGPS), check); + } + else if (ephGlo) { + _ephUser->putNewEph(new t_ephGlo(*ephGlo), check); + } + else if (ephGal) { + _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; + } + + 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; +} Index: /trunk/BNC/src/PPP_SSR_I/pppClient.h =================================================================== --- /trunk/BNC/src/PPP_SSR_I/pppClient.h (revision 7235) +++ /trunk/BNC/src/PPP_SSR_I/pppClient.h (revision 7235) @@ -0,0 +1,68 @@ +// 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 "pppInclude.h" +#include "pppOptions.h" +#include "pppFilter.h" +#include "pppUtils.h" + +class bncEphUser; +class t_eph; + +namespace BNC_PPP { + +class t_pppClient : public interface_pppClient { + public: + t_pppClient(const t_pppOptions* opt); + ~t_pppClient(); + void processEpoch(const std::vector& satObs, t_output* output); + void putEphemeris(const t_eph* eph); + void putTec(const t_vTec* vTec); + void putOrbCorrections(const std::vector& corr); + void putClkCorrections(const std::vector& corr); + void putCodeBiases(const std::vector& satCodeBias); + std::ostringstream& log() {return *_log;} + const t_pppOptions* opt() const {return _opt;} + + 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); + bncEphUser* _ephUser; + t_pppOptions* _opt; + t_epoData* _epoData; + t_pppFilter* _filter; + t_pppUtils* _pppUtils; + std::ostringstream* _log; +}; + +} // namespace + +#endif Index: /trunk/BNC/src/PPP_SSR_I/pppFilter.cpp =================================================================== --- /trunk/BNC/src/PPP_SSR_I/pppFilter.cpp (revision 7235) +++ /trunk/BNC/src/PPP_SSR_I/pppFilter.cpp (revision 7235) @@ -0,0 +1,1325 @@ +// 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_pppParam, t_pppFilter + * + * Purpose: Model for PPP + * + * Author: L. Mervart + * + * Created: 01-Dec-2009 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include + +#include "pppFilter.h" +#include "pppClient.h" +#include "bncutils.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_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 _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; + } + + 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; + + 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); + } + } + _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 + // ---------------------- + 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() << ' '; + } + } + LOG << strResCode.data() << strResPhase.data(); + + return success; + } + } + + // Outlier Found + // ------------- + else { + 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 { + 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; + } + + 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_SSR_I/pppFilter.h =================================================================== --- /trunk/BNC/src/PPP_SSR_I/pppFilter.h (revision 7235) +++ /trunk/BNC/src/PPP_SSR_I/pppFilter.h (revision 7235) @@ -0,0 +1,268 @@ +// 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 "bncconst.h" +#include "bnctime.h" + +class bncAntex; + +namespace BNC_PPP { + +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_pppClient* pppClient); + ~t_pppFilter(); + 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 _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 { + 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 { + 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: + 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; +}; + +} + +#endif Index: /trunk/BNC/src/PPP_SSR_I/pppUtils.cpp =================================================================== --- /trunk/BNC/src/PPP_SSR_I/pppUtils.cpp (revision 7235) +++ /trunk/BNC/src/PPP_SSR_I/pppUtils.cpp (revision 7235) @@ -0,0 +1,47 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: t_pppUtils + * + * Purpose: Auxiliary Functions for PPP + * + * Author: A. Stürze + * + * Created: 18-Aug-2015 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include "pppUtils.h" +#include "bncutils.h" +#include "pppModel.h" + +using namespace BNC_PPP; + + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_pppUtils::t_pppUtils() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + _satCodeBiases[ii] = 0; + } +} + +// Destructor +////////////////////////////////////////////////////////////////////////////// +t_pppUtils::~t_pppUtils() { + for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) { + delete _satCodeBiases[ii]; + } +} + +// +////////////////////////////////////////////////////////////////////////////// +void t_pppUtils::putCodeBias(t_satCodeBias* satCodeBias) { + int iPrn = satCodeBias->_prn.toInt(); + delete _satCodeBiases[iPrn]; + _satCodeBiases[iPrn] = satCodeBias; +} + Index: /trunk/BNC/src/PPP_SSR_I/pppUtils.h =================================================================== --- /trunk/BNC/src/PPP_SSR_I/pppUtils.h (revision 7235) +++ /trunk/BNC/src/PPP_SSR_I/pppUtils.h (revision 7235) @@ -0,0 +1,25 @@ +#ifndef PPPUTILS_H +#define PPPUTILS_H + +#include +#include +#include "satObs.h" + +namespace BNC_PPP { + +class t_pppUtils { + public: + t_pppUtils(); + ~t_pppUtils(); + void putCodeBias(t_satCodeBias* satCodeBias); + const t_satCodeBias* satCodeBias(const t_prn& prn) const { + return _satCodeBiases[prn.toInt()]; + } + + private: + t_satCodeBias* _satCodeBiases[t_prn::MAXPRN+1]; +}; + +} + +#endif