Index: trunk/BNC/src/combination/bnccomb.cpp =================================================================== --- trunk/BNC/src/combination/bnccomb.cpp (revision 4281) +++ trunk/BNC/src/combination/bnccomb.cpp (revision 4281) @@ -0,0 +1,1167 @@ +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: bncComb + * + * Purpose: Combinations of Orbit/Clock Corrections + * + * Author: L. Mervart + * + * Created: 22-Jan-2011 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include + +#include "bnccomb.h" +#include "bncapp.h" +#include "upload/bncrtnetdecoder.h" +#include "bncsettings.h" +#include "bncmodel.h" +#include "bncutils.h" +#include "bncpppclient.h" +#include "bncsp3.h" +#include "bncantex.h" +#include "bnctides.h" + +const double sig0_offAC = 1000.0; +const double sig0_offACSat = 100.0; +const double sigP_offACSat = 0.01; +const double sig0_clkSat = 100.0; + +const double sigObs = 0.05; + +const int MAXPRN_GPS = 32; +const int MAXPRN_GLONASS = 24; + +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +cmbParam::cmbParam(parType type_, int index_, + const QString& ac_, const QString& prn_) { + + type = type_; + index = index_; + AC = ac_; + prn = prn_; + xx = 0.0; + eph = 0; + + if (type == offACgps) { + epoSpec = true; + sig0 = sig0_offAC; + sigP = sig0; + } + else if (type == offACglo) { + epoSpec = true; + sig0 = sig0_offAC; + sigP = sig0; + } + else if (type == offACSat) { + epoSpec = false; + sig0 = sig0_offACSat; + sigP = sigP_offACSat; + } + else if (type == clkSat) { + epoSpec = true; + sig0 = sig0_clkSat; + sigP = sig0; + } +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +cmbParam::~cmbParam() { +} + +// Partial +//////////////////////////////////////////////////////////////////////////// +double cmbParam::partial(const QString& AC_, const QString& prn_) { + + if (type == offACgps) { + if (AC == AC_ && prn_[0] == 'G') { + return 1.0; + } + } + else if (type == offACglo) { + if (AC == AC_ && prn_[0] == 'R') { + return 1.0; + } + } + else if (type == offACSat) { + if (AC == AC_ && prn == prn_) { + return 1.0; + } + } + else if (type == clkSat) { + if (prn == prn_) { + return 1.0; + } + } + + return 0.0; +} + +// +//////////////////////////////////////////////////////////////////////////// +QString cmbParam::toString() const { + + QString outStr; + + if (type == offACgps) { + outStr = "AC offset GPS " + AC; + } + else if (type == offACglo) { + outStr = "AC offset GLO " + AC; + } + else if (type == offACSat) { + outStr = "Sat Offset " + AC + " " + prn; + } + else if (type == clkSat) { + outStr = "Clk Corr " + prn; + } + + return outStr; +} + +// Constructor +//////////////////////////////////////////////////////////////////////////// +bncComb::bncComb() { + + bncSettings settings; + + QStringList combineStreams = settings.value("combineStreams").toStringList(); + + _cmbSampl = settings.value("cmbSampl").toInt(); + if (_cmbSampl <= 0) { + _cmbSampl = 10; + } + + _masterMissingEpochs = 0; + + if (combineStreams.size() >= 1 && !combineStreams[0].isEmpty()) { + QListIterator it(combineStreams); + while (it.hasNext()) { + QStringList hlp = it.next().split(" "); + cmbAC* newAC = new cmbAC(); + newAC->mountPoint = hlp[0]; + newAC->name = hlp[1]; + newAC->weight = hlp[2].toDouble(); + if (_masterOrbitAC.isEmpty()) { + _masterOrbitAC = newAC->name; + } + _ACs.append(newAC); + } + } + + _rtnetDecoder = 0; + + connect(this, SIGNAL(newMessage(QByteArray,bool)), + ((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool))); + + // Combination Method + // ------------------ + if (settings.value("cmbMethod").toString() == "Single-Epoch") { + _method = singleEpoch; + } + else { + _method = filter; + } + + // Use Glonass + // ----------- + if ( Qt::CheckState(settings.value("pppGLONASS").toInt()) == Qt::Checked) { + _useGlonass = true; + } + else { + _useGlonass = false; + } + + // Initialize Parameters (model: Clk_Corr = AC_Offset + Sat_Offset + Clk) + // ---------------------------------------------------------------------- + if (_method == filter) { + int nextPar = 0; + QListIterator it(_ACs); + while (it.hasNext()) { + cmbAC* AC = it.next(); + _params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC->name, "")); + for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) { + QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0')); + _params.push_back(new cmbParam(cmbParam::offACSat, ++nextPar, + AC->name, prn)); + } + if (_useGlonass) { + _params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC->name, "")); + for (int iGlo = 1; iGlo <= MAXPRN_GLONASS; iGlo++) { + QString prn = QString("R%1").arg(iGlo, 2, 10, QChar('0')); + _params.push_back(new cmbParam(cmbParam::offACSat, ++nextPar, + AC->name, prn)); + } + } + } + for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) { + QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0')); + _params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn)); + } + if (_useGlonass) { + for (int iGlo = 1; iGlo <= MAXPRN_GLONASS; iGlo++) { + QString prn = QString("R%1").arg(iGlo, 2, 10, QChar('0')); + _params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn)); + } + } + + // Initialize Variance-Covariance Matrix + // ------------------------------------- + _QQ.ReSize(_params.size()); + _QQ = 0.0; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + _QQ(iPar,iPar) = pp->sig0 * pp->sig0; + } + } + + // ANTEX File + // ---------- + _antex = 0; + QString antexFileName = settings.value("pppAntex").toString(); + if (!antexFileName.isEmpty()) { + _antex = new bncAntex(); + if (_antex->readFile(antexFileName) != success) { + emit newMessage("wrong ANTEX file", true); + delete _antex; + _antex = 0; + } + } + + // Maximal Residuum + // ---------------- + _MAXRES = settings.value("cmbMaxres").toDouble(); + if (_MAXRES <= 0.0) { + _MAXRES = 999.0; + } +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +bncComb::~bncComb() { + QListIterator icAC(_ACs); + while (icAC.hasNext()) { + delete icAC.next(); + } + delete _rtnetDecoder; + delete _antex; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + delete _params[iPar-1]; + } + QListIterator itTime(_buffer.keys()); + while (itTime.hasNext()) { + bncTime epoTime = itTime.next(); + _buffer.remove(epoTime); + } + QMapIterator itOrbCorr(_orbitCorrs); + while (itOrbCorr.hasNext()) { + itOrbCorr.next(); + delete itOrbCorr.value(); + } +} + +// Read and store one correction line +//////////////////////////////////////////////////////////////////////////// +void bncComb::processCorrLine(const QString& staID, const QString& line) { + QMutexLocker locker(&_mutex); + + // Find the AC Name + // ---------------- + QString acName; + QListIterator icAC(_ACs); + while (icAC.hasNext()) { + cmbAC* AC = icAC.next(); + if (AC->mountPoint == staID) { + acName = AC->name; + break; + } + } + if (acName.isEmpty()) { + return; + } + + // Read the Correction + // ------------------- + cmbCorr* newCorr = new cmbCorr(); + newCorr->acName = acName; + if (!newCorr->readLine(line) == success) { + delete newCorr; + return; + } + + // Check Glonass + // ------------- + if (!_useGlonass) { + if (newCorr->prn[0] == 'R') { + delete newCorr; + return; + } + } + + // Save Orbit-Only Corrections + // --------------------------- + if (newCorr->tRao.valid() && !newCorr->tClk.valid()) { + QString corrID = newCorr->ID(); + if (_orbitCorrs.find(corrID) != _orbitCorrs.end()) { + delete _orbitCorrs[corrID]; + } + _orbitCorrs[corrID] = newCorr; + return; + } + + // Merge with saved orbit correction + // --------------------------------- + else if (newCorr->tClk.valid() && !newCorr->tRao.valid()) { + QString corrID = newCorr->ID(); + if (_orbitCorrs.find(corrID) != _orbitCorrs.end()) { + mergeOrbitCorr(_orbitCorrs[corrID], newCorr); + } + } + + // Check Modulo Time + // ----------------- + if (int(newCorr->tClk.gpssec()) % _cmbSampl != 0.0) { + delete newCorr; + return; + } + + // Delete old corrections + // ---------------------- + if (_resTime.valid() && newCorr->tClk <= _resTime) { + delete newCorr; + return; + } + + // Check the Ephemeris + //-------------------- + if (_eph.find(newCorr->prn) == _eph.end()) { + delete newCorr; + return; + } + else { + t_eph* lastEph = _eph[newCorr->prn]->last; + t_eph* prevEph = _eph[newCorr->prn]->prev; + if (lastEph && lastEph->IOD() == newCorr->iod) { + newCorr->eph = lastEph; + } + else if (lastEph && prevEph && prevEph->IOD() == newCorr->iod) { + newCorr->eph = prevEph; + switchToLastEph(lastEph, newCorr); + } + else { + delete newCorr; + return; + } + } + + // Process previous Epoch(s) + // ------------------------- + QListIterator itTime(_buffer.keys()); + while (itTime.hasNext()) { + bncTime epoTime = itTime.next(); + if (epoTime < newCorr->tClk - _cmbSampl) { + _resTime = epoTime; + processEpoch(); + } + } + + // Merge or add the correction + // --------------------------- + QVector& corrs = _buffer[newCorr->tClk].corrs; + cmbCorr* existingCorr = 0; + QVectorIterator itCorr(corrs); + while (itCorr.hasNext()) { + cmbCorr* hlp = itCorr.next(); + if (hlp->prn == newCorr->prn && hlp->acName == newCorr->acName) { + existingCorr = hlp; + break; + } + } + if (existingCorr) { + delete newCorr; newCorr = 0; + existingCorr->readLine(line); // merge (multiple messages) + + } + else { + corrs.append(newCorr); + } +} + +// Change the correction so that it refers to last received ephemeris +//////////////////////////////////////////////////////////////////////////// +void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) { + + if (corr->eph == lastEph) { + return; + } + + ColumnVector oldXC(4); + ColumnVector oldVV(3); + corr->eph->position(corr->tRao.gpsw(), corr->tRao.gpssec(), + oldXC.data(), oldVV.data()); + + ColumnVector newXC(4); + ColumnVector newVV(3); + lastEph->position(corr->tRao.gpsw(), corr->tRao.gpssec(), + newXC.data(), newVV.data()); + + ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3); + ColumnVector dV = newVV - oldVV; + double dC = newXC(4) - oldXC(4); + + ColumnVector dRAO(3); + XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO); + + ColumnVector dDotRAO(3); + XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO); + + QString msg = "switch corr " + corr->prn + + QString(" %1 -> %2 %3").arg(corr->iod,3) + .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4); + + emit newMessage(msg.toAscii(), false); + + corr->iod = lastEph->IOD(); + corr->eph = lastEph; + corr->rao += dRAO; + corr->dotRao += dDotRAO; + corr->dClk -= dC; +} + +// Process Epoch +//////////////////////////////////////////////////////////////////////////// +void bncComb::processEpoch() { + + _log.clear(); + + QTextStream out(&_log, QIODevice::WriteOnly); + + out << endl << "Combination:" << endl + << "------------------------------" << endl; + + // Observation Statistics + // ---------------------- + bool masterPresent = false; + QListIterator icAC(_ACs); + while (icAC.hasNext()) { + cmbAC* AC = icAC.next(); + AC->numObs = 0; + QVectorIterator itCorr(corrs()); + while (itCorr.hasNext()) { + cmbCorr* corr = itCorr.next(); + if (corr->acName == AC->name) { + AC->numObs += 1; + if (AC->name == _masterOrbitAC) { + masterPresent = true; + } + } + } + out << AC->name.toAscii().data() << ": " << AC->numObs << endl; + } + + // If Master not present, switch to another one + // -------------------------------------------- + if (masterPresent) { + _masterMissingEpochs = 0; + } + else { + ++_masterMissingEpochs; + if (_masterMissingEpochs < 10) { + out << "Missing Master, Epoch skipped" << endl; + _buffer.remove(_resTime); + emit newMessage(_log, false); + return; + } + else { + _masterMissingEpochs = 0; + QListIterator icAC(_ACs); + while (icAC.hasNext()) { + cmbAC* AC = icAC.next(); + if (AC->numObs > 0) { + out << "Switching Master AC " + << _masterOrbitAC.toAscii().data() << " --> " + << AC->name.toAscii().data() << " " + << _resTime.datestr().c_str() << " " + << _resTime.timestr().c_str() << endl; + _masterOrbitAC = AC->name; + break; + } + } + } + } + + QMap resCorr; + + // Perform the actual Combination using selected Method + // ---------------------------------------------------- + t_irc irc; + ColumnVector dx; + if (_method == filter) { + irc = processEpoch_filter(out, resCorr, dx); + } + else { + irc = processEpoch_singleEpoch(out, resCorr, dx); + } + + // Update Parameter Values, Print Results + // -------------------------------------- + if (irc == success) { + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + pp->xx += dx(iPar); + if (pp->type == cmbParam::clkSat) { + if (resCorr.find(pp->prn) != resCorr.end()) { + resCorr[pp->prn]->dClk = pp->xx / t_CST::c; + } + } + out << _resTime.datestr().c_str() << " " + << _resTime.timestr().c_str() << " "; + out.setRealNumberNotation(QTextStream::FixedNotation); + out.setFieldWidth(8); + out.setRealNumberPrecision(4); + out << pp->toString() << " " + << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl; + out.setFieldWidth(0); + } + printResults(out, resCorr); + dumpResults(resCorr); + } + + // Delete Data, emit Message + // ------------------------- + _buffer.remove(_resTime); + emit newMessage(_log, false); +} + +// Process Epoch - Filter Method +//////////////////////////////////////////////////////////////////////////// +t_irc bncComb::processEpoch_filter(QTextStream& out, + QMap& resCorr, + ColumnVector& dx) { + + // Prediction Step + // --------------- + int nPar = _params.size(); + ColumnVector x0(nPar); + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + if (pp->epoSpec) { + pp->xx = 0.0; + _QQ.Row(iPar) = 0.0; + _QQ.Column(iPar) = 0.0; + _QQ(iPar,iPar) = pp->sig0 * pp->sig0; + } + else { + _QQ(iPar,iPar) += pp->sigP * pp->sigP; + } + x0(iPar) = pp->xx; + } + + // Check Satellite Positions for Outliers + // -------------------------------------- + if (checkOrbits(out) != success) { + return failure; + } + + // Update and outlier detection loop + // --------------------------------- + SymmetricMatrix QQ_sav = _QQ; + while (true) { + + Matrix AA; + ColumnVector ll; + DiagonalMatrix PP; + + if (createAmat(AA, ll, PP, x0, resCorr) != success) { + return failure; + } + + bncModel::kalman(AA, ll, PP, _QQ, dx); + ColumnVector vv = ll - AA * dx; + + int maxResIndex; + double maxRes = vv.maximum_absolute_value1(maxResIndex); + out.setRealNumberNotation(QTextStream::FixedNotation); + out.setRealNumberPrecision(3); + out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str() + << " Maximum Residuum " << maxRes << ' ' + << corrs()[maxResIndex-1]->acName << ' ' << corrs()[maxResIndex-1]->prn; + + if (maxRes > _MAXRES) { + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + if (pp->type == cmbParam::offACSat && + pp->AC == corrs()[maxResIndex-1]->acName && + pp->prn == corrs()[maxResIndex-1]->prn) { + QQ_sav.Row(iPar) = 0.0; + QQ_sav.Column(iPar) = 0.0; + QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0; + } + } + + out << " Outlier" << endl; + _QQ = QQ_sav; + corrs().remove(maxResIndex-1); + } + else { + out << " OK" << endl; + break; + } + } + + return success; +} + +// Print results +//////////////////////////////////////////////////////////////////////////// +void bncComb::printResults(QTextStream& out, + const QMap& resCorr) { + + QMapIterator it(resCorr); + while (it.hasNext()) { + it.next(); + t_corr* corr = it.value(); + const t_eph* eph = corr->eph; + if (eph) { + double xx, yy, zz, cc; + eph->position(_resTime.gpsw(), _resTime.gpssec(), xx, yy, zz, cc); + + out << _resTime.datestr().c_str() << " " + << _resTime.timestr().c_str() << " "; + out.setFieldWidth(3); + out << "Full Clock " << corr->prn << " " << corr->iod << " "; + out.setFieldWidth(14); + out << (cc + corr->dClk) * t_CST::c << endl; + out.setFieldWidth(0); + } + else { + out << "bncComb::printResuls bug" << endl; + } + } +} + +// Send results to RTNet Decoder and directly to PPP Client +//////////////////////////////////////////////////////////////////////////// +void bncComb::dumpResults(const QMap& resCorr) { + + ostringstream out; out.setf(std::ios::fixed); + QStringList corrLines; + + unsigned year, month, day, hour, minute; + double sec; + _resTime.civil_date(year, month, day); + _resTime.civil_time(hour, minute, sec); + + out << "* " + << setw(4) << year << " " + << setw(2) << month << " " + << setw(2) << day << " " + << setw(2) << hour << " " + << setw(2) << minute << " " + << setw(12) << setprecision(8) << sec << " " + << endl; + + QMapIterator it(resCorr); + while (it.hasNext()) { + it.next(); + t_corr* corr = it.value(); + + double dT = 60.0; + + for (int iTime = 1; iTime <= 2; iTime++) { + + bncTime time12 = (iTime == 1) ? _resTime : _resTime + dT; + + ColumnVector xc(4); + ColumnVector vv(3); + corr->eph->position(time12.gpsw(), time12.gpssec(), + xc.data(), vv.data()); + bncPPPclient::applyCorr(time12, corr, xc, vv); + + // Relativistic Correction + // ----------------------- + double relCorr = - 2.0 * DotProduct(xc.Rows(1,3),vv) / t_CST::c / t_CST::c; + xc(4) -= relCorr; + + // Code Biases + // ----------- + double dcbP1C1 = 0.0; + double dcbP1P2 = 0.0; + + // Correction Phase Center --> CoM + // ------------------------------- + ColumnVector dx(3); dx = 0.0; + if (_antex) { + double Mjd = time12.mjd() + time12.daysec()/86400.0; + if (_antex->satCoMcorrection(corr->prn, Mjd, xc.Rows(1,3), dx) == success) { + xc(1) -= dx(1); + xc(2) -= dx(2); + xc(3) -= dx(3); + } + else { + cout << "antenna not found" << endl; + } + } + + if (iTime == 1) { + out << 'P' << corr->prn.toAscii().data() + << setw(14) << setprecision(6) << xc(1) / 1000.0 + << setw(14) << setprecision(6) << xc(2) / 1000.0 + << setw(14) << setprecision(6) << xc(3) / 1000.0 + << setw(14) << setprecision(6) << xc(4) * 1e6 + << setw(14) << setprecision(6) << relCorr * 1e6 + << setw(8) << setprecision(3) << dx(1) + << setw(8) << setprecision(3) << dx(2) + << setw(8) << setprecision(3) << dx(3) + << setw(8) << setprecision(3) << dcbP1C1 + << setw(8) << setprecision(3) << dcbP1P2 + << setw(6) << setprecision(1) << dT; + + QString line; + int messageType = COTYPE_GPSCOMBINED; + int updateInt = 0; + line.sprintf("%d %d %d %.1f %s" + " %3d" + " %8.3f %8.3f %8.3f %8.3f" + " %10.5f %10.5f %10.5f %10.5f" + " %10.5f %10.5f %10.5f %10.5f INTERNAL", + messageType, updateInt, time12.gpsw(), time12.gpssec(), + corr->prn.toAscii().data(), + corr->iod, + corr->dClk * t_CST::c, + corr->rao[0], + corr->rao[1], + corr->rao[2], + corr->dotDClk * t_CST::c, + corr->dotRao[0], + corr->dotRao[1], + corr->dotRao[2], + corr->dotDotDClk * t_CST::c, + corr->dotDotRao[0], + corr->dotDotRao[1], + corr->dotDotRao[2]); + corrLines << line; + } + else { + out << setw(14) << setprecision(6) << xc(1) / 1000.0 + << setw(14) << setprecision(6) << xc(2) / 1000.0 + << setw(14) << setprecision(6) << xc(3) / 1000.0 << endl; + } + } + + delete corr; + } + out << "EOE" << endl; // End Of Epoch flag + + if (!_rtnetDecoder) { + _rtnetDecoder = new bncRtnetDecoder(); + } + + vector errmsg; + _rtnetDecoder->Decode((char*) out.str().data(), out.str().size(), errmsg); + + // Optionally send new Corrections to PPP + // -------------------------------------- + bncApp* app = (bncApp*) qApp; + if (app->_bncPPPclient) { + app->_bncPPPclient->slotNewCorrections(corrLines); + } +} + +// Create First Design Matrix and Vector of Measurements +//////////////////////////////////////////////////////////////////////////// +t_irc bncComb::createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, + const ColumnVector& x0, + QMap& resCorr) { + + unsigned nPar = _params.size(); + unsigned nObs = corrs().size(); + + if (nObs == 0) { + return failure; + } + + int MAXPRN = MAXPRN_GPS; +// if (_useGlonass) { +// MAXPRN = MAXPRN_GPS + MAXPRN_GLONASS; +// } + + const int nCon = (_method == filter) ? 1 + MAXPRN : 0; + + AA.ReSize(nObs+nCon, nPar); AA = 0.0; + ll.ReSize(nObs+nCon); ll = 0.0; + PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs); + + int iObs = 0; + + QVectorIterator itCorr(corrs()); + while (itCorr.hasNext()) { + cmbCorr* corr = itCorr.next(); + QString prn = corr->prn; + + ++iObs; + + if (corr->acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) { + resCorr[prn] = new t_corr(*corr); + } + + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + AA(iObs, iPar) = pp->partial(corr->acName, prn); + } + + ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0); + } + + // Regularization + // -------------- + if (_method == filter) { + const double Ph = 1.e6; + PP(nObs+1) = Ph; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + if ( AA.Column(iPar).maximum_absolute_value() > 0.0 && + pp->type == cmbParam::clkSat ) { + AA(nObs+1, iPar) = 1.0; + } + } + int iCond = 1; + for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) { + QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0')); + ++iCond; + PP(nObs+iCond) = Ph; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + if ( AA.Column(iPar).maximum_absolute_value() > 0.0 && + pp->type == cmbParam::offACSat && + pp->prn == prn) { + AA(nObs+iCond, iPar) = 1.0; + } + } + } +// if (_useGlonass) { +// for (int iGlo = 1; iGlo <= MAXPRN_GLONASS; iGlo++) { +// QString prn = QString("R%1").arg(iGlo, 2, 10, QChar('0')); +// ++iCond; +// PP(nObs+iCond) = Ph; +// for (int iPar = 1; iPar <= _params.size(); iPar++) { +// cmbParam* pp = _params[iPar-1]; +// if ( AA.Column(iPar).maximum_absolute_value() > 0.0 && +// pp->type == cmbParam::offACSat && +// pp->prn == prn) { +// AA(nObs+iCond, iPar) = 1.0; +// } +// } +// } +// } + } + + return success; +} + +// Process Epoch - Single-Epoch Method +//////////////////////////////////////////////////////////////////////////// +t_irc bncComb::processEpoch_singleEpoch(QTextStream& out, + QMap& resCorr, + ColumnVector& dx) { + + // Check Satellite Positions for Outliers + // -------------------------------------- + if (checkOrbits(out) != success) { + return failure; + } + + // Outlier Detection Loop + // ---------------------- + while (true) { + + // Remove Satellites that are not in Master + // ---------------------------------------- + QMutableVectorIterator it(corrs()); + while (it.hasNext()) { + cmbCorr* corr = it.next(); + QString prn = corr->prn; + bool foundMaster = false; + QVectorIterator itHlp(corrs()); + while (itHlp.hasNext()) { + cmbCorr* corrHlp = itHlp.next(); + QString prnHlp = corrHlp->prn; + QString ACHlp = corrHlp->acName; + if (ACHlp == _masterOrbitAC && prn == prnHlp) { + foundMaster = true; + break; + } + } + if (!foundMaster) { + delete corr; + it.remove(); + } + } + + // Count Number of Observations per Satellite and per AC + // ----------------------------------------------------- + QMap numObsPrn; + QMap numObsAC; + QVectorIterator itCorr(corrs()); + while (itCorr.hasNext()) { + cmbCorr* corr = itCorr.next(); + QString prn = corr->prn; + QString AC = corr->acName; + if (numObsPrn.find(prn) == numObsPrn.end()) { + numObsPrn[prn] = 1; + } + else { + numObsPrn[prn] += 1; + } + if (numObsAC.find(AC) == numObsAC.end()) { + numObsAC[AC] = 1; + } + else { + numObsAC[AC] += 1; + } + } + + // Clean-Up the Paramters + // ---------------------- + for (int iPar = 1; iPar <= _params.size(); iPar++) { + delete _params[iPar-1]; + } + _params.clear(); + + // Set new Parameters + // ------------------ + int nextPar = 0; + + QMapIterator itAC(numObsAC); + while (itAC.hasNext()) { + itAC.next(); + const QString& AC = itAC.key(); + int numObs = itAC.value(); + if (AC != _masterOrbitAC && numObs > 0) { + _params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC, "")); + if (_useGlonass) { + _params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC, "")); + } + } + } + + QMapIterator itPrn(numObsPrn); + while (itPrn.hasNext()) { + itPrn.next(); + const QString& prn = itPrn.key(); + int numObs = itPrn.value(); + if (numObs > 0) { + _params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn)); + } + } + + int nPar = _params.size(); + ColumnVector x0(nPar); + x0 = 0.0; + + // Create First-Design Matrix + // -------------------------- + Matrix AA; + ColumnVector ll; + DiagonalMatrix PP; + if (createAmat(AA, ll, PP, x0, resCorr) != success) { + return failure; + } + + ColumnVector vv; + try { + Matrix ATP = AA.t() * PP; + SymmetricMatrix NN; NN << ATP * AA; + ColumnVector bb = ATP * ll; + _QQ = NN.i(); + dx = _QQ * bb; + vv = ll - AA * dx; + } + catch (Exception& exc) { + out << exc.what() << endl; + return failure; + } + + int maxResIndex; + double maxRes = vv.maximum_absolute_value1(maxResIndex); + out.setRealNumberNotation(QTextStream::FixedNotation); + out.setRealNumberPrecision(3); + out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str() + << " Maximum Residuum " << maxRes << ' ' + << corrs()[maxResIndex-1]->acName << ' ' << corrs()[maxResIndex-1]->prn; + + if (maxRes > _MAXRES) { + out << " Outlier" << endl; + delete corrs()[maxResIndex-1]; + corrs().remove(maxResIndex-1); + } + else { + out << " OK" << endl; + out.setRealNumberNotation(QTextStream::FixedNotation); + out.setRealNumberPrecision(3); + for (int ii = 0; ii < vv.Nrows(); ii++) { + const cmbCorr* corr = corrs()[ii]; + out << _resTime.datestr().c_str() << ' ' + << _resTime.timestr().c_str() << " " + << corr->acName << ' ' << corr->prn; + out.setFieldWidth(6); + out << " dClk = " << corr->dClk * t_CST::c << " res = " << vv[ii] << endl; + out.setFieldWidth(0); + } + return success; + } + + } + + return failure; +} + +// Check Satellite Positions for Outliers +//////////////////////////////////////////////////////////////////////////// +t_irc bncComb::checkOrbits(QTextStream& out) { + + const double MAX_DISPLACEMENT = 0.20; + + // Switch to last ephemeris (if possible) + // -------------------------------------- + QMutableVectorIterator im(corrs()); + while (im.hasNext()) { + cmbCorr* corr = im.next(); + QString prn = corr->prn; + if (_eph.find(prn) == _eph.end()) { + out << "checkOrbit: missing eph (not found) " << corr->prn << endl; + delete corr; + im.remove(); + } + else if (corr->eph == 0) { + out << "checkOrbit: missing eph (zero) " << corr->prn << endl; + delete corr; + im.remove(); + } + else { + if ( corr->eph == _eph[prn]->last || corr->eph == _eph[prn]->prev ) { + switchToLastEph(_eph[prn]->last, corr); + } + else { + out << "checkOrbit: missing eph (deleted) " << corr->prn << endl; + delete corr; + im.remove(); + } + } + } + + while (true) { + + // Compute Mean Corrections for all Satellites + // ------------------------------------------- + QMap numCorr; + QMap meanRao; + QVectorIterator it(corrs()); + while (it.hasNext()) { + cmbCorr* corr = it.next(); + QString prn = corr->prn; + if (meanRao.find(prn) == meanRao.end()) { + meanRao[prn].ReSize(4); + meanRao[prn].Rows(1,3) = corr->rao; + meanRao[prn](4) = 1; + } + else { + meanRao[prn].Rows(1,3) += corr->rao; + meanRao[prn](4) += 1; + } + if (numCorr.find(prn) == numCorr.end()) { + numCorr[prn] = 1; + } + else { + numCorr[prn] += 1; + } + } + + // Compute Differences wrt Mean, find Maximum + // ------------------------------------------ + QMap maxDiff; + it.toFront(); + while (it.hasNext()) { + cmbCorr* corr = it.next(); + QString prn = corr->prn; + if (meanRao[prn](4) != 0) { + meanRao[prn] /= meanRao[prn](4); + meanRao[prn](4) = 0; + } + corr->diffRao = corr->rao - meanRao[prn].Rows(1,3); + if (maxDiff.find(prn) == maxDiff.end()) { + maxDiff[prn] = corr; + } + else { + double normMax = maxDiff[prn]->diffRao.norm_Frobenius(); + double norm = corr->diffRao.norm_Frobenius(); + if (norm > normMax) { + maxDiff[prn] = corr; + } + } + } + + if (_ACs.size() == 1) { + break; + } + + // Remove Outliers + // --------------- + bool removed = false; + QMutableVectorIterator im(corrs()); + while (im.hasNext()) { + cmbCorr* corr = im.next(); + QString prn = corr->prn; + if (numCorr[prn] < 2) { + delete corr; + im.remove(); + } + else if (corr == maxDiff[prn]) { + double norm = corr->diffRao.norm_Frobenius(); + if (norm > MAX_DISPLACEMENT) { + out << _resTime.datestr().c_str() << " " + << _resTime.timestr().c_str() << " " + << "Orbit Outlier: " + << corr->acName.toAscii().data() << " " + << prn.toAscii().data() << " " + << corr->iod << " " + << norm << endl; + delete corr; + im.remove(); + removed = true; + } + } + } + + if (!removed) { + break; + } + } + + return success; +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc bncComb::mergeOrbitCorr(const cmbCorr* orbitCorr, cmbCorr* clkCorr) { + + clkCorr->iod = orbitCorr->iod; // is it always correct? + clkCorr->eph = orbitCorr->eph; + clkCorr->tRao = orbitCorr->tRao; + clkCorr->rao = orbitCorr->rao; + clkCorr->dotRao = orbitCorr->dotRao; + clkCorr->dotDotRao = orbitCorr->dotDotRao; + + return success; +} Index: trunk/BNC/src/combination/bnccomb.h =================================================================== --- trunk/BNC/src/combination/bnccomb.h (revision 4281) +++ trunk/BNC/src/combination/bnccomb.h (revision 4281) @@ -0,0 +1,114 @@ + +#ifndef BNCCOMB_H +#define BNCCOMB_H + +#include +#include +#include "bncephuser.h" + +class bncRtnetDecoder; +class bncSP3; +class bncAntex; + +class cmbParam { + public: + enum parType {offACgps, offACglo, offACSat, clkSat}; + cmbParam(parType type_, int index_, const QString& ac_, const QString& prn_); + ~cmbParam(); + double partial(const QString& AC_, const QString& prn_); + QString toString() const; + parType type; + int index; + QString AC; + QString prn; + double xx; + double sig0; + double sigP; + bool epoSpec; + const t_eph* eph; +}; + +class bncComb : public bncEphUser { + Q_OBJECT + + public: + bncComb(); + virtual ~bncComb(); + void processCorrLine(const QString& staID, const QString& line); + int nStreams() const {return _ACs.size();} + + signals: + void newMessage(QByteArray msg, bool showOnScreen); + + private: + + enum e_method{singleEpoch, filter}; + + class cmbAC { + public: + cmbAC() { + weight = 0.0; + numObs = 0; + } + ~cmbAC() {} + QString mountPoint; + QString name; + double weight; + unsigned numObs; + }; + + class cmbCorr : public t_corr { + public: + QString acName; + ColumnVector diffRao; + QString ID() {return acName + "_" + prn;} + }; + + class cmbEpoch { + public: + cmbEpoch() {} + ~cmbEpoch() { + QVectorIterator it(corrs); + while (it.hasNext()) { + delete it.next(); + } + } + QVector corrs; + }; + + void processEpoch(); + t_irc processEpoch_filter(QTextStream& out, + QMap& resCorr, + ColumnVector& dx); + t_irc processEpoch_singleEpoch(QTextStream& out, + QMap& resCorr, + ColumnVector& dx); + t_irc createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, + const ColumnVector& x0, QMap& resCorr); + void dumpResults(const QMap& resCorr); + void printResults(QTextStream& out, const QMap& resCorr); + void switchToLastEph(const t_eph* lastEph, t_corr* corr); + t_irc checkOrbits(QTextStream& out); + + QVector& corrs() {return _buffer[_resTime].corrs;} + + t_irc mergeOrbitCorr(const cmbCorr* orbitCorr, cmbCorr* clkCorr); + + QList _ACs; + bncTime _resTime; + QVector _params; + QMap _buffer; + bncRtnetDecoder* _rtnetDecoder; + SymmetricMatrix _QQ; + QByteArray _log; + bncAntex* _antex; + double _MAXRES; + QString _masterOrbitAC; + unsigned _masterMissingEpochs; + e_method _method; + bool _useGlonass; + int _cmbSampl; + QMap _orbitCorrs; +}; + +#endif