Changeset 3202 in ntrip

Timestamp:

Mar 30, 2011, 4:15:54 PM (14 years ago)

Author:

mervart

Message:

 

File:

• trunk/BNC/combination/bnccomb.cpp (modified) (2 diffs)

trunk/BNC/combination/bnccomb.cpp

@@ -299 +299 @@
   else {
     newEpoch->corr[newCorr->prn] = newCorr;
+  }
+}
+
+// Change the correction so that it refers to last received ephemeris 
+////////////////////////////////////////////////////////////////////////////
+void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) {
+
+  ColumnVector oldXC(4);
+  ColumnVector oldVV(3);
+  corr->eph->position(corr->tt.gpsw(), corr->tt.gpssec(), 
+                      oldXC.data(), oldVV.data());
+
+  ColumnVector newXC(4);
+  ColumnVector newVV(3);
+  lastEph->position(corr->tt.gpsw(), corr->tt.gpssec(), 
+                    newXC.data(), newVV.data());
+
+  ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
+  ColumnVector dV = newVV           - oldVV;
+  double       dC = newXC(4)        - oldXC(4);
+
+  ColumnVector dRAO(3);
+  XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
+
+  ColumnVector dDotRAO(3);
+  XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
+
+  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);
+
+  emit newMessage(msg.toAscii(), false);
+
+  corr->iod     = lastEph->IOD();
+  corr->eph     = lastEph;
+  corr->rao    += dRAO;
+  corr->dotRao += dDotRAO;
+  corr->dClk   -= dC;
+}
+
+// Process Epochs
+////////////////////////////////////////////////////////////////////////////
+void bncComb::processEpochs(const QList<cmbEpoch*>& epochs) {
+
+  _log.clear();
+
+  QTextStream out(&_log, QIODevice::WriteOnly);
+
+  out <<                   "Combination:" << endl 
+      << "------------------------------" << endl;
+
+  // Predict Parameters Values, Add White Noise
+  // ------------------------------------------
+  for (int iPar = 1; iPar <= _params.size(); iPar++) {
+    cmbParam* pp = _params[iPar-1];
+    if (pp->sig_P != 0.0) {
+      pp->xx = 0.0;
+      for (int jj = 1; jj <= _params.size(); jj++) {
+        _QQ(iPar, jj) = 0.0;
+      }
+      _QQ(iPar,iPar) = pp->sig_P * pp->sig_P;
+    }
+  }
+
+  bncTime                resTime = epochs.first()->time;
+  QMap<QString, t_corr*> resCorr;
+
+  int nPar = _params.size();
+  int nObs = 0;  
+
+  ColumnVector x0(nPar);
+  for (int iPar = 1; iPar <= _params.size(); iPar++) {
+    cmbParam* pp = _params[iPar-1];
+    x0(iPar) = pp->xx;
+  }
+
+  // Count Observations
+  // ------------------
+  QListIterator<cmbEpoch*> itEpo(epochs);
+  while (itEpo.hasNext()) {
+    cmbEpoch* epo = itEpo.next();
+    QMutableMapIterator<QString, t_corr*> itCorr(epo->corr);
+    while (itCorr.hasNext()) {
+      itCorr.next();
+      t_corr* corr = itCorr.value();
+
+      // Switch to last ephemeris
+      // ------------------------
+      t_eph* lastEph = _eph[corr->prn]->last;
+      if (lastEph == corr->eph) {      
+        ++nObs;
+      }
+      else {
+        if (corr->eph == _eph[corr->prn]->prev) {
+          switchToLastEph(lastEph, corr);
+          ++nObs;
+        }
+        else {
+          itCorr.remove();
+        }
+      }
+    }
+  }
+
+  if (nObs > 0) {
+    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;
+    QListIterator<cmbEpoch*> itEpo(epochs);
+    while (itEpo.hasNext()) {
+      cmbEpoch* epo = itEpo.next();
+      QMapIterator<QString, t_corr*> itCorr(epo->corr);
+    
+      while (itCorr.hasNext()) {
+        itCorr.next();
+        ++iObs;
+        t_corr* corr = itCorr.value();
+
+        if (epo->acName == _masterAC) {
+          resCorr[corr->prn] = new t_corr(*corr);
+        }
+
+        for (int iPar = 1; iPar <= _params.size(); iPar++) {
+          cmbParam* pp = _params[iPar-1];
+          AA(iObs, iPar) = pp->partial(epo->acName, corr);
+        }
+
+        ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
+      }
+
+      delete epo;
+    }
+
+    // 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::clk &&
+          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::Sat_offset && pp->prn == prn) {
+            AA(nObs+iCond, iPar) = 1.0;
+          }
+        }
+      }
+    }
+
+    const double MAXRES = 999.10;  // TODO: make it an option
+
+    ColumnVector dx;
+    SymmetricMatrix QQ_sav = _QQ;
+
+    for (int ii = 1; ii < 10; ii++) {
+      bncModel::kalman(AA, ll, PP, _QQ, dx);
+      ColumnVector vv = ll - AA * dx;
+
+      int    maxResIndex;
+      double maxRes = vv.maximum_absolute_value1(maxResIndex);   
+      out.setRealNumberNotation(QTextStream::FixedNotation);
+      out.setRealNumberPrecision(3);  
+      out << "Maximum Residuum " << maxRes << " (index " << maxResIndex << ")\n";
+
+      if (maxRes > MAXRES) {
+        out << "Outlier Detected" << endl;
+        _QQ = QQ_sav;
+        AA.Row(maxResIndex) = 0.0;
+        ll.Row(maxResIndex) = 0.0;
+      }
+      else {
+        break;
+      }
+    }
+
+    for (int iPar = 1; iPar <= _params.size(); iPar++) {
+      cmbParam* pp = _params[iPar-1];
+      pp->xx += dx(iPar);
+      if (pp->type == cmbParam::clk) {
+        if (resCorr.find(pp->prn) != resCorr.end()) {
+          resCorr[pp->prn]->dClk = pp->xx / t_CST::c;
+        }
+      }
+      out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
+      out.setRealNumberNotation(QTextStream::FixedNotation);
+      out.setFieldWidth(8);
+      out.setRealNumberPrecision(4);
+      out << pp->toString() << " "
+          << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
+    }
+  }
+
+  printResults(out, resTime, resCorr);
+  dumpResults(resTime, resCorr);
+
+  emit newMessage(_log, false);
+}
+
+// Print results
+////////////////////////////////////////////////////////////////////////////
+void bncComb::printResults(QTextStream& out, const bncTime& resTime,
+                           const QMap<QString, t_corr*>& resCorr) {
+
+  QMapIterator<QString, t_corr*> it(resCorr);
+  while (it.hasNext()) {
+    it.next();
+    t_corr* corr = it.value();
+    const t_eph* eph = corr->eph;
+    if (eph) {
+      double xx, yy, zz, cc;
+      eph->position(resTime.gpsw(), resTime.gpssec(), xx, yy, zz, cc);
+
+      out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
+      out.setFieldWidth(3);
+      out << "Full Clock " << corr->prn << " " << corr->iod << " ";
+      out.setFieldWidth(14);
+      out << (cc + corr->dClk) * t_CST::c << endl;
+    }
+    else {
+      out << "bncComb::printResuls bug" << endl;
+    }
   }
 }
