Changeset 5807 in ntrip for trunk/BNC/src/bncutils.cpp

Timestamp:

Aug 6, 2014, 11:09:26 AM (10 years ago)

Author:

mervart

Message:

 

File:

• trunk/BNC/src/bncutils.cpp (modified) (3 diffs)

trunk/BNC/src/bncutils.cpp

@@ -46 +46 @@
 #include <QStringList>
 #include <QDateTime>
+
+#include <newmatap.h>
 
 #include "bncutils.h"
@@ -285 +287 @@
 }
 
+// 
+////////////////////////////////////////////////////////////////////////////
+double Frac (double x) {
+  return x-floor(x); 
+}
+
+// 
+////////////////////////////////////////////////////////////////////////////
+double Modulo (double x, double y) { 
+  return y*Frac(x/y); 
+}
+
 // Round to nearest integer
 ////////////////////////////////////////////////////////////////////////////
@@ -548 +562 @@
   }
 }
+
+//
+//////////////////////////////////////////////////////////////////////////////
+void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP, 
+            SymmetricMatrix& QQ, ColumnVector& dx) {
+
+  Tracer tracer("kalman");
+
+  int nPar = AA.Ncols();
+  int nObs = AA.Nrows();
+  UpperTriangularMatrix SS = Cholesky(QQ).t();
+
+  Matrix SA = SS*AA.t();
+  Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
+  for (int ii = 1; ii <= nObs; ++ii) {
+    SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
+  }
+
+  SRF.SubMatrix   (nObs+1, nObs+nPar, 1, nObs) = SA;
+  SRF.SymSubMatrix(nObs+1, nObs+nPar)          = SS;
+  
+  UpperTriangularMatrix UU;
+  QRZ(SRF, UU);
+  
+  SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
+  UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
+  Matrix YY  = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
+  
+  UpperTriangularMatrix SHi = SH_rt.i();
+  
+  Matrix KT  = SHi * YY; 
+  SymmetricMatrix Hi; Hi << SHi * SHi.t();
+
+  dx = KT.t() * ll;
+  QQ << (SS.t() * SS);
+}
+