Index: trunk/BNC/src/combination/bnccomb.cpp =================================================================== --- trunk/BNC/src/combination/bnccomb.cpp (revision 10669) +++ trunk/BNC/src/combination/bnccomb.cpp (revision 10694) @@ -219,4 +219,5 @@ connect(BNC_CORE, SIGNAL(newClkCorrections(QList)), this, SLOT(slotNewClkCorrections(QList))); connect(BNC_CORE, SIGNAL(newCodeBiases(QList)), this, SLOT(slotNewCodeBiases(QList))); + connect(BNC_CORE, SIGNAL(newPhaseBiases(QList)), this, SLOT(slotNewPhaseBiases(QList))); // Combination Method @@ -479,6 +480,46 @@ storage[satCodeBias._prn] = satCodeBias; } - -} + return; +} + +// Remember Satellite Phase Biases +//////////////////////////////////////////////////////////////////////////// +void bncComb::slotNewPhaseBiases(QList satPhaseBiases) { + QMutexLocker locker(&_mutex); + + for (int ii = 0; ii < satPhaseBiases.size(); ii++) { + t_satPhaseBias& satPhaseBias = satPhaseBiases[ii]; + QString staID(satPhaseBias._staID.c_str()); + char sys = satPhaseBias._prn.system(); + + if (!_cmbSysPrn.contains(sys)){ + continue; + } + + // Find/Check the AC Name + // ---------------------- + QString acName; + QStringList excludeSats; + QListIterator icAC(_ACs); + while (icAC.hasNext()) { + cmbAC* AC = icAC.next(); + if (AC->mountPoint == staID) { + acName = AC->name; + excludeSats = AC->excludeSats; + break; + } + } + if (acName.isEmpty() || excludeSat(satPhaseBias._prn, excludeSats)) { + continue; + } + + // Store the correction + // -------------------- + QMap& storage = _satPhaseBiases[acName]; + storage[satPhaseBias._prn] = satPhaseBias; + } + return; +} + // Process clock corrections @@ -783,15 +824,40 @@ } } - if (codeBiasesRefSig.size() == 2) { - map codeCoeff; - double channel = double(_newCorr->_eph->slotNum()); - cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff); - map::const_iterator it; - for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { - t_frequency::type frqType = it->first; - _newCorr->_satCodeBiasIF += it->second * codeBiasesRefSig[frqType]; - } + map codeCoeff; + double channel = double(_newCorr->_eph->slotNum()); + cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff); + map::const_iterator it; + for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { + t_frequency::type frqType = it->first; + double codeCoeff = it->second; + _newCorr->_satCodeBiasIF += codeCoeff * codeBiasesRefSig[frqType]; } _newCorr->_satCodeBias._bias.clear(); + } + } + + // Check satellite phase biases + // ---------------------------- + if (_satPhaseBiases.contains(acName)) { + QMap& storage = _satPhaseBiases[acName]; + if (storage.contains(clkCorr._prn)) { + // Yaw angle + t_satPhaseBias& pb = storage[clkCorr._prn]; + double dt = _newCorr->_time - pb._time; + if (pb._updateInt) { + dt -= (0.5 * ssrUpdateInt[pb._updateInt]); + } + _newCorr->_satYawAngle = pb._yaw + pb._yawRate * dt; + + // _lambdaIF + map codeCoeff; + double channel = double(_newCorr->_eph->slotNum()); + cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff); + map::const_iterator it; + for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) { + t_frequency::type frqType = it->first; + double codeCoeff = it->second; + _newCorr->_lambdaIF += codeCoeff * t_CST::c / t_CST::freq(frqType, channel); + } } } @@ -901,5 +967,5 @@ } - QMap resCorr; + QMap masterCorr; // Perform the actual Combination using selected Method @@ -908,8 +974,8 @@ ColumnVector dx; if (_method == filter) { - irc = processEpoch_filter(epoTime, sys, out, resCorr, dx); + irc = processEpoch_filter(epoTime, sys, out, masterCorr, dx); } else { - irc = processEpoch_singleEpoch(epoTime, sys, out, resCorr, dx); + irc = processEpoch_singleEpoch(epoTime, sys, out, masterCorr, dx); } @@ -921,15 +987,15 @@ pp->xx += dx(iPar); if (pp->type == cmbParam::clkSat) { - if (resCorr.find(pp->prn) != resCorr.end()) { + if (masterCorr.find(pp->prn) != masterCorr.end()) { // set clock result - resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c; + masterCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c; // Add Code Biases from SINEX File if (_bsx) { map codeCoeff; - double channel = double(resCorr[pp->prn]->_eph->slotNum()); + double channel = double(masterCorr[pp->prn]->_eph->slotNum()); cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff); t_frequency::type fType1 = cmbRefSig::toFreq(sys, cmbRefSig::c1); t_frequency::type fType2 = cmbRefSig::toFreq(sys, cmbRefSig::c2); - _bsx->determineSsrSatCodeBiases(pp->prn.mid(0,3), codeCoeff[fType1], codeCoeff[fType2], resCorr[pp->prn]->_satCodeBias); + _bsx->determineSsrSatCodeBiases(pp->prn.mid(0,3), codeCoeff[fType1], codeCoeff[fType2], masterCorr[pp->prn]->_satCodeBias); } } @@ -945,6 +1011,6 @@ out.flush(); } - printResults(epoTime, out, resCorr); - dumpResults(epoTime, resCorr); + printResults(epoTime, out, masterCorr); + dumpResults(epoTime, masterCorr); } } @@ -953,5 +1019,5 @@ //////////////////////////////////////////////////////////////////////////// t_irc bncComb::processEpoch_filter(bncTime epoTime, char sys, QTextStream& out, - QMap& resCorr, + QMap& masterCorr, ColumnVector& dx) { @@ -976,5 +1042,5 @@ // Check Satellite Positions for Outliers // -------------------------------------- - if (checkOrbits(epoTime, sys, out, resCorr) != success) { + if (checkOrbits(epoTime, sys, out, masterCorr) != success) { return failure; } @@ -989,5 +1055,5 @@ DiagonalMatrix PP; - if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) { + if (createAmat(sys, AA, ll, PP, x0) != success) { return failure; } @@ -1045,7 +1111,7 @@ //////////////////////////////////////////////////////////////////////////// void bncComb::printResults(bncTime epoTime, QTextStream& out, - const QMap& resCorr) { - - QMapIterator it(resCorr); + const QMap& masterCorr) { + + QMapIterator it(masterCorr); while (it.hasNext()) { it.next(); @@ -1075,9 +1141,10 @@ // Send results to RTNet Decoder and directly to PPP Client //////////////////////////////////////////////////////////////////////////// -void bncComb::dumpResults(bncTime epoTime, QMap& resCorr) { +void bncComb::dumpResults(bncTime epoTime, QMap& masterCorr) { QList orbCorrections; QList clkCorrections; QList satCodeBiasList; + QList satPhaseBiasList; unsigned year, month, day, hour, minute; @@ -1089,5 +1156,5 @@ year, month, day, hour, minute, sec); - QMutableMapIterator it(resCorr); + QMutableMapIterator it(masterCorr); while (it.hasNext()) { it.next(); @@ -1154,9 +1221,11 @@ " Clk 1 %15.4f" " Vel 3 %15.4f %15.4f %15.4f" - " CoM 3 %15.4f %15.4f %15.4f", + " CoM 3 %15.4f %15.4f %15.4f" + " YawAngle %1.4f", apc(1), apc(2), apc(3), xc(4) * t_CST::c, vv(1), vv(2), vv(3), - com(1), com(2), com(3)); + com(1), com(2), com(3), + corr->_satYawAngle); outLines += hlp; hlp.clear(); @@ -1179,4 +1248,5 @@ } } + outLines += "\n"; delete corr; @@ -1203,6 +1273,5 @@ // Create First Design Matrix and Vector of Measurements //////////////////////////////////////////////////////////////////////////// -t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, - const ColumnVector& x0, QMap& resCorr) { +t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, const ColumnVector& x0) { unsigned nPar = _params[sys].size(); @@ -1233,10 +1302,11 @@ AA(iObs, iPar) = pp->partial(sys, corr->_acName, prn); } - // Consistency correction to keep the combined clock consistent to MeanOrb - // ----------------------------------------------------------------------- - double dC_radial = (corr->_diffRao.t() * resCorr[prn]->_orbCorr._xr).AsScalar() - / (resCorr[prn]->_orbCorr._xr).norm_Frobenius(); - - ll(iObs) = (corr->_clkCorr._dClk * t_CST::c - corr->_satCodeBiasIF + dC_radial) - DotProduct(AA.Row(iObs), x0); + // Consistency corrections to keep the combined clock consistent to masterOrbit + // ---------------------------------------------------------------------------- + double dC_orb = dotproduct(corr->_orbCorr._xr, corr->_diffRao) / corr->_orbCorr._xr.NormFrobenius(); + double dC_att = corr->_diffYaw / (2 * M_PI); + dC_att *= corr->_lambdaIF; + + ll(iObs) = (corr->_clkCorr._dClk * t_CST::c - corr->_satCodeBiasIF + dC_orb + dC_att) - DotProduct(AA.Row(iObs), x0); PP(iObs, iObs) *= 1.0 / (corr->_weightFactor * corr->_weightFactor); @@ -1283,10 +1353,10 @@ //////////////////////////////////////////////////////////////////////////// t_irc bncComb::processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out, - QMap& resCorr, + QMap& masterCorr, ColumnVector& dx) { // Check Satellite Positions for Outliers // -------------------------------------- - if (checkOrbits(epoTime, sys, out, resCorr) != success) { + if (checkOrbits(epoTime, sys, out, masterCorr) != success) { return failure; } @@ -1358,5 +1428,5 @@ ColumnVector ll; DiagonalMatrix PP; - if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) { + if (createAmat(sys, AA, ll, PP, x0) != success) { return failure; } @@ -1412,5 +1482,5 @@ // Check Satellite Positions for Outliers //////////////////////////////////////////////////////////////////////////// -t_irc bncComb::checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap& resCorr) { +t_irc bncComb::checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap& masterCorr) { // Switch to last ephemeris (if possible) @@ -1505,21 +1575,8 @@ QString prn = corr->_prn; if (corr->_acName == _masterOrbitAC[sys] && - resCorr.find(prn) == resCorr.end()) { - resCorr[prn] = new cmbCorr(*corr); - } - } - // Remove satellites that are not in masterOrbit - // and compute differences wrt. masterOrbit - // ---------------------------------------------- - QMutableVectorIterator im(corrs(sys)); - while (im.hasNext()) { - cmbCorr* corr = im.next(); - QString prn = corr->_prn; - if (resCorr.find(prn) == resCorr.end()) { - delete corr; - im.remove(); - } - else { - corr->_diffRao = resCorr[prn]->_orbCorr._xr - corr->_orbCorr._xr; + masterCorr.find(prn) == masterCorr.end()) { + masterCorr[prn] = new cmbCorr(*corr); + masterCorr[prn]->_diffRao = 0.0; + masterCorr[prn]->_diffYaw = 0.0; } } @@ -1566,6 +1623,6 @@ QString prn = corr->_prn; if (corr->_acName == _masterOrbitAC[sys] && - resCorr.find(prn) == resCorr.end()) { - resCorr[prn] = new cmbCorr(*corr); + masterCorr.find(prn) == masterCorr.end()) { + masterCorr[prn] = new cmbCorr(*corr); } } @@ -1577,10 +1634,11 @@ cmbCorr* corr = im.next(); QString prn = corr->_prn; - if (resCorr.find(prn) == resCorr.end()) { + if (masterCorr.find(prn) == masterCorr.end()) { delete corr; im.remove(); } else { - corr->_diffRao = resCorr[prn]->_orbCorr._xr - corr->_orbCorr._xr; + corr->_diffRao = corr->_orbCorr._xr - masterCorr[prn]->_orbCorr._xr; + corr->_diffYaw = corr->_satYawAngle - masterCorr[prn]->_satYawAngle; } } Index: trunk/BNC/src/combination/bnccomb.h =================================================================== --- trunk/BNC/src/combination/bnccomb.h (revision 10669) +++ trunk/BNC/src/combination/bnccomb.h (revision 10694) @@ -12,4 +12,5 @@ #include #include +#include #include #include "bncephuser.h" @@ -46,4 +47,5 @@ void slotNewClkCorrections(QList clkCorrections); void slotNewCodeBiases(QList satCodeBiases); + void slotNewPhaseBiases(QList satPhaseBiases); private slots: @@ -55,4 +57,5 @@ void newClkCorrections(QList); void newCodeBiases(QList); + void newPhaseBiases(QList); private: @@ -111,5 +114,9 @@ _dClkResult = 0.0; _satCodeBiasIF = 0.0; + _lambdaIF = 0.0; + _satYawAngle = 0.0; _weightFactor = 1.0; + _diffRao.ReSize(3); _diffRao = 0.0; + _diffYaw = 0.0; } ~cmbCorr() { @@ -122,8 +129,12 @@ t_clkCorr _clkCorr; t_satCodeBias _satCodeBias; + //t_satPhaseBias _satPhaseBias; QString _acName; double _satCodeBiasIF; + double _lambdaIF; + double _satYawAngle; double _dClkResult; ColumnVector _diffRao; + double _diffYaw; double _weightFactor; QString ID() {return _acName + "_" + _prn;} @@ -237,12 +248,11 @@ void processEpoch(bncTime epoTime, const std::vector& clkCorrVec); void processSystem(bncTime epoTime, char sys, QTextStream& out); - t_irc processEpoch_filter(bncTime epoTime, char sys, QTextStream& out, QMap& resCorr, ColumnVector& dx); - t_irc processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out, QMap& resCorr, ColumnVector& dx); - t_irc createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, - const ColumnVector& x0, QMap& resCorr); - void dumpResults(bncTime epoTime, QMap& resCorr); - void printResults(bncTime epoTime, QTextStream& out, const QMap& resCorr); + t_irc processEpoch_filter(bncTime epoTime, char sys, QTextStream& out, QMap& masterCorr, ColumnVector& dx); + t_irc processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out, QMap& masterCorr, ColumnVector& dx); + t_irc createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, const ColumnVector& x0); + void dumpResults(bncTime epoTime, QMap& masterCorr); + void printResults(bncTime epoTime, QTextStream& out, const QMap& masterCorr); void switchToLastEph(t_eph* lastEph, cmbCorr* corr); - t_irc checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap& resCorr); + t_irc checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap& masterCorr); bool excludeSat(const t_prn& prn, const QStringList excludeSats) const; QVector& corrs(char sys) {return _buffer[sys].corrs;} @@ -271,5 +281,6 @@ QMap> _params; QMap > _orbCorrections; - QMap > _satCodeBiases; + QMap> _satCodeBiases; + QMap> _satPhaseBiases; QMap _cmbSysPrn; bncEphUser _ephUser;