Changeset 3408 in ntrip for trunk/BNC/bncmodel.cpp


Ignore:
Timestamp:
Sep 2, 2011, 8:43:49 PM (13 years ago)
Author:
mervart
Message:
 
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/bncmodel.cpp

    r3407 r3408  
    5252#include "bnctides.h"
    5353#include "bncantex.h"
     54#include "bnccomb.h"
    5455
    5556using namespace std;
    5657
    57 const unsigned MINOBS           =    5;
    58 const double   MINELE_GPS       = 10.0 * M_PI / 180.0;
    59 const double   MINELE_GLO       = 10.0 * M_PI / 180.0;
    60 const double   MINELE_GAL       = 10.0 * M_PI / 180.0;
    61 const double   MAXRES_CODE_GPS  = 10.0;
    62 const double   MAXRES_PHASE_GPS = 0.05;
    63 const double   MAXRES_PHASE_GLO = 0.05;
    64 const double   MAXRES_CODE_GAL  = 10.0;
    65 const double   MAXRES_PHASE_GAL = 0.05;
     58const unsigned MINOBS                = 5;
     59const double   MINELE                = 10.0 * M_PI / 180.0;
     60const double   MAXRES_CODE           = 10.0;
     61const double   MAXRES_PHASE          = 0.02;
     62const double   GLONASS_WEIGHT_FACTOR = 5.0;
    6663
    6764// Constructor
    … …  
    257254  _xcBanc.ReSize(4);  _xcBanc  = 0.0;
    258255  _ellBanc.ReSize(3); _ellBanc = 0.0;
     256
     257  // Save copy of data (used in outlier detection)
     258  // ---------------------------------------------
     259  _epoData_sav = new t_epoData();
    259260}
    260261
    … …  
    271272    delete _params[iPar-1];
    272273  }
     274  delete _epoData_sav;
    273275}
    274276
    … …  
    322324  Tracer tracer("bncModel::cmpBancroft");
    323325
    324   if (epoData->sizeGPS() < MINOBS) {
     326  if (epoData->sizeSys('G') < MINOBS) {
    325327    _log += "bncModel::cmpBancroft: not enough data\n";
    326328    return failure;
    327329  }
    328330
    329   Matrix BB(epoData->sizeGPS(), 4);
    330 
    331   QMapIterator<QString, t_satData*> it(epoData->satDataGPS);
     331  Matrix BB(epoData->sizeSys('G'), 4);
     332
     333  QMapIterator<QString, t_satData*> it(epoData->satData);
    332334  int iObsBanc = 0;
    333335  while (it.hasNext()) {
    334     ++iObsBanc;
    335336    it.next();
    336     QString    prn     = it.key();
    337337    t_satData* satData = it.value();
    338     BB(iObsBanc, 1) = satData->xx(1);
    339     BB(iObsBanc, 2) = satData->xx(2);
    340     BB(iObsBanc, 3) = satData->xx(3);
    341     BB(iObsBanc, 4) = satData->P3 + satData->clk;
     338    if (satData->system() == 'G') {
     339      ++iObsBanc;
     340      QString    prn     = it.key();
     341      BB(iObsBanc, 1) = satData->xx(1);
     342      BB(iObsBanc, 2) = satData->xx(2);
     343      BB(iObsBanc, 3) = satData->xx(3);
     344      BB(iObsBanc, 4) = satData->P3 + satData->clk;
     345    }
    342346  }
    343347
    … …  
    350354  // Compute Satellite Elevations
    351355  // ----------------------------
    352   QMutableMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS);
    353   while (iGPS.hasNext()) {
    354     iGPS.next();
    355     t_satData* satData = iGPS.value();
     356  QMutableMapIterator<QString, t_satData*> im(epoData->satData);
     357  while (im.hasNext()) {
     358    im.next();
     359    t_satData* satData = im.value();
    356360    cmpEle(satData);
    357     if (satData->eleSat < MINELE_GPS) {
     361    if (satData->eleSat < MINELE) {
    358362      delete satData;
    359       iGPS.remove();
    360     }
    361   }
    362 
    363   QMutableMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo);
    364   while (iGlo.hasNext()) {
    365     iGlo.next();
    366     t_satData* satData = iGlo.value();
    367     cmpEle(satData);
    368     if (satData->eleSat < MINELE_GLO) {
    369       delete satData;
    370       iGlo.remove();
    371     }
    372   }
    373 
    374   QMutableMapIterator<QString, t_satData*> iGal(epoData->satDataGal);
    375   while (iGal.hasNext()) {
    376     iGal.next();
    377     t_satData* satData = iGal.value();
    378     cmpEle(satData);
    379     if (satData->eleSat < MINELE_GAL) {
    380       delete satData;
    381       iGal.remove();
     363      im.remove();
    382364    }
    383365  }
    … …  
    600582    // ------------------------------------------------
    601583    int iPar = 0;
    602     QMutableVectorIterator<bncParam*> it(_params);
    603     while (it.hasNext()) {
    604       bncParam* par = it.next();
     584    QMutableVectorIterator<bncParam*> im(_params);
     585    while (im.hasNext()) {
     586      bncParam* par = im.next();
    605587      bool removed = false;
    606588      if (par->type == bncParam::AMB_L3) {
    607         if (epoData->satDataGPS.find(par->prn) == epoData->satDataGPS.end() &&
    608             epoData->satDataGlo.find(par->prn) == epoData->satDataGlo.end() &&
    609             epoData->satDataGal.find(par->prn) == epoData->satDataGal.end() ) {
     589        if (epoData->satData.find(par->prn) == epoData->satData.end()) {
    610590          removed = true;
    611591          delete par;
    612           it.remove();
     592          im.remove();
    613593        }
    614594      }
    … …  
    621601    // Add new ambiguity parameters
    622602    // ----------------------------
    623     QMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS);
    624     while (iGPS.hasNext()) {
    625       iGPS.next();
    626       t_satData* satData = iGPS.value();
    627       addAmb(satData);
    628     }
    629 
    630     QMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo);
    631     while (iGlo.hasNext()) {
    632       iGlo.next();
    633       t_satData* satData = iGlo.value();
    634       addAmb(satData);
    635     }
    636 
    637     QMapIterator<QString, t_satData*> iGal(epoData->satDataGal);
    638     while (iGal.hasNext()) {
    639       iGal.next();
    640       t_satData* satData = iGal.value();
     603    QMapIterator<QString, t_satData*> it(epoData->satData);
     604    while (it.hasNext()) {
     605      it.next();
     606      t_satData* satData = it.value();
    641607      addAmb(satData);
    642608    }
    … …  
    691657  // ----------------------
    692658  if (update_p(epoData) != success) {
     659    emit newMessage(_log, false);
    693660    return failure;
    694661  }
    … …  
    936903// Outlier Detection
    937904////////////////////////////////////////////////////////////////////////////
    938 QString bncModel::outlierDetection(int iPhase, const SymmetricMatrix& QQsav,
    939                                    const ColumnVector& vv,
    940                                    QMap<QString, t_satData*>& satDataGPS,
    941                                    QMap<QString, t_satData*>& satDataGlo,
    942                                    QMap<QString, t_satData*>& satDataGal) {
     905bool bncModel::outlierDetection(int iPhase, const ColumnVector& vv,
     906                                QMap<QString, t_satData*>& satData) {
    943907
    944908  Tracer tracer("bncModel::outlierDetection");
    945909
    946   QString prnCode;
    947   QString prnPhase;
    948   double  maxResCode  = 0.0;
    949   double  maxResPhase = 0.0;
    950 
    951   QString prnRemoved;
    952   double  maxRes;
    953 
    954   if (iPhase == 0) {
    955 
    956     // Check GPS Code
    957     // --------------
    958     if (prnRemoved.isEmpty()) {
    959       findMaxRes(iPhase, vv,satDataGPS, prnCode, maxResCode, prnPhase, maxResPhase);
    960       if (maxResCode > MAXRES_CODE_GPS) {
    961         satDataGPS.remove(prnCode);
    962         prnRemoved = prnCode;
    963         maxRes     = maxResCode;
    964       }
    965     }
    966    
    967     // Check Galileo Code
    968     // ------------------
    969     if (prnRemoved.isEmpty()) {
    970       findMaxRes(iPhase, vv,satDataGal, prnCode, maxResCode, prnPhase, maxResPhase);
    971       if (maxResCode > MAXRES_CODE_GAL) {
    972         satDataGal.remove(prnCode);
    973         prnRemoved = prnCode;
    974         maxRes     = maxResCode;
    975       }
    976     }
    977   }
    978 
     910  QString prn;
     911  double  maxRes  = 0.0;
     912  findMaxRes(vv, satData, prn, maxRes);
     913
     914  if      (iPhase == 1 && maxRes > MAXRES_PHASE) {
     915    _log += "Outlier Phase " + prn + " "
     916          + QByteArray::number(maxRes, 'f', 3) + "\n";
     917    return true;
     918  }
     919  else if (iPhase == 0 && maxRes > MAXRES_CODE) {
     920    _log += "Outlier Code  " + prn + " "
     921          + QByteArray::number(maxRes, 'f', 3) + "\n";
     922    return true;
     923  }
    979924  else {
    980 
    981     // Check Glonass Phase
    982     // -------------------
    983     if (prnRemoved.isEmpty()) {
    984       findMaxRes(iPhase, vv,satDataGlo, prnCode, maxResCode, prnPhase, maxResPhase);
    985       if (maxResPhase > MAXRES_PHASE_GLO) {
    986         satDataGlo.remove(prnPhase);
    987         prnRemoved = prnPhase;
    988         maxRes     = maxResPhase;
    989       }
    990     }
    991    
    992     // Check Galileo Phase
    993     // -------------------
    994     if (prnRemoved.isEmpty()) {
    995       findMaxRes(iPhase, vv,satDataGal, prnCode, maxResCode, prnPhase, maxResPhase);
    996       if      (maxResPhase > MAXRES_PHASE_GAL) {
    997         satDataGal.remove(prnPhase);
    998         prnRemoved = prnPhase;
    999         maxRes     = maxResPhase;
    1000       }
    1001     }
    1002    
    1003     // Check GPS Phase
    1004     // ---------------
    1005     if (prnRemoved.isEmpty()) {
    1006       findMaxRes(iPhase, vv,satDataGPS, prnCode, maxResCode, prnPhase, maxResPhase);
    1007       if      (maxResPhase > MAXRES_PHASE_GPS) {
    1008         satDataGPS.remove(prnPhase);
    1009         prnRemoved = prnPhase;
    1010         maxRes     = maxResPhase;
    1011       }
    1012     }
    1013   }
    1014  
    1015   if (!prnRemoved.isEmpty()) {
    1016     _log += "Outlier " + prnRemoved.toAscii() + " "
    1017           + QByteArray::number(maxRes, 'f', 3) + "\n";
    1018     _QQ = QQsav;
    1019     QVectorIterator<bncParam*> itPar(_params);
    1020     while (itPar.hasNext()) {
    1021       bncParam* par = itPar.next();
    1022       if (par->type == bncParam::AMB_L3 && par->prn == prnRemoved) {
    1023         par->numEpo = 0;
    1024       }
    1025     }
    1026   }
    1027 
    1028   return prnRemoved;
     925    return false;
     926  }
    1029927}
    1030928
    … …  
    1051949}
    1052950
    1053 ////
     951//
    1054952//////////////////////////////////////////////////////////////////////////////
    1055953void bncModel::kalman(const Matrix& AA, const ColumnVector& ll,
    … …  
    12231121  }
    12241122
     1123  // Remember Observation Index
     1124  // --------------------------
     1125  ++iObs;
     1126  satData->obsIndex = iObs;
     1127
    12251128  // Phase Observations
    12261129  // ------------------
    12271130  if (iPhase == 1) {
    1228     ++iObs;
    12291131    ll(iObs)      = satData->L3 - cmpValue(satData, true);
    1230     PP(iObs,iObs) = 1.0 / (_sigL3 * _sigL3) / (ellWgtCoef * ellWgtCoef);
     1132    double sigL3 = _sigL3;
    12311133    if (satData->system() == 'R') {
    1232       PP(iObs,iObs) /= 25.0;
    1233     }
     1134      sigL3 *= GLONASS_WEIGHT_FACTOR;
     1135    }
     1136    PP(iObs,iObs) = 1.0 / (sigL3 * sigL3) / (ellWgtCoef * ellWgtCoef);
    12341137    for (int iPar = 1; iPar <= _params.size(); iPar++) {
    12351138      if (_params[iPar-1]->type == bncParam::AMB_L3 &&
    … …  
    12391142      AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
    12401143    }
    1241     satData->indexPhase = iObs;
    12421144  }
    12431145
    … …  
    12451147  // -----------------
    12461148  else {
    1247     ++iObs;
    12481149    ll(iObs)      = satData->P3 - cmpValue(satData, false);
    12491150    PP(iObs,iObs) = 1.0 / (_sigP3 * _sigP3) / (ellWgtCoef * ellWgtCoef);
    … …  
    12511152      AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
    12521153    }
    1253     satData->indexCode = iObs;
    12541154  }
    12551155}
    … …  
    12571157//
    12581158///////////////////////////////////////////////////////////////////////////
    1259 void bncModel::printRes(int iPhase, const ColumnVector& vv,
    1260                         ostringstream& str, t_satData* satData) {
     1159QByteArray bncModel::printRes(int iPhase, const ColumnVector& vv,
     1160                              const QMap<QString, t_satData*>& satDataMap) {
     1161
    12611162  Tracer tracer("bncModel::printRes");
    1262   if (iPhase == 1) {
    1263     str << _time.timestr(1)
    1264         << " RES " << satData->prn.toAscii().data() << "   L3 "
    1265         << setw(9) << setprecision(4) << vv(satData->indexPhase) << endl;
    1266   }
    1267   else {
    1268     str << _time.timestr(1)
    1269         << " RES " << satData->prn.toAscii().data() << "   P3 "
    1270         << setw(9) << setprecision(4) << vv(satData->indexCode) << endl;
    1271   }
     1163
     1164  ostringstream str;
     1165  str.setf(ios::fixed);
     1166       
     1167  QMapIterator<QString, t_satData*> it(satDataMap);
     1168  while (it.hasNext()) {
     1169    it.next();
     1170    t_satData* satData = it.value();
     1171    if (satData->obsIndex != 0) {
     1172      str << _time.timestr(1)
     1173          << " RES " << satData->prn.toAscii().data()
     1174          << (iPhase ? "   L3 " : "   P3 ")
     1175          << setw(9) << setprecision(4) << vv(satData->obsIndex) << endl;
     1176    }
     1177  }
     1178
     1179  return QByteArray(str.str().c_str());
    12721180}
    12731181
    12741182//
    12751183///////////////////////////////////////////////////////////////////////////
    1276 void bncModel::findMaxRes(int iPhase, const ColumnVector& vv,
     1184void bncModel::findMaxRes(const ColumnVector& vv,
    12771185                          const QMap<QString, t_satData*>& satData,
    1278                           QString& prnCode,  double& maxResCode,
    1279                           QString& prnPhase, double& maxResPhase) {
     1186                          QString& prn,  double& maxRes) {
     1187
    12801188  Tracer tracer("bncModel::findMaxRes");
    1281   maxResCode  = 0.0;
    1282   maxResPhase = 0.0;
     1189
     1190  maxRes  = 0.0;
    12831191
    12841192  QMapIterator<QString, t_satData*> it(satData);
    … …  
    12861194    it.next();
    12871195    t_satData* satData = it.value();
    1288     if (iPhase == 0) {
    1289       if (satData->indexCode) {
    1290         if (fabs(vv(satData->indexCode)) > maxResCode) {
    1291           maxResCode = fabs(vv(satData->indexCode));
    1292           prnCode    = satData->prn;
    1293         }
    1294       }
    1295     }
    1296     else {
    1297       if (satData->indexPhase) {
    1298         if (fabs(vv(satData->indexPhase)) > maxResPhase) {
    1299           maxResPhase = fabs(vv(satData->indexPhase));
    1300           prnPhase    = satData->prn;
    1301         }
    1302       }
     1196    if (satData->obsIndex != 0 && fabs(vv(satData->obsIndex)) > maxRes) {
     1197      maxRes = fabs(vv(satData->obsIndex));
     1198      prn    = satData->prn;
    13031199    }
    13041200  }
    … …  
    13111207  Tracer tracer("bncModel::update_p");
    13121208
    1313   rememberState();
    1314 
    1315   for (int iPhase = 0; iPhase <= (_usePhase ? 1 : 0); iPhase++) {
    1316 
    1317     SymmetricMatrix QQsav;
    1318     ColumnVector    vv;
    1319     ColumnVector    dx;
    1320 
    1321     unsigned numOutliers = 0;
    1322 
    1323     for (;;) {
     1209  // Save Variance-Covariance Matrix, and Status Vector
     1210  // --------------------------------------------------
     1211  rememberState(epoData);
     1212
     1213  ColumnVector dx;
     1214
     1215  std::vector<QString> allPrns;
     1216  QMapIterator<QString, t_satData*> it(epoData->satData);
     1217  while (it.hasNext()) {
     1218    it.next();
     1219    t_satData* satData = it.value();
     1220    allPrns.push_back(satData->prn);
     1221  }
     1222  std::vector<QString> usedPrns;
     1223
     1224  // Try with all satellites, then with all minus one, etc.
     1225  // ------------------------------------------------------
     1226  const unsigned MAX_NEGLECT = 1;
     1227  for (unsigned nNeglected = 0; nNeglected <= MAX_NEGLECT; nNeglected++) {
     1228    usedPrns = allPrns;
     1229
     1230    for (unsigned ii = 0; ii < nNeglected && usedPrns.size() > 0; ii++) {
     1231      usedPrns.pop_back();
     1232    }
     1233
     1234    // Loop over all Combinations of used Satellites
     1235    // ---------------------------------------------
     1236    do {
     1237
     1238      QByteArray strResCode;
     1239      QByteArray strResPhase;
     1240      QString    strNeglected;
     1241
     1242      // Remove Neglected Satellites from epoData
     1243      // ----------------------------------------
     1244      for (unsigned ip = 0; ip < allPrns.size(); ip++) {
     1245        QString prn = allPrns[ip];
     1246        if ( !findInVector(usedPrns, prn) ) {
     1247          epoData->satData.remove(prn);
     1248          strNeglected += prn + " ";
     1249        }
     1250      }
     1251      if (epoData->sizeSys('G') < MINOBS) {
     1252        continue;
     1253      }
    13241254
    13251255      // Bancroft Solution
    13261256      // -----------------
    1327       if (iPhase == 0) {     
    1328         if (cmpBancroft(epoData) != success) {
    1329           restoreState();
    1330           emit newMessage(_log, false);
    1331           return failure;
    1332         }
    1333       }
    1334       else {
    1335         if (epoData->sizeGPS() < MINOBS) {
    1336           restoreState();
    1337           _log += "bncModel::update_p: not enough data\n";
    1338           emit newMessage(_log, false);
    1339           return failure;
    1340         }
    1341         unsigned numSatNoSlip = 0;
    1342         QVectorIterator<bncParam*> itPar(_params);
    1343         while (itPar.hasNext()) {
    1344           bncParam* par = itPar.next();
    1345           if (par->type == bncParam::AMB_L3 && par->prn[0] == 'G') {
    1346             if (par->numEpo >= 1) {
    1347               ++numSatNoSlip;
    1348             }
     1257      if (cmpBancroft(epoData) != success) {
     1258        continue;
     1259      }
     1260
     1261      // First update using code observations, then phase observations
     1262      // -------------------------------------------------------------     
     1263      for (int iPhase = 0; iPhase <= (_usePhase ? 1 : 0); iPhase++) {
     1264     
     1265        // Status Prediction
     1266        // -----------------
     1267        predict(iPhase, epoData);
     1268       
     1269        // Create First-Design Matrix
     1270        // --------------------------
     1271        unsigned nPar = _params.size();
     1272        unsigned nObs = 0;
     1273        if (iPhase == 0) {
     1274          nObs = epoData->sizeAll() - epoData->sizeSys('R'); // Glonass code not used
     1275        }
     1276        else {
     1277          nObs = epoData->sizeAll();
     1278        }
     1279       
     1280        // Prepare first-design Matrix, vector observed-computed
     1281        // -----------------------------------------------------
     1282        Matrix          AA(nObs, nPar);  // first design matrix
     1283        ColumnVector    ll(nObs);        // tems observed-computed
     1284        DiagonalMatrix  PP(nObs); PP = 0.0;
     1285       
     1286        unsigned iObs = 0;
     1287        QMapIterator<QString, t_satData*> it(epoData->satData);
     1288        while (it.hasNext()) {
     1289          it.next();
     1290          t_satData* satData = it.value();
     1291          if (iPhase == 1 || satData->system() != 'R') {
     1292            QString prn = satData->prn;
     1293            addObs(iPhase, iObs, satData, AA, ll, PP);
    13491294          }
    13501295        }
    1351         if (numSatNoSlip > 0 && numSatNoSlip < MINOBS) {
    1352           restoreState();
    1353           _log += "bncModel::update_p: not enough GPS satellites without cycle-slips\n";
    1354           emit newMessage(_log, false);
    1355           return failure;
    1356         }
    1357       }
    1358      
    1359       // Status Prediction
    1360       // -----------------
    1361       predict(iPhase, epoData);
    1362      
    1363       // Create First-Design Matrix
    1364       // --------------------------
    1365       unsigned nPar = _params.size();
    1366       unsigned nObs = 0;
    1367       if (iPhase == 0) {
    1368         nObs = epoData->sizeGPS() + epoData->sizeGal(); // Glonass code not used
    1369       }
    1370       else {
    1371         nObs = epoData->sizeGPS() + epoData->sizeGal() + epoData->sizeGlo();
    1372       }
    1373    
    1374       Matrix          AA(nObs, nPar);  // first design matrix
    1375       ColumnVector    ll(nObs);        // tems observed-computed
    1376       DiagonalMatrix  PP(nObs); PP = 0.0;
    1377      
    1378       unsigned iObs = 0;
    1379    
    1380       // GPS
    1381       // ---
    1382       QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
    1383       while (itGPS.hasNext()) {
    1384         itGPS.next();
    1385         t_satData* satData = itGPS.value();
    1386         addObs(iPhase, iObs, satData, AA, ll, PP);
    1387       }
    1388    
    1389       // Glonass
    1390       // -------
    1391       if (iPhase == 1) {
    1392         QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
    1393         while (itGlo.hasNext()) {
    1394           itGlo.next();
    1395           t_satData* satData = itGlo.value();
    1396           addObs(iPhase, iObs, satData, AA, ll, PP);
    1397         }
    1398       }
    1399 
    1400       // Galileo
    1401       // -------
    1402       QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
    1403       while (itGal.hasNext()) {
    1404         itGal.next();
    1405         t_satData* satData = itGal.value();
    1406         addObs(iPhase, iObs, satData, AA, ll, PP);
    1407       }
    1408    
    1409       // Compute Filter Update
    1410       // ---------------------
    1411       QQsav = _QQ;
    1412    
    1413       kalman(AA, ll, PP, _QQ, dx);
    1414    
    1415       vv = ll - AA * dx;
    1416 
    1417       // Print Residuals
    1418       // ---------------
    1419       if (true) {
    1420         ostringstream str;
    1421         str.setf(ios::fixed);
    1422    
    1423         QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
    1424         while (itGPS.hasNext()) {
    1425           itGPS.next();
    1426           t_satData* satData = itGPS.value();
    1427           printRes(iPhase, vv, str, satData);
    1428         }
    1429         if (iPhase == 1) {
    1430           QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
    1431           while (itGlo.hasNext()) {
    1432             itGlo.next();
    1433             t_satData* satData = itGlo.value();
    1434             printRes(iPhase, vv, str, satData);
     1296
     1297        // Compute Filter Update
     1298        // ---------------------
     1299        kalman(AA, ll, PP, _QQ, dx);
     1300       
     1301        ColumnVector vv = ll - AA * dx;
     1302       
     1303        // Print Residuals
     1304        // ---------------
     1305        if (iPhase == 0) {
     1306          strResCode  = printRes(iPhase, vv, epoData->satData);
     1307        }
     1308        else {
     1309          strResPhase = printRes(iPhase, vv, epoData->satData);
     1310        }
     1311
     1312        // Check the residuals
     1313        // -------------------
     1314        if ( outlierDetection(iPhase, vv, epoData->satData) ) {
     1315          restoreState(epoData);
     1316          break;
     1317        }
     1318
     1319        // Set estimated values
     1320        // --------------------
     1321        else if (!_usePhase || iPhase == 1) {
     1322          QVectorIterator<bncParam*> itPar(_params);
     1323          while (itPar.hasNext()) {
     1324            bncParam* par = itPar.next();
     1325            par->xx += dx(par->index);
    14351326          }
    1436         }
    1437         QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
    1438         while (itGal.hasNext()) {
    1439           itGal.next();
    1440           t_satData* satData = itGal.value();
    1441           printRes(iPhase, vv, str, satData);
    1442         }
    1443         _log += str.str().c_str();
    1444       }
    1445    
    1446       QString prnRemoved = outlierDetection(iPhase, QQsav, vv,
    1447                                             epoData->satDataGPS,
    1448                                             epoData->satDataGlo,
    1449                                             epoData->satDataGal);
    1450       if (prnRemoved.isEmpty()) {
    1451         break;
    1452       }
    1453       else if (prnRemoved[0] == 'G') {
    1454         ++numOutliers;
    1455       }
    1456 
    1457       if (numOutliers > 1) {
    1458         restoreState();
    1459         _log += "bncModel::update_p: too many outliers\n";
    1460         emit newMessage(_log, false);
    1461         return failure;
    1462       }
    1463 
    1464     }
    1465 
    1466     // Update Parameters
    1467     // -----------------
    1468     QVectorIterator<bncParam*> itPar(_params);
    1469     while (itPar.hasNext()) {
    1470       bncParam* par = itPar.next();
    1471       par->xx += dx(par->index);
    1472     }
    1473   }
    1474 
    1475   return success;
     1327
     1328          // Print Neglected PRNs
     1329          // --------------------
     1330          if (nNeglected > 0) {
     1331            _log += "Neglected PRNs: " + strNeglected + '\n';
     1332          }
     1333
     1334          _log += strResCode + strResPhase;
     1335       
     1336          return success;
     1337        }
     1338
     1339      } // for (int iPhase = 0; iPhase <= (_usePhase ? 1 : 0); iPhase++)
     1340
     1341    } while ( next_combination(allPrns.begin(), allPrns.end(),
     1342                               usedPrns.begin(), usedPrns.end()) );
     1343
     1344  } // for (unsigned nNeglected
     1345
     1346  return failure;
    14761347}
    14771348
    14781349// Remeber Original State Vector and Variance-Covariance Matrix
    14791350////////////////////////////////////////////////////////////////////////////
    1480 void bncModel::rememberState() {
     1351void bncModel::rememberState(t_epoData* epoData) {
    14811352
    14821353  _QQ_sav = _QQ;
    … …  
    14941365    _params_sav.push_back(new bncParam(*par));
    14951366  }
     1367
     1368  _epoData_sav->deepCopy(epoData);
    14961369}
    14971370
    14981371// Restore Original State Vector and Variance-Covariance Matrix
    14991372////////////////////////////////////////////////////////////////////////////
    1500 void bncModel::restoreState() {
     1373void bncModel::restoreState(t_epoData* epoData) {
    15011374
    15021375  _QQ = _QQ_sav;
    … …  
    15141387    _params.push_back(new bncParam(*par));
    15151388  }
    1516 }
    1517 
     1389
     1390  epoData->deepCopy(_epoData_sav);
     1391}
Note: See TracChangeset for help on using the changeset viewer.