Changeset 8956 in ntrip for trunk/BNC/src/PPP/pppFilter.cpp

Timestamp:

Jun 23, 2020, 11:58:46 AM (4 years ago)

Author:

stuerze

Message:

update regarding PPP

File:

• trunk/BNC/src/PPP/pppFilter.cpp (modified) (22 diffs)

trunk/BNC/src/PPP/pppFilter.cpp

@@ -83 +83 @@
   // Set Parameters
   // --------------
-  _parlist->set(_epoTime, allObs);
+  _parlist->set(_epoTime, allObs, _obsPool->getRefSatMap());
   const vector<t_pppParam*>& params = _parlist->params();
 
@@ -90 +90 @@
   ColumnVector    xFltOld = _xFlt;
   SymmetricMatrix QFltOld = _QFlt;
+
   _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
   _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
@@ -114 +115 @@
   predictCovCrdPart(QFltOld);
 
+  // Init Datum Trafo
+  // ----------------------------------------
+  if ((OPT->_obsModelType == OPT->DCMcodeBias ||
+       OPT->_obsModelType == OPT->DCMphaseBias) &&
+      (_numEpoProcessing == 1)) {
+    _numAllUsedLCs = 0;
+    for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
+      char system = OPT->systems()[iSys];
+      _numAllUsedLCs += OPT->LCs(system).size();
+      if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {
+        _numAllUsedLCs -= 1;  // GIM not used
+      }
+    }
+    int maxObs = allObs.size() * _numAllUsedLCs;
+    _datumTrafo->initAA(maxObs, _parlist->nPar());
+  }
 
   // Pre-Process Satellite Systems separately
@@ -121 +138 @@
       OPT->_obsModelType == OPT->DCMphaseBias) {
     preProcessing = true;
-    _numAllUsedLCs = 0;
     for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
+      (iSys) ? _datumTrafo->setFirstSystem(false) : _datumTrafo->setFirstSystem(true);
       char system = OPT->systems()[iSys];
-      _numAllUsedLCs += OPT->LCs(system).size();
-      if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {
-        _numAllUsedLCs -= 1;  // GIM not used
-      }
       if (OPT->_refSatRequired) {
         _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
+        if (_obsPool->hasHistoricalRefSat(_refPrn)) {
+          return failure;
+        }
       }
       vector<t_pppSatObs*> obsVector;
@@ -142 +158 @@
       }
     }
-  }
-
-  if (_numEpoProcessing == 1) {
-    int maxObs = allObs.size() * _numAllUsedLCs;
-    _datumTrafo->initAA(maxObs, _parlist->nPar());
+    // refSat change required?
+    // -----------------------
+    if (_obsPool->refSatChangeRequired()) {
+      _xFlt = xFltOld;
+      _QFlt = QFltOld;
+      return success;
+    }
+    else if (!_obsPool->refSatChangeRequired() &&
+             _numEpoProcessing > 1) {
+      datumTransformation(xFltOld, QFltOld);
+    }
   }
 
@@ -154 +176 @@
   for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
     char system = OPT->systems()[iSys];
-    (iSys) ? _datumTrafo->setFirstSystem(false) :
-                 _datumTrafo->setFirstSystem(true);
     if (OPT->_refSatRequired) {
       _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
@@ -174 +194 @@
   }
 
-  // refSat change required?
-  // -----------------------
-  if (_obsPool->refSatChangeRequired()) {
-    _xFlt = xFltOld;
-    _QFlt = QFltOld;
-  }
   // close epoch processing
   // ----------------------
-  else {
-    cmpDOP(allObs);
-    _parlist->printResult(_epoTime, _QFlt, _xFlt);
-    _lastEpoTimeOK = _epoTime;  // remember time of last successful epoch processing
-  }
+  cmpDOP(allObs);
+  _parlist->printResult(_epoTime, _QFlt, _xFlt);
+  _lastEpoTimeOK = _epoTime;  // remember time of last successful epoch processing
 
   return success;
