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

Timestamp:

Sep 28, 2026, 9:55:30 PM (11 hours ago)

Author:

stuerze

Message:

filter reset after exception

File:

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

trunk/BNC/src/PPP/pppFilter.cpp

@@ -255 +255 @@
     // Kalman update step
     // ------------------
-    kalman(AA, ll, PP, _QFlt, _xFlt);
+    try {
+      kalman(AA, ll, PP, _QFlt, _xFlt);
+    }
+    catch (Exception&) {
+      logNonPosDef(_QFlt, params);
+      throw;
+    }
 
     // Check Residuals
@@ -319 +325 @@
   }
   return success;
+}
+
+// Diagnose a Variance-Covariance Matrix that failed Cholesky decomposition
+////////////////////////////////////////////////////////////////////////////
+void t_pppFilter::logNonPosDef(const SymmetricMatrix& QQ,
+                               const vector<t_pppParam*>& params) const {
+
+  string epoTimeStr = string(_epoTime);
+  int nn = QQ.Nrows();
+
+  for (int ii = 1; ii <= nn; ii++) {
+    double qq = QQ(ii,ii);
+    if (!(qq > 0.0)) {
+      LOG << epoTimeStr << " NotPosDef diag " << params[ii-1]->toString()
+          << " Q = " << scientific << qq << fixed << endl;
+    }
+  }
+
+  // Cholesky pivots: first index where the matrix stops being positive definite
+  // ---------------------------------------------------------------------------
+  Matrix LL(nn, nn); LL = 0.0;
+  for (int jj = 1; jj <= nn; jj++) {
+    double sum = QQ(jj,jj);
+    for (int kk = 1; kk < jj; kk++) {
+      sum -= LL(jj,kk) * LL(jj,kk);
+    }
+    if (!(sum > 0.0)) {
+      const t_pppParam* par = params[jj-1];
+      LOG << epoTimeStr << " NotPosDef pivot " << jj << " " << par->toString()
+          << " Q = " << scientific << QQ(jj,jj) << " pivot = " << sum
+          << fixed << " indexOld = " << par->indexOld() << endl;
+      for (int kk = 1; kk < jj; kk++) {
+        double rho = QQ(jj,kk) / sqrt(QQ(jj,jj) * QQ(kk,kk));
+        if (fabs(rho) > 0.999) {
+          LOG << epoTimeStr << " NotPosDef   corr " << params[kk-1]->toString()
+              << ' ' << setprecision(9) << rho << endl;
+        }
+      }
+      return;
+    }
+    LL(jj,jj) = sqrt(sum);
+    for (int ii = jj + 1; ii <= nn; ii++) {
+      double hlp = QQ(ii,jj);
+      for (int kk = 1; kk < jj; kk++) {
+        hlp -= LL(ii,kk) * LL(jj,kk);
+      }
+      LL(ii,jj) = hlp / LL(jj,jj);
+    }
+  }
 }