Changeset 3455 in ntrip


Ignore:
Timestamp:
Sep 23, 2011, 10:21:38 AM (13 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC/combination
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/combination/bnccomb.cpp

    r3454 r3455  
    3030#include "bnctides.h"
    3131
     32const int moduloTime = 10;
     33
     34const double sig0_offAC    = 1000.0;
     35const double sig0_offACSat =  100.0;
     36const double sigP_offACSat =    0.0;
     37const double sig0_clkSat   =  100.0;
     38
     39const double sigObs        =   0.05;
     40
     41const int MAXPRN_GPS = 32;
     42
    3243using namespace std;
    33 
    34 const int MAXPRN_GPS = 32;
    3544
    3645// Constructor
    … …  
    4453  prn    = prn_;
    4554  xx     = 0.0;
     55  eph    = 0;
    4656
    4757  if      (type == offAC) {
    48     sig_0 = 1000.0;
    49     sig_P = 1000.0;
     58    epoSpec = true;
     59    sig0    = sig0_offAC;
     60    sigP    = sig0;
    5061  }
    5162  else if (type == offACSat) {
    52     sig_0 = 100.0;
    53     sig_P =   0.0;
     63    epoSpec = false;
     64    sig0    = sig0_offACSat;
     65    sigP    = sigP_offACSat;
    5466  }
    5567  else if (type == clkSat) {
    56     sig_0 = 100.0;
    57     sig_P = 100.0;
     68    epoSpec = true;
     69    sig0    = sig0_clkSat;
     70    sigP    = sig0;
    5871  }
    5972}
    … …  
    113126
    114127  QStringList combineStreams = settings.value("combineStreams").toStringList();
     128
     129  _masterMissingEpochs = 0;
    115130
    116131  if (combineStreams.size() >= 1 && !combineStreams[0].isEmpty()) {
    … …  
    159174  for (int iPar = 1; iPar <= _params.size(); iPar++) {
    160175    cmbParam* pp = _params[iPar-1];
    161     _QQ(iPar,iPar) = pp->sig_0 * pp->sig_0;
     176    _QQ(iPar,iPar) = pp->sig0 * pp->sig0;
    162177  }
    163178
    … …  
    236251  // Check Modulo Time
    237252  // -----------------
    238   const int moduloTime = 10;
    239253  if (int(newCorr->tt.gpssec()) % moduloTime != 0.0) {
    240254    delete newCorr;
    … …  
    331345  XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
    332346
    333   QString msg = "switch " + corr->prn
     347  QString msg = "switch corr " + corr->prn
    334348    + QString(" %1 -> %2 %3").arg(corr->iod,3)
    335349    .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
    … …  
    349363
    350364  int nPar = _params.size();
    351   int nObs = corrs().size();
    352 
    353   if (nObs == 0) {
    354     return;
    355   }
    356365
    357366  _log.clear();
    … …  
    364373  // Observation Statistics
    365374  // ----------------------
    366   bool    masterPresent = false;
    367 
     375  bool masterPresent = false;
    368376  QListIterator<cmbAC*> icAC(_ACs);
    369377  while (icAC.hasNext()) {
    … …  
    386394  // --------------------------------------------
    387395  if (!masterPresent) {
    388     QListIterator<cmbAC*> icAC(_ACs);
    389     while (icAC.hasNext()) {
    390       cmbAC* AC = icAC.next();
    391       if (AC->numObs > 0) {
    392         out << "Switching Master AC "
    393             << _masterOrbitAC.toAscii().data() << " --> "
    394             << AC->name.toAscii().data()   << " "
    395             << _resTime.datestr().c_str()    << " "
    396             << _resTime.timestr().c_str()    << endl;
    397         _masterOrbitAC = AC->name;
    398         break;
    399       }
    400     }
    401   }
    402 
    403   // Check Number of Observations per Satellite
    404   // ------------------------------------------
    405   QMap<QString, int> numObs;
    406   for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) {
    407     QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
    408     numObs[prn] = 0;
    409     QVectorIterator<cmbCorr*> itCorr(corrs());
    410     while (itCorr.hasNext()) {
    411       cmbCorr* corr = itCorr.next();
    412       if (corr->prn == prn) {
    413         ++numObs[prn];
     396    ++_masterMissingEpochs;
     397    if (_masterMissingEpochs < 2) {
     398      out << "Missing Master, Epoch skipped\n";
     399      _buffer.remove(_resTime);
     400      emit newMessage(_log, false);
     401      return;
     402    }
     403    else {
     404      _masterMissingEpochs = 0;
     405      QListIterator<cmbAC*> icAC(_ACs);
     406      while (icAC.hasNext()) {
     407        cmbAC* AC = icAC.next();
     408        if (AC->numObs > 0) {
     409          out << "Switching Master AC "
     410              << _masterOrbitAC.toAscii().data() << " --> "
     411              << AC->name.toAscii().data()   << " "
     412              << _resTime.datestr().c_str()    << " "
     413              << _resTime.timestr().c_str()    << endl;
     414          _masterOrbitAC = AC->name;
     415          break;
     416        }
    414417      }
    415418    }
    … …  
    420423  ColumnVector x0(nPar);
    421424  for (int iPar = 1; iPar <= _params.size(); iPar++) {
    422     cmbParam* pp = _params[iPar-1];
    423     if (pp->sig_P != 0.0) {
     425    cmbParam* pp  = _params[iPar-1];
     426    QString   prn = pp->prn;
     427    if (!prn.isEmpty() && _eph.find(prn) != _eph.end()) {
     428      switchToLastEph(_eph[prn]->last, pp);
     429    }
     430    if (pp->epoSpec) {
    424431      pp->xx = 0.0;
    425       for (int jj = 1; jj <= _params.size(); jj++) {
    426         _QQ(iPar, jj) = 0.0;
    427       }
    428       _QQ(iPar,iPar) = pp->sig_P * pp->sig_P;
     432      _QQ.Row(iPar)    = 0.0;
     433      _QQ.Column(iPar) = 0.0;
     434      _QQ(iPar,iPar) = pp->sig0 * pp->sig0;
     435    }
     436    else {
     437      _QQ(iPar,iPar) += pp->sigP * pp->sigP;
    429438    }
    430439    x0(iPar) = pp->xx;
    431440  }
    432441
    433   // Create First Design Matrix and Vector of Measurements
    434   // -----------------------------------------------------
    435   const double Pl = 1.0 / (0.05 * 0.05);
    436 
    437   const int nCon = (_firstReg == false) ? 1 + MAXPRN_GPS : 1;
    438   Matrix         AA(nObs+nCon, nPar);  AA = 0.0;
    439   ColumnVector   ll(nObs+nCon);        ll = 0.0;
    440   DiagonalMatrix PP(nObs+nCon);        PP = Pl;
    441 
    442   int iObs = 0;
    443 
     442  SymmetricMatrix QQ_sav = _QQ;
     443
     444  ColumnVector dx;
    444445  QMap<QString, t_corr*> resCorr;
    445 
    446   QVectorIterator<cmbCorr*> itCorr(corrs());
    447   while (itCorr.hasNext()) {
    448     cmbCorr* corr = itCorr.next();
    449     QString  prn  = corr->prn;
    450     switchToLastEph(_eph[prn]->last, corr);
    451     ++iObs;
    452 
    453     if (corr->acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
    454       resCorr[prn] = new t_corr(*corr);
    455     }
    456 
    457     for (int iPar = 1; iPar <= _params.size(); iPar++) {
    458       cmbParam* pp = _params[iPar-1];
    459       AA(iObs, iPar) = pp->partial(corr->acName, prn);
    460     }
    461 
    462     ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
    463   }
    464 
    465   // Regularization
    466   // --------------
    467   const double Ph = 1.e6;
    468   int iCond = 1;
    469   PP(nObs+iCond)          = Ph;
    470   for (int iPar = 1; iPar <= _params.size(); iPar++) {
    471     cmbParam* pp = _params[iPar-1];
    472     if (pp->type == cmbParam::clkSat &&
    473         AA.Column(iPar).maximum_absolute_value() > 0.0) {
    474       AA(nObs+iCond, iPar) = 1.0;
    475     }
    476   }
    477 
    478   if (!_firstReg) {
    479     _firstReg = true;
    480     for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) {
    481       ++iCond;
    482       QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
    483       PP(nObs+1+iGps)       = Ph;
    484       for (int iPar = 1; iPar <= _params.size(); iPar++) {
    485         cmbParam* pp = _params[iPar-1];
    486         if (pp->type == cmbParam::offACSat && pp->prn == prn) {
    487           AA(nObs+iCond, iPar) = 1.0;
    488         }
    489       }
    490     }
    491   }
    492 
    493   ColumnVector dx;
    494   SymmetricMatrix QQ_sav = _QQ;
    495 
     446   
    496447  // Update and outlier detection loop
    497448  // ---------------------------------
    498   for (int ii = 1; ii < 10; ii++) {
     449  while (true) {
     450
     451    Matrix         AA;
     452    ColumnVector   ll;
     453    DiagonalMatrix PP;
     454
     455    if (createAmat(AA, ll, PP, x0, resCorr) != success) {
     456      _buffer.remove(_resTime);
     457      emit newMessage(_log, false);
     458      return;
     459    }
     460
    499461    bncModel::kalman(AA, ll, PP, _QQ, dx);
    500462    ColumnVector vv = ll - AA * dx;
    … …  
    516478          QQ_sav.Row(iPar)    = 0.0;
    517479          QQ_sav.Column(iPar) = 0.0;
    518           QQ_sav(iPar,iPar)   = pp->sig_0 * pp->sig_0;
     480          QQ_sav(iPar,iPar)   = pp->sig0 * pp->sig0;
    519481        }
    520482      }
    … …  
    522484      out << "  Outlier" << endl;
    523485      _QQ = QQ_sav;
    524       AA.Row(maxResIndex) = 0.0;
    525       ll.Row(maxResIndex) = 0.0;
     486      corrs().remove(maxResIndex-1);
    526487    }
    527488    else {
    … …  
    530491    }
    531492  }
     493
     494  _firstReg = true;
    532495
    533496  // Update Parameter Values
    … …  
    718681  }
    719682}
     683
     684// Create First Design Matrix and Vector of Measurements
     685////////////////////////////////////////////////////////////////////////////
     686t_irc bncComb::createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
     687                          const ColumnVector& x0,
     688                          QMap<QString, t_corr*>& resCorr) {
     689
     690  unsigned nPar = _params.size();
     691  unsigned nObs = corrs().size();
     692
     693  if (nObs == 0) {
     694    return failure;
     695  }
     696
     697  const int nCon = (_firstReg == false) ? 1 + MAXPRN_GPS : 1;
     698
     699  AA.ReSize(nObs+nCon, nPar);  AA = 0.0;
     700  ll.ReSize(nObs+nCon);        ll = 0.0;
     701  PP.ReSize(nObs+nCon);        PP = 1.0 / (sigObs * sigObs);
     702
     703  int iObs = 0;
     704
     705  QVectorIterator<cmbCorr*> itCorr(corrs());
     706  while (itCorr.hasNext()) {
     707    cmbCorr* corr = itCorr.next();
     708    QString  prn  = corr->prn;
     709    switchToLastEph(_eph[prn]->last, corr);
     710    ++iObs;
     711
     712    if (corr->acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
     713      resCorr[prn] = new t_corr(*corr);
     714    }
     715
     716    for (int iPar = 1; iPar <= _params.size(); iPar++) {
     717      cmbParam* pp = _params[iPar-1];
     718      AA(iObs, iPar) = pp->partial(corr->acName, prn);
     719    }
     720
     721    ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
     722  }
     723
     724  // Regularization
     725  // --------------
     726  const double Ph = 1.e6;
     727  int iCond = 1;
     728  PP(nObs+iCond) = Ph;
     729  for (int iPar = 1; iPar <= _params.size(); iPar++) {
     730    cmbParam* pp = _params[iPar-1];
     731    if (pp->type == cmbParam::clkSat &&
     732        AA.Column(iPar).maximum_absolute_value() > 0.0) {
     733      AA(nObs+iCond, iPar) = 1.0;
     734    }
     735  }
     736
     737  if (!_firstReg) {
     738    for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) {
     739      ++iCond;
     740      QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
     741      PP(nObs+1+iGps) = Ph;
     742      for (int iPar = 1; iPar <= _params.size(); iPar++) {
     743        cmbParam* pp = _params[iPar-1];
     744        if (pp->type == cmbParam::offACSat && pp->prn == prn) {
     745          AA(nObs+iCond, iPar) = 1.0;
     746        }
     747      }
     748    }
     749  }
     750
     751  return success;
     752}
     753
     754// Change the parameter so that it refers to last received ephemeris
     755////////////////////////////////////////////////////////////////////////////
     756void bncComb::switchToLastEph(const t_eph* lastEph, cmbParam* pp) {
     757
     758  if (pp->type != cmbParam::clkSat) {
     759    return;
     760  }
     761
     762  if (pp->eph == 0) {
     763    pp->eph = lastEph;
     764    return;
     765  }
     766
     767  if (pp->eph == lastEph) {
     768    return;
     769  }
     770
     771  ColumnVector oldXC(4);
     772  ColumnVector oldVV(3);
     773  pp->eph->position(_resTime.gpsw(), _resTime.gpssec(),
     774                      oldXC.data(), oldVV.data());
     775
     776  ColumnVector newXC(4);
     777  ColumnVector newVV(3);
     778  lastEph->position(_resTime.gpsw(), _resTime.gpssec(),
     779                    newXC.data(), newVV.data());
     780
     781  double dC = newXC(4) - oldXC(4);
     782
     783  QString msg = "switch param " + pp->prn
     784    + QString(" %1 -> %2 %3").arg(pp->eph->IOD(),3)
     785    .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
     786
     787  emit newMessage(msg.toAscii(), false);
     788
     789  pp->eph = lastEph;
     790  pp->xx  -= dC * t_CST::c;
     791}
     792

  • trunk/BNC/combination/bnccomb.h

    r3453 r3455  
    2323  QString prn;
    2424  double  xx;
    25   double  sig_0;
    26   double  sig_P;
     25  double  sig0;
     26  double  sigP;
     27  bool    epoSpec;
     28  const t_eph* eph;
    2729};
    2830
    … …  
    7274
    7375  void processEpoch();
     76  t_irc createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
     77                   const ColumnVector& x0, QMap<QString, t_corr*>& resCorr);
    7478  void dumpResults(const QMap<QString, t_corr*>& resCorr);
    7579  void printResults(QTextStream& out, const QMap<QString, t_corr*>& resCorr);
    7680  void switchToLastEph(const t_eph* lastEph, t_corr* corr);
     81  void switchToLastEph(const t_eph* lastEph, cmbParam* pp);
    7782
    7883  QVector<cmbCorr*>& corrs() {return _buffer[_resTime].corrs;}
    … …  
    8489  bncRtnetDecoder*        _rtnetDecoder;
    8590  SymmetricMatrix         _QQ;
    86   bool                    _firstReg;
    8791  QByteArray              _log;
    8892  bncAntex*               _antex;
    8993  double                  _MAXRES;
    9094  QString                 _masterOrbitAC;
     95  unsigned                _masterMissingEpochs;
     96  bool                    _firstReg;
    9197};
    9298
Note: See TracChangeset for help on using the changeset viewer.