@@ -198 +210 @@
                                  bool pseudoObsIonoAvailable,
                                  bool preProcessing) {
-  qDebug() << "======t_pppFilter::processSystem=======";
   LOG.setf(ios::fixed);
 
   // Detect Cycle Slips
   // ------------------
+
   if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
     return failure;
@@ -221 +233 @@
     maxObs -= 1;
   }
-  qDebug() << "par.size()      : " << _parlist->nPar() << " LCs.size(): " << usedLCs;
-  qDebug() << "obsVector.size(): " << obsVector.size() << " maxObs    : " << maxObs;
 
   // Outlier Detection Loop
   // ----------------------
   for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
-    qDebug() << "iOutlier: " << iOutlier;
 
     if (iOutlier > 0) {
@@ -236 +245 @@
     // First-Design Matrix, Terms Observed-Computed, Weight Matrix
     // -----------------------------------------------------------
-    qDebug() << "A(" << maxObs << "," << _parlist->nPar() << ")";
     Matrix                AA(maxObs, _parlist->nPar());
     ColumnVector          ll(maxObs);
     DiagonalMatrix        PP(maxObs); PP = 0.0;
-
-    // TETSPLOT
-    for (unsigned iPar = 0; iPar < params.size(); iPar++) {
-      const t_pppParam* par = params[iPar];
-      cout << "  " << par->toString() <<endl;
-    }
-    cout << endl;
-    //END TETSPLOT
 
     int iObs = -1;
@@ -253 +253 @@
     vector<t_lc::type>   usedTypes;
     for (unsigned ii = 0; ii < obsVector.size(); ii++) {
-      t_pppSatObs* obs = obsVector[ii]; qDebug() << "SATELLITE: " << obs->prn().toString().c_str() << "isRef: " << obs->isReference();
+      t_pppSatObs* obs = obsVector[ii];
       if (!obs->outlier()) {
         for (unsigned jj = 0; jj < usedLCs; jj++) {
@@ -263 +263 @@
           for (unsigned iPar = 0; iPar < params.size(); iPar++) {
             const t_pppParam* par = params[iPar];
-            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
-            cout << setw(10) << par->partial(_epoTime, obs, tLC);
-          }
-          cout << endl;
+            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
+          }
           ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
           PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
-          //qDebug() << "ll[iObs]: " << ll[iObs];
-          //qDebug() << "PP[iObs]: " << PP[iObs];
-        }
-      }
-    }
-
-    if ((!preProcessing) &&
-       (OPT->_obsModelType == OPT->DCMcodeBias ||
-        OPT->_obsModelType == OPT->DCMphaseBias)) {
+        }
+      }
+    }
+
+    if ((preProcessing && ! iOutlier) &&
+        (OPT->_obsModelType == OPT->DCMcodeBias || OPT->_obsModelType == OPT->DCMphaseBias)) {
           _datumTrafo->updateIndices(maxObs);
       _datumTrafo->prepareAA(AA, _numEpoProcessing, _parlist->nPar());
+      if (_obsPool->refSatChangeRequired()) { // from detectCycleSlips()
+        return success;
+      }
     }
 
@@ -331 +329 @@
         }
       }
-      if      (par && preProcessing) {
-        if (par->prn() == refPrn) {
+      if (preProcessing) {
+        if (par && obs->prn() == refPrn) {
           _obsPool->setRefSatChangeRequired(true);
-        }
-      }
-      else if (par && !preProcessing) {
-        if (par->ambResetCandidate()) {
-          resetAmb(par->prn(), obsVector, &QSav, &xSav);
+          break;
+        }
+      }
+      else {
+        if (par) {
+          if (par->ambResetCandidate() || _obsPool->hasHistoricalRefSat(par->prn())) {
+            resetAmb(par->prn(), obsVector, &QSav, &xSav);
+          }
+          else {
+            par->setAmbResetCandidate();
+            obs->setOutlier();
+          }
         }
         else {
-          par->setAmbResetCandidate();
           obs->setOutlier();
         }
       }
-      else if (!par && !preProcessing){
-        obs->setOutlier();
-      }
-    }
-
+    }
     // Print Residuals
     // ---------------