@@ -363 +601 @@
   }
 }
-
-// Change the correction so that it refers to last received ephemeris 
-////////////////////////////////////////////////////////////////////////////
-void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) {
-
-  ColumnVector oldXC(4);
-  ColumnVector oldVV(3);
-  corr->eph->position(corr->tt.gpsw(), corr->tt.gpssec(), 
-                      oldXC.data(), oldVV.data());
-
-  ColumnVector newXC(4);
-  ColumnVector newVV(3);
-  lastEph->position(corr->tt.gpsw(), corr->tt.gpssec(), 
-                    newXC.data(), newVV.data());
-
-  ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
-  ColumnVector dV = newVV           - oldVV;
-  double       dC = newXC(4)        - oldXC(4);
-
-  ColumnVector dRAO(3);
-  XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
-
-  ColumnVector dDotRAO(3);
-  XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
-
-  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);
-
-  emit newMessage(msg.toAscii(), false);
-
-  corr->iod     = lastEph->IOD();
-  corr->eph     = lastEph;
-  corr->rao    += dRAO;
-  corr->dotRao += dDotRAO;
-  corr->dClk   -= dC;
-}
-
-// Process Epochs
-////////////////////////////////////////////////////////////////////////////
-void bncComb::processEpochs(const QList<cmbEpoch*>& epochs) {
-
-  _log.clear();
-
-  QTextStream out(&_log, QIODevice::WriteOnly);
-
-  out <<                   "Combination:" << endl 
-      << "------------------------------" << endl;
-
-  // Predict Parameters Values, Add White Noise
-  // ------------------------------------------
-  for (int iPar = 1; iPar <= _params.size(); iPar++) {
-    cmbParam* pp = _params[iPar-1];
-    if (pp->sig_P != 0.0) {
-      pp->xx = 0.0;
-      for (int jj = 1; jj <= _params.size(); jj++) {
-        _QQ(iPar, jj) = 0.0;
-      }
-      _QQ(iPar,iPar) = pp->sig_P * pp->sig_P;
-    }
-  }
-
-  bncTime                resTime = epochs.first()->time;
-  QMap<QString, t_corr*> resCorr;
-
-  int nPar = _params.size();
-  int nObs = 0;  
-
-  ColumnVector x0(nPar);
-  for (int iPar = 1; iPar <= _params.size(); iPar++) {
-    cmbParam* pp = _params[iPar-1];
-    x0(iPar) = pp->xx;
-  }
-
-  // Count Observations
-  // ------------------
-  QListIterator<cmbEpoch*> itEpo(epochs);
-  while (itEpo.hasNext()) {
-    cmbEpoch* epo = itEpo.next();
-    QMutableMapIterator<QString, t_corr*> itCorr(epo->corr);
-    while (itCorr.hasNext()) {
-      itCorr.next();
-      t_corr* corr = itCorr.value();
-
-      // Switch to last ephemeris
-      // ------------------------
-      t_eph* lastEph = _eph[corr->prn]->last;
-      if (lastEph == corr->eph) {      
-        ++nObs;
-      }
-      else {
-        if (corr->eph == _eph[corr->prn]->prev) {
-          switchToLastEph(lastEph, corr);
-          ++nObs;
-        }
-        else {
-          itCorr.remove();
-        }
-      }
-    }
-  }
-
-  if (nObs > 0) {
-    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;
-    QListIterator<cmbEpoch*> itEpo(epochs);
-    while (itEpo.hasNext()) {
-      cmbEpoch* epo = itEpo.next();
-      QMapIterator<QString, t_corr*> itCorr(epo->corr);
-    
-      while (itCorr.hasNext()) {
-        itCorr.next();
-        ++iObs;
-        t_corr* corr = itCorr.value();
-
-        if (epo->acName == _masterAC) {
-          resCorr[corr->prn] = new t_corr(*corr);
-        }
-
-        for (int iPar = 1; iPar <= _params.size(); iPar++) {
-          cmbParam* pp = _params[iPar-1];
-          AA(iObs, iPar) = pp->partial(epo->acName, corr);
-        }
-
-        ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
-      }
-
-      delete epo;
-    }
-
-    // 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::clk &&
-          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::Sat_offset && pp->prn == prn) {
-            AA(nObs+iCond, iPar) = 1.0;
-          }
-        }
-      }
-    }
-
-    const double MAXRES = 999.10;  // TODO: make it an option
-
-    ColumnVector dx;
-    SymmetricMatrix QQ_sav = _QQ;
-
-    for (int ii = 1; ii < 10; ii++) {
-      bncModel::kalman(AA, ll, PP, _QQ, dx);
-      ColumnVector vv = ll - AA * dx;
-
-      int    maxResIndex;
-      double maxRes = vv.maximum_absolute_value1(maxResIndex);   
-      out.setRealNumberNotation(QTextStream::FixedNotation);
-      out.setRealNumberPrecision(3);  
-      out << "Maximum Residuum " << maxRes << " (index " << maxResIndex << ")\n";
-
-      if (maxRes > MAXRES) {
-        out << "Outlier Detected" << endl;
-        _QQ = QQ_sav;
-        AA.Row(maxResIndex) = 0.0;
-        ll.Row(maxResIndex) = 0.0;
-      }
-      else {
-        break;
-      }
-    }
-
-    for (int iPar = 1; iPar <= _params.size(); iPar++) {
-      cmbParam* pp = _params[iPar-1];
-      pp->xx += dx(iPar);
-      if (pp->type == cmbParam::clk) {
-        if (resCorr.find(pp->prn) != resCorr.end()) {
-          resCorr[pp->prn]->dClk = pp->xx / t_CST::c;
-        }
-      }
-      out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
-      out.setRealNumberNotation(QTextStream::FixedNotation);
-      out.setFieldWidth(8);
-      out.setRealNumberPrecision(4);
-      out << pp->toString() << " "
-          << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
-    }
-  }
-
-  printResults(out, resTime, resCorr);
-  dumpResults(resTime, resCorr);
-
-  emit newMessage(_log, false);
-}
-
-// Print results
-////////////////////////////////////////////////////////////////////////////
-void bncComb::printResults(QTextStream& out, const bncTime& resTime,
-                           const QMap<QString, t_corr*>& resCorr) {
-
-  QMapIterator<QString, t_corr*> it(resCorr);
-  while (it.hasNext()) {
-    it.next();
-    t_corr* corr = it.value();
-    const t_eph* eph = corr->eph;
-    if (eph) {
-      double xx, yy, zz, cc;
-      eph->position(resTime.gpsw(), resTime.gpssec(), xx, yy, zz, cc);
-
-      out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
-      out.setFieldWidth(3);
-      out << "Full Clock " << corr->prn << " " << corr->iod << " ";
-      out.setFieldWidth(14);
-      out << (cc + corr->dClk) * t_CST::c << endl;
-    }
-    else {
-      out << "bncComb::printResuls bug" << endl;
-    }
-  }
-}