Index: trunk/BNC/combination/bnccomb.cpp =================================================================== --- trunk/BNC/combination/bnccomb.cpp (revision 3447) +++ trunk/BNC/combination/bnccomb.cpp (revision 3448) @@ -35,8 +35,6 @@ const double sig0_offACSat = 100.0; const double sigP_offACSat = 0.0; -const double sig0_clkSat = 10.0; -const double sigP_clkSat = 0.1; - -const double sigObs = 0.05; +const double sig0_clkSat = 100.0; +const double sigP_clkSat = 100.0; const int MAXPRN_GPS = 32; @@ -54,5 +52,4 @@ prn = prn_; xx = 0.0; - eph = 0; if (type == offAC) { @@ -186,4 +183,8 @@ } + // Not yet regularized + // ------------------- + _firstReg = false; + // Maximal Residuum // ---------------- @@ -337,5 +338,5 @@ XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO); - QString msg = "switch corr " + corr->prn + QString msg = "switch " + corr->prn + QString(" %1 -> %2 %3").arg(corr->iod,3) .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4); @@ -355,4 +356,9 @@ int nPar = _params.size(); + int nObs = corrs().size(); + + if (nObs == 0) { + return; + } _log.clear(); @@ -383,9 +389,5 @@ ColumnVector x0(nPar); for (int iPar = 1; iPar <= _params.size(); iPar++) { - cmbParam* pp = _params[iPar-1]; - QString prn = pp->prn; - if (!prn.isEmpty() && _eph.find(prn) != _eph.end()) { - switchToLastEph(_eph[prn]->last, pp); - } + cmbParam* pp = _params[iPar-1]; if (pp->epoSpec) { pp->xx = 0.0; @@ -400,21 +402,70 @@ } + // Create First Design Matrix and Vector of Measurements + // ----------------------------------------------------- + const double Pl = 1.0 / (0.05 * 0.05); + + const int nCon = (_firstReg == false) ? 1 + MAXPRN_GPS : 1; + Matrix AA(nObs+nCon, nPar); AA = 0.0; + ColumnVector ll(nObs+nCon); ll = 0.0; + DiagonalMatrix PP(nObs+nCon); PP = Pl; + + int iObs = 0; + + QMap resCorr; + + QVectorIterator itCorr(corrs()); + while (itCorr.hasNext()) { + cmbCorr* corr = itCorr.next(); + QString prn = corr->prn; + switchToLastEph(_eph[prn]->last, corr); + ++iObs; + + if (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 + // -------------- + const double Ph = 1.e6; + int iCond = 1; + PP(nObs+iCond) = Ph; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + if (pp->type == cmbParam::clkSat && + AA.Column(iPar).maximum_absolute_value() > 0.0) { + AA(nObs+iCond, iPar) = 1.0; + } + } + + if (!_firstReg) { + _firstReg = true; + for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) { + ++iCond; + QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0')); + PP(nObs+1+iGps) = Ph; + for (int iPar = 1; iPar <= _params.size(); iPar++) { + cmbParam* pp = _params[iPar-1]; + if (pp->type == cmbParam::offACSat && pp->prn == prn) { + AA(nObs+iCond, iPar) = 1.0; + } + } + } + } + + ColumnVector dx; SymmetricMatrix QQ_sav = _QQ; - ColumnVector dx; - QMap resCorr; - // Update and outlier detection loop // --------------------------------- - while (true) { - - Matrix AA; - ColumnVector ll; - DiagonalMatrix PP; - - if (createAmat(AA, ll, PP, x0, resCorr) != success) { - return; - } - + for (int ii = 1; ii < 10; ii++) { bncModel::kalman(AA, ll, PP, _QQ, dx); ColumnVector vv = ll - AA * dx; @@ -442,5 +493,6 @@ out << " Outlier" << endl; _QQ = QQ_sav; - corrs().remove(maxResIndex-1); + AA.Row(maxResIndex) = 0.0; + ll.Row(maxResIndex) = 0.0; } else { @@ -637,100 +689,2 @@ } } - -// 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; - } - - const int nCon = 2; - - 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; - switchToLastEph(_eph[prn]->last, corr); - ++iObs; - - if (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 - // -------------- - const double Ph = 1.e6; - PP(nObs+1) = Ph; - PP(nObs+2) = Ph; - for (int iPar = 1; iPar <= _params.size(); iPar++) { - cmbParam* pp = _params[iPar-1]; - if (pp->type == cmbParam::clkSat) { - AA(nObs+1, iPar) = 1.0; - } - else if (pp->type == cmbParam::offAC) { - AA(nObs+2, iPar) = 1.0; - } - } - - return success; -} - -// Change the parameter so that it refers to last received ephemeris -//////////////////////////////////////////////////////////////////////////// -void bncComb::switchToLastEph(const t_eph* lastEph, cmbParam* pp) { - - if (pp->type != cmbParam::clkSat) { - return; - } - - if (pp->eph == 0) { - pp->eph = lastEph; - return; - } - - if (pp->eph == lastEph) { - return; - } - - ColumnVector oldXC(4); - ColumnVector oldVV(3); - pp->eph->position(_resTime.gpsw(), _resTime.gpssec(), - oldXC.data(), oldVV.data()); - - ColumnVector newXC(4); - ColumnVector newVV(3); - lastEph->position(_resTime.gpsw(), _resTime.gpssec(), - newXC.data(), newVV.data()); - - double dC = newXC(4) - oldXC(4); - - QString msg = "switch param " + pp->prn - + QString(" %1 -> %2 %3").arg(pp->eph->IOD(),3) - .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4); - - emit newMessage(msg.toAscii(), false); - - pp->eph = lastEph; - pp->xx -= dC * t_CST::c; -} - Index: trunk/BNC/combination/bnccomb.h =================================================================== --- trunk/BNC/combination/bnccomb.h (revision 3447) +++ trunk/BNC/combination/bnccomb.h (revision 3448) @@ -26,5 +26,4 @@ double sigP; bool epoSpec; - const t_eph* eph; }; @@ -74,10 +73,7 @@ void processEpoch(); - 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); - void switchToLastEph(const t_eph* lastEph, cmbParam* pp); QVector& corrs() {return _buffer[_resTime].corrs;} @@ -89,4 +85,5 @@ bncRtnetDecoder* _rtnetDecoder; SymmetricMatrix _QQ; + bool _firstReg; QByteArray _log; bncAntex* _antex;