@@ -381 +381 @@
                                     const t_prn& refPrn,
                                     bool preProcessing) {
-
-  const double            SLIP       = 20.0;  // slip threshold
+  if (_obsPool->hasHistoricalRefSat(refPrn)) {
+    return success;
+  }
+
+  double                  SLIP       = 20.0;  // slip threshold
   string                  epoTimeStr = string(_epoTime);
   const vector<t_pppParam*>& params  = _parlist->params();
+
+  if (preProcessing) {
+    SLIP += 10.0;
+  }
 
   for (unsigned ii = 0; ii < LCs.size(); ii++) {
@@ -391 +398 @@
       for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
         const t_pppSatObs* obs = obsVector[iObs];
+
+        if (preProcessing && obs->prn() != refPrn) {continue;}
 
         // Check set Slips and Jump Counters
@@ -405 +414 @@
           slip = true;
         }
-        _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
+        if (!preProcessing) {
+          _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
+        }
 
         if (_slips[obs->prn()]._biasJumpCounter != -1 &&
@@ -412 +423 @@
           slip = true;
         }
-
+        if (!preProcessing) {
+          _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
+        }
         // Slip Set
         // --------
         if (slip) {
           if (preProcessing) {
-            if (obs->prn() == refPrn) {
-              _obsPool->setRefSatChangeRequired(true);
-            }
+            _obsPool->setRefSatChangeRequired(true);
           }
           else {
@@ -431 +442 @@
           for (unsigned iPar = 0; iPar < params.size(); iPar++) {
             const t_pppParam* par = params[iPar];
-            AA[iPar] = par->partial(_epoTime, obs, tLC);
-          }
+            AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
+          }
+
           double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
           double vv = DotProduct(AA, _xFlt) - ll;
           if (fabs(vv) > SLIP) {
             if (preProcessing) {
-              if (obs->prn() == refPrn) {
-                _obsPool->setRefSatChangeRequired(true);
-              }
+              _obsPool->setRefSatChangeRequired(true);
             }
             else {
@@ -451 +461 @@
     }
   }
-
   return success;
 }
@@ -459 +468 @@
 t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
                             SymmetricMatrix* QSav, ColumnVector* xSav) {
+
   t_irc irc = failure;
   vector<t_pppParam*>& params = _parlist->params();
@@ -504 +514 @@
     for (unsigned ii = 0; ii < obsVector.size(); ii++) {
       t_pppSatObs* obs = obsVector[ii];
+      char system = obs->prn().system();
+      t_prn refPrn = t_prn();
+      if (OPT->_refSatRequired) {
+        refPrn = _obsPool->getRefSatMapElement(system)->prn();
+      }
       if (obs->isValid() && !obs->outlier()) {
         ++_numSat;
         for (unsigned iPar = 0; iPar < numPar; iPar++) {
           const t_pppParam* par = _parlist->params()[iPar];
-          AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
+          AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
         }
       }
@@ -571 +586 @@
 // Compute datum transformation
 ////////////////////////////////////////////////////////////////////////////
-void t_pppFilter::datumTransformation(void) {
-  _QFlt = _datumTrafo->varCov(Q());
-}
-
+void t_pppFilter::datumTransformation(const ColumnVector& xFltOld, const SymmetricMatrix& QFltOld) {
+
+  Matrix D21 = _datumTrafo->computeTrafoMatrix();
+  _QFlt << D21 * QFltOld * D21.t();
+  _xFlt = D21 * xFltOld;
+}
+
+
+// Reset Ambiguity Parameter (cycle slip)
+////////////////////////////////////////////////////////////////////////////
+t_irc t_pppFilter::addInfiniteNoise(t_pppParam::e_type para) {
+  t_irc irc = failure;
+  vector<t_pppParam*>& params = _parlist->params();
+  for (unsigned iPar = 0; iPar < params.size(); iPar++) {
+    t_pppParam* par = params[iPar];
+    if (par->type() == para) {
+      int ind = par->indexNew();
+      if (ind < 0) {
+        return irc;
+      }
+      _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
+      irc = success;
+    }
+  }
+  return irc;
+}