Changeset 3375 in ntrip for trunk


Ignore:
Timestamp:
Aug 28, 2011, 3:54:10 PM (13 years ago)
Author:
mervart
Message:

LM: new Outlier Detection

Location:
trunk/BNC
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/bnc.pro

    r3374 r3375  
    22# Switch to debug configuration
    33# -----------------------------
    4 CONFIG -= debug
    5 CONFIG += release
     4CONFIG -= release
     5CONFIG += debug
    66
    77

  • trunk/BNC/bncmodel.cpp

    r3331 r3375  
    5252#include "bnctides.h"
    5353#include "bncantex.h"
     54#include "bnccomb.h"
    5455
    5556using namespace std;
    … …  
    6061const double   MINELE_GAL       = 10.0 * M_PI / 180.0;
    6162const double   MAXRES_CODE_GPS  = 10.0;
    62 const double   MAXRES_PHASE_GPS = 0.05;
    63 const double   MAXRES_PHASE_GLO = 0.05;
     63const double   MAXRES_PHASE_GPS = 0.04;
     64const double   MAXRES_PHASE_GLO = 0.04;
    6465const double   MAXRES_CODE_GAL  = 10.0;
    65 const double   MAXRES_PHASE_GAL = 0.05;
     66const double   MAXRES_PHASE_GAL = 0.04;
    6667
    6768// Constructor
    … …  
    257258  _xcBanc.ReSize(4);  _xcBanc  = 0.0;
    258259  _ellBanc.ReSize(3); _ellBanc = 0.0;
     260
     261  // Save for Outlier Detection
     262  // --------------------------
     263  _epoData_sav = 0;
    259264}
    260265
    … …  
    691696  // ----------------------
    692697  if (update_p(epoData) != success) {
     698    emit newMessage(_log, false);
    693699    return failure;
    694700  }
    … …  
    936942// Outlier Detection
    937943////////////////////////////////////////////////////////////////////////////
    938 int bncModel::outlierDetection(int iPhase, const SymmetricMatrix& QQsav,
    939                                const ColumnVector& vv,
     944bool bncModel::outlierDetection(int iPhase, const ColumnVector& vv,
    940945                               QMap<QString, t_satData*>& satDataGPS,
    941946                               QMap<QString, t_satData*>& satDataGlo,
    … …  
    952957  double  maxRes;
    953958
    954   int irc = 0;
     959  bool irc = false;
    955960
    956961  if (iPhase == 0) {
    … …  
    958963    // Check GPS Code
    959964    // --------------
    960     if (irc == 0) {
     965    if (!irc) {
    961966      findMaxRes(iPhase, vv,satDataGPS, prnCode, maxResCode, prnPhase, maxResPhase);
    962967      if (maxResCode > MAXRES_CODE_GPS) {
    963         satDataGPS.remove(prnCode);
    964968        prnRemoved = prnCode;
    965969        maxRes     = maxResCode;
    966         irc        = 1;
     970        irc        = true;
    967971      }
    968972    }
    … …  
    970974    // Check Galileo Code
    971975    // ------------------
    972     if (irc == 0) {
     976    if (!irc) {
    973977      findMaxRes(iPhase, vv,satDataGal, prnCode, maxResCode, prnPhase, maxResPhase);
    974978      if (maxResCode > MAXRES_CODE_GAL) {
    975         satDataGal.remove(prnCode);
    976979        prnRemoved = prnCode;
    977980        maxRes     = maxResCode;
    978         irc        = 1;
     981        irc        = true;
    979982      }
    980983    }
    … …  
    985988    // Check Glonass Phase
    986989    // -------------------
    987     if (irc == 0) {
     990    if (!irc) {
    988991      findMaxRes(iPhase, vv,satDataGlo, prnCode, maxResCode, prnPhase, maxResPhase);
    989992      if (maxResPhase > MAXRES_PHASE_GLO) {
    990         satDataGlo.remove(prnPhase);
    991993        prnRemoved = prnPhase;
    992994        maxRes     = maxResPhase;
    993         irc        = 1;
     995        irc        = true;
    994996      }
    995997    }
    … …  
    997999    // Check Galileo Phase
    9981000    // -------------------
    999     if (irc == 0) {
     1001    if (!irc) {
    10001002      findMaxRes(iPhase, vv,satDataGal, prnCode, maxResCode, prnPhase, maxResPhase);
    10011003      if      (maxResPhase > MAXRES_PHASE_GAL) {
    1002         satDataGal.remove(prnPhase);
    10031004        prnRemoved = prnPhase;
    10041005        maxRes     = maxResPhase;
    1005         irc        = 1;
     1006        irc        = true;
    10061007      }
    10071008    }
    … …  
    10091010    // Check GPS Phase
    10101011    // ---------------
    1011     if (irc == 0) {
     1012    if (!irc) {
    10121013      findMaxRes(iPhase, vv,satDataGPS, prnCode, maxResCode, prnPhase, maxResPhase);
    10131014      if      (maxResPhase > MAXRES_PHASE_GPS) {
    1014         satDataGPS.remove(prnPhase);
    10151015        prnRemoved = prnPhase;
    10161016        maxRes     = maxResPhase;
    1017         irc        = 1;
     1017        irc        = true;
    10181018      }
    10191019    }
    10201020  }
    10211021 
    1022   if (irc != 0) {
     1022  if (irc) {
    10231023    _log += "Outlier " + prnRemoved.toAscii() + " "
    10241024          + QByteArray::number(maxRes, 'f', 3) + "\n";
    1025     _QQ = QQsav;
    1026     QVectorIterator<bncParam*> itPar(_params);
    1027     while (itPar.hasNext()) {
    1028       bncParam* par = itPar.next();
    1029       if (par->type == bncParam::AMB_L3 && par->prn == prnRemoved) {
    1030         par->numEpo = 0;
    1031       }
    1032     }
    10331025  }
    10341026
    … …  
    13121304}
    13131305 
     1306bool findInVector(const std::vector<QString>& vv, const QString& str) {
     1307  std::vector<QString>::const_iterator it;
     1308  for (it = vv.begin(); it != vv.end(); ++it) {
     1309    if ( (*it) == str) {
     1310      return true;
     1311    }
     1312  }
     1313  return false;
     1314}
     1315
    13141316// Update Step (private - loop over outliers)
    13151317////////////////////////////////////////////////////////////////////////////
    … …  
    13181320  Tracer tracer("bncModel::update_p");
    13191321
    1320   rememberState();
    1321 
    1322   for (int iPhase = 0; iPhase <= (_usePhase ? 1 : 0); iPhase++) {
    1323 
    1324     SymmetricMatrix QQsav;
    1325     ColumnVector    vv;
    1326     ColumnVector    dx;
    1327 
     1322  rememberState(epoData);
     1323
     1324  ColumnVector dx;
     1325
     1326  std::vector<QString> allPrns;
     1327  getAllPrns(epoData, &allPrns);
     1328
     1329  std::vector<QString> usedPrns;
     1330
     1331  // Try with all satellites, then with all minus one, etc.
     1332  // ------------------------------------------------------
     1333  for (unsigned nNeglected = 0; nNeglected < allPrns.size(); nNeglected++) {
     1334    usedPrns = allPrns;
     1335
     1336    for (unsigned ii = 0; ii < nNeglected && usedPrns.size() > 0; ii++) {
     1337      usedPrns.pop_back();
     1338    }
     1339
     1340    bool outlierDetected = false;
     1341
     1342    // Loop over all Combinations of "nUsed" Satellites
     1343    // ------------------------------------------------
    13281344    do {
    13291345
    1330       // Bancroft Solution
    1331       // -----------------
    1332       if (iPhase == 0) {     
    1333         if (cmpBancroft(epoData) != success) {
    1334           restoreState();
    1335           emit newMessage(_log, false);
    1336           return failure;
    1337         }
    1338       }
    1339       else {
    1340         if (epoData->sizeGPS() < MINOBS) {
    1341           restoreState();
    1342           _log += "bncModel::update_p: not enough data\n";
    1343           emit newMessage(_log, false);
    1344           return failure;
    1345         }
    1346         unsigned numSatNoSlip = 0;
    1347         QVectorIterator<bncParam*> itPar(_params);
    1348         while (itPar.hasNext()) {
    1349           bncParam* par = itPar.next();
    1350           if (par->type == bncParam::AMB_L3 && par->prn[0] == 'G') {
    1351             if (par->numEpo >= 1) {
    1352               ++numSatNoSlip;
     1346      if (outlierDetected) {
     1347        _log += "TRY WITH PRNs: ";
     1348        for (unsigned ip = 0; ip < usedPrns.size(); ip++) {
     1349          _log += usedPrns[ip] + ' ';
     1350        }
     1351        _log += '\n';
     1352      }
     1353
     1354      for (unsigned ip = 0; ip < allPrns.size(); ip++) {
     1355        QString prn = allPrns[ip];
     1356        if ( !findInVector(usedPrns, prn) ) {
     1357          epoData->satDataGPS.remove(prn);
     1358          epoData->satDataGlo.remove(prn);
     1359          epoData->satDataGal.remove(prn);
     1360        }
     1361      }
     1362
     1363      // First update using code observations, then phase observations
     1364      // -------------------------------------------------------------     
     1365      for (int iPhase = 0; iPhase <= (_usePhase ? 1 : 0); iPhase++) {
     1366     
     1367        // Bancroft Solution
     1368        // -----------------
     1369        if (iPhase == 0) {     
     1370          if (cmpBancroft(epoData) != success) {
     1371            restoreState(epoData);
     1372            return failure;
     1373          }
     1374        }
     1375        else {
     1376          if (epoData->sizeGPS() < MINOBS) {
     1377            restoreState(epoData);
     1378            _log += "bncModel::update_p: not enough data\n";
     1379            return failure;
     1380          }
     1381          unsigned numSatNoSlip = 0;
     1382          QVectorIterator<bncParam*> itPar(_params);
     1383          while (itPar.hasNext()) {
     1384            bncParam* par = itPar.next();
     1385            if (par->type == bncParam::AMB_L3 && par->prn[0] == 'G') {
     1386              if (par->numEpo >= 1) {
     1387                ++numSatNoSlip;
     1388              }
    13531389            }
    13541390          }
    1355         }
    1356         if (numSatNoSlip > 0 && numSatNoSlip < MINOBS) {
    1357           restoreState();
    1358           _log += "bncModel::update_p: not enough GPS satellites without cycle-slips\n";
    1359           emit newMessage(_log, false);
    1360           return failure;
    1361         }
    1362       }
    1363      
    1364       // Status Prediction
    1365       // -----------------
    1366       predict(iPhase, epoData);
    1367      
    1368       // Create First-Design Matrix
    1369       // --------------------------
    1370       unsigned nPar = _params.size();
    1371       unsigned nObs = 0;
    1372       if (iPhase == 0) {
    1373         nObs = epoData->sizeGPS() + epoData->sizeGal(); // Glonass code not used
    1374       }
    1375       else {
    1376         nObs = epoData->sizeGPS() + epoData->sizeGal() + epoData->sizeGlo();
    1377       }
    1378    
    1379       Matrix          AA(nObs, nPar);  // first design matrix
    1380       ColumnVector    ll(nObs);        // tems observed-computed
    1381       DiagonalMatrix  PP(nObs); PP = 0.0;
    1382      
    1383       unsigned iObs = 0;
    1384    
    1385       // GPS
    1386       // ---
    1387       QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
    1388       while (itGPS.hasNext()) {
    1389         itGPS.next();
    1390         t_satData* satData = itGPS.value();
    1391         addObs(iPhase, iObs, satData, AA, ll, PP);
    1392       }
    1393    
    1394       // Glonass
    1395       // -------
    1396       if (iPhase == 1) {
    1397         QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
    1398         while (itGlo.hasNext()) {
    1399           itGlo.next();
    1400           t_satData* satData = itGlo.value();
    1401           addObs(iPhase, iObs, satData, AA, ll, PP);
    1402         }
    1403       }
    1404 
    1405       // Galileo
    1406       // -------
    1407       QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
    1408       while (itGal.hasNext()) {
    1409         itGal.next();
    1410         t_satData* satData = itGal.value();
    1411         addObs(iPhase, iObs, satData, AA, ll, PP);
    1412       }
    1413    
    1414       // Compute Filter Update
    1415       // ---------------------
    1416       QQsav = _QQ;
    1417    
    1418       kalman(AA, ll, PP, _QQ, dx);
    1419    
    1420       vv = ll - AA * dx;
    1421 
    1422       // Print Residuals
    1423       // ---------------
    1424       if (true) {
    1425         ostringstream str;
    1426         str.setf(ios::fixed);
    1427    
     1391          if (numSatNoSlip > 0 && numSatNoSlip < MINOBS) {
     1392            restoreState(epoData);
     1393            _log += "bncModel::update_p: not enough GPS satellites without cycle-slips\n";
     1394            return failure;
     1395          }
     1396        }
     1397       
     1398        // Status Prediction
     1399        // -----------------
     1400        predict(iPhase, epoData);
     1401       
     1402        // Create First-Design Matrix
     1403        // --------------------------
     1404        unsigned nPar = _params.size();
     1405        unsigned nObs = 0;
     1406        if (iPhase == 0) {
     1407          nObs = epoData->sizeGPS() + epoData->sizeGal(); // Glonass code not used
     1408        }
     1409        else {
     1410          nObs = epoData->sizeGPS() + epoData->sizeGal() + epoData->sizeGlo();
     1411        }
     1412       
     1413        Matrix          AA(nObs, nPar);  // first design matrix
     1414        ColumnVector    ll(nObs);        // tems observed-computed
     1415        DiagonalMatrix  PP(nObs); PP = 0.0;
     1416       
     1417        unsigned iObs = 0;
     1418       
     1419        // GPS
     1420        // ---
    14281421        QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
    14291422        while (itGPS.hasNext()) {
    14301423          itGPS.next();
    14311424          t_satData* satData = itGPS.value();
    1432           printRes(iPhase, vv, str, satData);
    1433         }
     1425          QString prn = satData->prn;
     1426          if (findInVector(usedPrns, satData->prn)) {
     1427            addObs(iPhase, iObs, satData, AA, ll, PP);
     1428          }
     1429        }
     1430       
     1431        // Glonass
     1432        // -------
    14341433        if (iPhase == 1) {
    14351434          QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
    … …  
    14371436            itGlo.next();
    14381437            t_satData* satData = itGlo.value();
    1439             printRes(iPhase, vv, str, satData);
    1440           }
    1441         }
     1438            if (findInVector(usedPrns, satData->prn)) {
     1439              addObs(iPhase, iObs, satData, AA, ll, PP);
     1440            }
     1441          }
     1442        }
     1443       
     1444        // Galileo
     1445        // -------
    14421446        QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
    14431447        while (itGal.hasNext()) {
    14441448          itGal.next();
    14451449          t_satData* satData = itGal.value();
    1446           printRes(iPhase, vv, str, satData);
    1447         }
    1448         _log += str.str().c_str();
    1449       }
    1450    
    1451     } while (outlierDetection(iPhase, QQsav, vv, epoData->satDataGPS,
    1452                               epoData->satDataGlo, epoData->satDataGal) != 0);
    1453 
    1454     // Update Parameters
    1455     // -----------------
    1456     QVectorIterator<bncParam*> itPar(_params);
    1457     while (itPar.hasNext()) {
    1458       bncParam* par = itPar.next();
    1459       par->xx += dx(par->index);
    1460     }
    1461   }
    1462 
    1463   return success;
     1450          if (findInVector(usedPrns, satData->prn)) {
     1451            addObs(iPhase, iObs, satData, AA, ll, PP);
     1452          }
     1453        }
     1454       
     1455        // Compute Filter Update
     1456        // ---------------------
     1457        kalman(AA, ll, PP, _QQ, dx);
     1458       
     1459        ColumnVector vv = ll - AA * dx;
     1460       
     1461        // Print Residuals
     1462        // ---------------
     1463        if (true) {
     1464          ostringstream str;
     1465          str.setf(ios::fixed);
     1466       
     1467          QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
     1468          while (itGPS.hasNext()) {
     1469            itGPS.next();
     1470            t_satData* satData = itGPS.value();
     1471            printRes(iPhase, vv, str, satData);
     1472          }
     1473          if (iPhase == 1) {
     1474            QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
     1475            while (itGlo.hasNext()) {
     1476              itGlo.next();
     1477              t_satData* satData = itGlo.value();
     1478              printRes(iPhase, vv, str, satData);
     1479            }
     1480          }
     1481          QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
     1482          while (itGal.hasNext()) {
     1483            itGal.next();
     1484            t_satData* satData = itGal.value();
     1485            printRes(iPhase, vv, str, satData);
     1486          }
     1487          _log += str.str().c_str();
     1488        }
     1489       
     1490        // Check the residuals
     1491        // -------------------
     1492        outlierDetected = outlierDetection(iPhase, vv,
     1493                                           epoData->satDataGPS,
     1494                                           epoData->satDataGlo,
     1495                                           epoData->satDataGal);
     1496        if (outlierDetected) {
     1497          restoreState(epoData);
     1498          break;
     1499        }
     1500
     1501      } // for (int iPhase = 0; iPhase <= (_usePhase ? 1 : 0); iPhase++)
     1502
     1503      // Update Parameters
     1504      // -----------------
     1505      if (!outlierDetected) {
     1506        QVectorIterator<bncParam*> itPar(_params);
     1507        while (itPar.hasNext()) {
     1508          bncParam* par = itPar.next();
     1509          par->xx += dx(par->index);
     1510        }
     1511        return success;
     1512      }
     1513
     1514    } while ( next_combination(allPrns.begin(), allPrns.end(),
     1515                                 usedPrns.begin(), usedPrns.end()) );
     1516
     1517  } // for (unsigned nUsed = allPrns.size(); nUsed >= MINOBS; nUsed--)
     1518
     1519  return failure;
    14641520}
    14651521
    14661522// Remeber Original State Vector and Variance-Covariance Matrix
    14671523////////////////////////////////////////////////////////////////////////////
    1468 void bncModel::rememberState() {
     1524void bncModel::rememberState(t_epoData* epoData) {
    14691525
    14701526  _QQ_sav = _QQ;
    … …  
    14821538    _params_sav.push_back(new bncParam(*par));
    14831539  }
     1540
     1541  delete _epoData_sav;
     1542  _epoData_sav = new t_epoData(*epoData);
    14841543}
    14851544
    14861545// Restore Original State Vector and Variance-Covariance Matrix
    14871546////////////////////////////////////////////////////////////////////////////
    1488 void bncModel::restoreState() {
     1547void bncModel::restoreState(t_epoData* epoData) {
    14891548
    14901549  _QQ = _QQ_sav;
    … …  
    15021561    _params.push_back(new bncParam(*par));
    15031562  }
    1504 }
    1505 
     1563
     1564  delete epoData;
     1565  epoData = new t_epoData(*_epoData_sav);
     1566}
     1567
     1568//
     1569////////////////////////////////////////////////////////////////////////////
     1570void bncModel::getAllPrns(const t_epoData* epoData,
     1571                          std::vector<QString>* allPrns) {
     1572
     1573  QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
     1574  while (itGPS.hasNext()) {
     1575    itGPS.next();
     1576    t_satData* satData = itGPS.value();
     1577    allPrns->push_back(satData->prn);
     1578  }
     1579 
     1580  // Glonass
     1581  // -------
     1582  QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
     1583  while (itGlo.hasNext()) {
     1584    itGlo.next();
     1585    t_satData* satData = itGlo.value();
     1586    allPrns->push_back(satData->prn);
     1587  }
     1588 
     1589  // Galileo
     1590  // -------
     1591  QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
     1592  while (itGal.hasNext()) {
     1593    itGal.next();
     1594    t_satData* satData = itGal.value();
     1595    allPrns->push_back(satData->prn);
     1596  }
     1597}
     1598

  • trunk/BNC/bncmodel.h

    r3330 r3375  
    2929#include <QtNetwork>
    3030#include <newmat.h>
     31#include <vector>
    3132
    3233#include "bncconst.h"
    … …  
    109110  void   predict(int iPhase, t_epoData* epoData);
    110111  t_irc  update_p(t_epoData* epoData);
    111   int    outlierDetection(int iPhase, const SymmetricMatrix& QQsav,
    112                           const ColumnVector& vv,
     112  bool   outlierDetection(int iPhase, const ColumnVector& vv,
    113113                          QMap<QString, t_satData*>& satDataGPS,
    114114                          QMap<QString, t_satData*>& satDataGlo,
    … …  
    119119                const ColumnVector& rRec);
    120120
     121  void getAllPrns(const t_epoData* epoData, std::vector<QString>* allPrns);
     122
    121123  bncTime  _startTime;
    122124
    123   void rememberState();
    124   void restoreState();
     125  void rememberState(t_epoData* epoData);
     126  void restoreState(t_epoData* epoData);
    125127
    126128  class pppPos {
    … …  
    142144  QVector<bncParam*>    _params_sav;
    143145  SymmetricMatrix       _QQ_sav;
     146  t_epoData*            _epoData_sav;
    144147  ColumnVector          _xcBanc;
    145148  ColumnVector          _ellBanc;

  • trunk/BNC/bncpppclient.h

    r3319 r3375  
    6565 public:
    6666  t_epoData() {}
     67
     68  t_epoData(const t_epoData& ed) {
     69    tt = ed.tt;
     70    QMapIterator<QString, t_satData*> itGPS(ed.satDataGPS);
     71    while (itGPS.hasNext()) {
     72      itGPS.next();
     73      satDataGPS[itGPS.key()] = new t_satData(*itGPS.value());
     74    }
     75    QMapIterator<QString, t_satData*> itGlo(ed.satDataGlo);
     76    while (itGlo.hasNext()) {
     77      itGlo.next();
     78      satDataGlo[itGPS.key()] = new t_satData(*itGlo.value());
     79    }
     80    QMapIterator<QString, t_satData*> itGal(ed.satDataGal);
     81    while (itGal.hasNext()) {
     82      itGal.next();
     83      satDataGPS[itGal.key()] = new t_satData(*itGal.value());
     84    }
     85  }
     86
    6787  ~t_epoData() {
    6888    QMapIterator<QString, t_satData*> itGPS(satDataGPS);
    … …  
    87107  unsigned sizeAll() const {return satDataGPS.size() + satDataGlo.size() +
    88108                                   satDataGal.size();}
    89   bncTime                    tt;
     109  bncTime                   tt;
    90110  QMap<QString, t_satData*> satDataGPS;
    91111  QMap<QString, t_satData*> satDataGlo;
Note: See TracChangeset for help on using the changeset viewer.