Changeset 3307 in ntrip for trunk/BNC


Ignore:
Timestamp:
Jun 21, 2011, 5:36:57 PM (14 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/bncmodel.cpp

    r3287 r3307  
    671671  }
    672672
    673   SymmetricMatrix QQsav;
    674   ColumnVector    dx;
    675   ColumnVector    vv;
    676 
    677   // Loop over all outliers
     673  // Outlier Detection Loop
    678674  // ----------------------
    679   do {
    680    
    681     // Bancroft Solution
    682     // -----------------
    683     if (cmpBancroft(epoData) != success) {
    684       emit newMessage(_log, false);
    685       return failure;
    686     }
    687 
    688     // Status Prediction
    689     // -----------------
    690     predict(epoData);
    691    
    692     // Create First-Design Matrix
    693     // --------------------------
    694     unsigned nPar = _params.size();
    695     unsigned nObs = 0;
    696     if (_usePhase) {
    697       nObs = 2 * (epoData->sizeGPS() + epoData->sizeGal()) + epoData->sizeGlo();
    698     }
    699     else {
    700       nObs = epoData->sizeGPS() + epoData->sizeGal(); // Glonass code not used
    701     }
    702    
    703     if (nObs < nPar) {
    704       _log += "bncModel::update: nObs < nPar\n";
    705       emit newMessage(_log, false);
    706       return failure;
    707     }
    708 
    709     Matrix          AA(nObs, nPar);  // first design matrix
    710     ColumnVector    ll(nObs);        // tems observed-computed
    711     DiagonalMatrix  PP(nObs); PP = 0.0;
    712    
    713     unsigned iObs = 0;
    714 
    715     // GPS code and (optionally) phase observations
    716     // --------------------------------------------
    717     QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
    718     while (itGPS.hasNext()) {
    719       itGPS.next();
    720       t_satData* satData = itGPS.value();
    721       addObs(iObs, satData, AA, ll, PP);
    722     }
    723 
    724     // Glonass phase observations
    725     // --------------------------
    726     QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
    727     while (itGlo.hasNext()) {
    728       itGlo.next();
    729       t_satData* satData = itGlo.value();
    730       addObs(iObs, satData, AA, ll, PP);
    731     }
    732 
    733     // Galileo code and (optionally) phase observations
    734     // ------------------------------------------------
    735     QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
    736     while (itGal.hasNext()) {
    737       itGal.next();
    738       t_satData* satData = itGal.value();
    739       addObs(iObs, satData, AA, ll, PP);
    740     }
    741 
    742     // Compute Filter Update
    743     // ---------------------
    744     QQsav = _QQ;
    745 
    746     kalman(AA, ll, PP, _QQ, dx);
    747 
    748     vv = ll - AA * dx;
    749 
    750     // Print Residuals
    751     // ---------------
    752     if (true) {
    753       ostringstream str;
    754       str.setf(ios::fixed);
    755 
    756       QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
    757       while (itGPS.hasNext()) {
    758         itGPS.next();
    759         t_satData* satData = itGPS.value();
    760         printRes(vv, str, satData);
    761       }
    762       QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
    763       while (itGlo.hasNext()) {
    764         itGlo.next();
    765         t_satData* satData = itGlo.value();
    766         printRes(vv, str, satData);
    767       }
    768       QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
    769       while (itGal.hasNext()) {
    770         itGal.next();
    771         t_satData* satData = itGal.value();
    772         printRes(vv, str, satData);
    773       }
    774       _log += str.str().c_str();
    775     }
    776 
    777   } while (outlierDetection(QQsav, vv, epoData->satDataGPS,
    778                             epoData->satDataGlo, epoData->satDataGal) != 0);
     675  ColumnVector dx;
     676  if (update_p(epoData, dx) != success) {
     677    return failure;
     678  }
    779679
    780680  // Remember the Epoch-specific Results for the computation of means
    … …  
    12781178//
    12791179///////////////////////////////////////////////////////////////////////////
    1280 void bncModel::addObs(unsigned& iObs, t_satData* satData,
     1180void bncModel::addObs(int phase, unsigned& iObs, t_satData* satData,
    12811181                      Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP) {
    12821182
    1283   // Code Observations
    1284   // -----------------
    1285   if (satData->system() != 'R') {
    1286     ++iObs;
    1287     ll(iObs)      = satData->P3 - cmpValue(satData, false);
    1288     PP(iObs,iObs) = 1.0 / (_sigP3 * _sigP3);
    1289     for (int iPar = 1; iPar <= _params.size(); iPar++) {
    1290       AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
    1291     }
    1292     satData->indexCode = iObs;
    1293   }
    1294  
    12951183  // Phase Observations
    12961184  // ------------------
    1297   if (_usePhase) {
     1185  if (phase == 1) {
    12981186    ++iObs;
    12991187    ll(iObs)      = satData->L3 - cmpValue(satData, true);
    … …  
    13081196    satData->indexPhase = iObs;
    13091197  }
     1198
     1199  // Code Observations
     1200  // -----------------
     1201  else {
     1202    ++iObs;
     1203    ll(iObs)      = satData->P3 - cmpValue(satData, false);
     1204    PP(iObs,iObs) = 1.0 / (_sigP3 * _sigP3);
     1205    for (int iPar = 1; iPar <= _params.size(); iPar++) {
     1206      AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
     1207    }
     1208    satData->indexCode = iObs;
     1209  }
    13101210}
    13111211
    … …  
    13171217    str << _time.timestr(1)
    13181218        << " RES " << satData->prn.toAscii().data() << "   L3 "
    1319         << setw(9) << setprecision(4) << vv(satData->indexPhase);
    1320     if (satData->indexCode) {
    1321       str << "   P3 " << setw(9) << setprecision(4) << vv(satData->indexCode);
    1322     }
    1323     str << endl;
     1219        << setw(9) << setprecision(4) << vv(satData->indexPhase) << endl;
     1220  }
     1221  if (satData->indexCode) {
     1222    str << _time.timestr(1)
     1223        << " RES " << satData->prn.toAscii().data() << "   P3 "
     1224        << setw(9) << setprecision(4) << vv(satData->indexCode) << endl;
    13241225  }
    13251226}
    … …  
    13531254}
    13541255 
     1256// Update Step (private - loop over outliers)
     1257////////////////////////////////////////////////////////////////////////////
     1258t_irc bncModel::update_p(t_epoData* epoData, ColumnVector& dx) {
     1259
     1260  SymmetricMatrix QQsav;
     1261  ColumnVector    vv;
     1262
     1263  for (int iPhase = 0; iPhase <= 1; iPhase++) {
     1264
     1265    do {
     1266
     1267      if (iPhase == 0) {     
     1268
     1269        // Bancroft Solution
     1270        // -----------------
     1271        if (cmpBancroft(epoData) != success) {
     1272          emit newMessage(_log, false);
     1273          return failure;
     1274        }
     1275     
     1276        // Status Prediction
     1277        // -----------------
     1278        predict(epoData);
     1279      }
     1280     
     1281      // Create First-Design Matrix
     1282      // --------------------------
     1283      unsigned nPar = _params.size();
     1284      unsigned nObs = 0;
     1285      nObs = epoData->sizeGPS() + epoData->sizeGal(); // Glonass code not used
     1286   
     1287      Matrix          AA(nObs, nPar);  // first design matrix
     1288      ColumnVector    ll(nObs);        // tems observed-computed
     1289      DiagonalMatrix  PP(nObs); PP = 0.0;
     1290     
     1291      unsigned iObs = 0;
     1292   
     1293      // GPS
     1294      // ---
     1295      QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
     1296      while (itGPS.hasNext()) {
     1297        itGPS.next();
     1298        t_satData* satData = itGPS.value();
     1299        addObs(iPhase, iObs, satData, AA, ll, PP);
     1300      }
     1301   
     1302      // Galileo
     1303      // -------
     1304      QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
     1305      while (itGal.hasNext()) {
     1306        itGal.next();
     1307        t_satData* satData = itGal.value();
     1308        addObs(iPhase, iObs, satData, AA, ll, PP);
     1309      }
     1310   
     1311      // Compute Filter Update
     1312      // ---------------------
     1313      QQsav = _QQ;
     1314   
     1315      kalman(AA, ll, PP, _QQ, dx);
     1316   
     1317      vv = ll - AA * dx;
     1318   
     1319      // Print Residuals
     1320      // ---------------
     1321      if (true) {
     1322        ostringstream str;
     1323        str.setf(ios::fixed);
     1324   
     1325        QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
     1326        while (itGPS.hasNext()) {
     1327          itGPS.next();
     1328          t_satData* satData = itGPS.value();
     1329          printRes(vv, str, satData);
     1330        }
     1331        QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
     1332        while (itGlo.hasNext()) {
     1333          itGlo.next();
     1334          t_satData* satData = itGlo.value();
     1335          printRes(vv, str, satData);
     1336        }
     1337        QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
     1338        while (itGal.hasNext()) {
     1339          itGal.next();
     1340          t_satData* satData = itGal.value();
     1341          printRes(vv, str, satData);
     1342        }
     1343        _log += str.str().c_str();
     1344      }
     1345   
     1346    } while (outlierDetection(QQsav, vv, epoData->satDataGPS,
     1347                              epoData->satDataGlo, epoData->satDataGal) != 0);
     1348  }
     1349
     1350  return success;
     1351}

  • trunk/BNC/bncmodel.h

    r3286 r3307  
    9696  void   cmpEle(t_satData* satData);
    9797  void   addAmb(t_satData* satData);
    98   void   addObs(unsigned& iObs, t_satData* satData,
     98  void   addObs(int phase, unsigned& iObs, t_satData* satData,
    9999                Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP);
    100100  void   printRes(const ColumnVector& vv,
    … …  
    107107  double delay_saast(double Ele);
    108108  void   predict(t_epoData* epoData);
     109  t_irc  update_p(t_epoData* epoData, ColumnVector& dx);
    109110  int    outlierDetection(const SymmetricMatrix& QQsav,
    110111                          const ColumnVector& vv,
Note: See TracChangeset for help on using the changeset viewer.