Changeset 2283 in ntrip

Timestamp:

Feb 9, 2010, 10:20:10 AM (14 years ago)

Author:

mervart

Message:

* empty log message *

Location:

trunk/BNC

Files:

• bncmodel.cpp (modified) (4 diffs)
• bncmodel.h (modified) (1 diff)
• ppp.gpt (modified) (1 diff)

trunk/BNC/bncmodel.cpp

@@ -60 +60 @@
 const double   sig_crd_0        =  100.0;
 const double   sig_crd_p        =  100.0;
-const double   sig_clk_0        =  100.0;
-const double   sig_trp_0        =    0.05;
-const double   sig_trp_p        =    1e-7;
+const double   sig_clk_0        = 1000.0;
+const double   sig_trp_0        =    0.01;
+const double   sig_trp_p        =    1e-6;
 const double   sig_amb_0_GPS    =  100.0;
 const double   sig_amb_0_GLO    = 1000.0;
-const double   sig_P3           =   10.0;
-const double   sig_L3_GPS       =    0.02;
-const double   sig_L3_GLO       =    0.02;
+const double   sig_P3           =    1.0;
+const double   sig_L3_GPS       =    0.01;
+const double   sig_L3_GLO       =    0.01;
 
 // Constructor
@@ -572 +572 @@
     Matrix          AA(nObs, nPar);  // first design matrix
     ColumnVector    ll(nObs);        // tems observed-computed
-    SymmetricMatrix PP(nObs); PP = 0.0;
+    DiagonalMatrix  PP(nObs); PP = 0.0;
     
     unsigned iObs = 0;
@@ -636 +636 @@
     QQsav = _QQ;
 
-    Matrix          ATP = AA.t() * PP;
-    SymmetricMatrix NN = _QQ.i();
-    NN    << NN + ATP * AA;
-    _QQ   = NN.i();
-    dx    = _QQ * ATP * ll; 
-    vv    = ll - AA * dx;
+//    Matrix          ATP = AA.t() * PP;
+//    SymmetricMatrix NN = _QQ.i();
+//    NN    << NN + ATP * AA;
+//    _QQ   = NN.i();
+//    dx    = _QQ * ATP * ll; 
+
+    kalman(AA, ll, PP, _QQ, dx);
+
+    vv = ll - AA * dx;
 
     ostringstream str;
@@ -878 +881 @@
   emit newNMEAstr(outStr.toAscii());
 }
+
+
+//// 
+//////////////////////////////////////////////////////////////////////////////
+void bncModel::kalman(const Matrix& AA, const ColumnVector& ll, 
+                      const DiagonalMatrix& PP, 
+                      SymmetricMatrix& QQ, ColumnVector& dx) {
+
+  int nObs = AA.Nrows();
+  int nPar = AA.Ncols();
+
+  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);
+}

trunk/BNC/bncmodel.h

@@ -88 +88 @@
   void writeNMEAstr(const QString& nmStr);
 
+  static void kalman(const Matrix& AA, const ColumnVector& ll, 
+                     const DiagonalMatrix& PP, 
+                     SymmetricMatrix& QQ, ColumnVector& dx);
+
   bncTime            _time;
   QByteArray         _staID;

trunk/BNC/ppp.gpt

@@ -5 +5 @@
 set format x "%H:%M"
 
-### set term png
-### set output "ffmj1.png"
+set term png
+set output "ffmj1.png"
 
 set title "FFMJ1"
 
 set ylabel "meters"
-set xlabel "19-Dec-2009"
-###set yrange [-1.5:1.5]
+set xlabel "29-Dec-2009"
+set yrange [-1.5:1.5]
 
-plot "ppp" u 1:4 t "dH" w lp 3, \
-     ""    u 1:2 t "dN" w lp 1, \
-     ""    u 1:3 t "dE" w lp 2
+plot "ppp" u 1:4 t "dH" w l 3, \
+     ""    u 1:2 t "dN" w l 1, \
+     ""    u 1:3 t "dE" w l 2
 
-pause -1
+###pause -1