Changeset 1199 for library

Timestamp:

09/29/10 11:22:16 (14 years ago)

Author:

smidl

Message:

verbose Kalman

Location:

library/bdm/estim

Files:

• kalman.cpp (modified) (2 diffs)
• kalman.h (modified) (2 diffs)

library/bdm/estim/kalman.cpp

@@ -519 +519 @@
                 mat P= U*diag(D)*U.T();
                 
-                vec xref(5);
+                vec xref(4);
                 xref(0)= 30.0*1.4142;
                 xref(1)= 30.0*1.4142;
                 xref(2)= 6.283185*200.;
                 xref(3) = 3.141593;
-                xref(4) = 34.0;
+                //xref(4) = 34.0;
                 
                 mat T = diag(1.0/(xref));
@@ -533 +533 @@
                 //vec tmp=vec(U._data(),dimension()*dimension());
                 vec tmp=vec(Pld._L()._data(),dimension()*dimension());
-                log_level.store(logU,round(((int)1<<14)*tmp));
-                log_level.store(logD,Pld._D());
+                log_level.store(logU,round(((int)1<<15)*tmp));
+                log_level.store(logD,round(Pld._D()*(1<<15)));
+                
+                mat Atr=T*A*diag(xref)*(1<<15);
+                mat Att=Atr.T();
+                vec vA(Att._data(), dimension()*dimension());
+                log_level.store(logA,vA);
+                
         }
         if ( log_level[logG] ){

library/bdm/estim/kalman.h

@@ -430 +430 @@
         protected:
                 //! logger
-                LOG_LEVEL(EKF_UD,logU, logG, logD);
+                LOG_LEVEL(EKF_UD,logU, logG, logD,logA);
                 //! Internal Model f(x,u)
                 shared_ptr<diffbifn> pfxu;
@@ -481 +481 @@
                         if ( log_level[logD] )
                                 L.add_vector ( log_level, logD, RV ( dimension()), prefix );
+                        
+                        L.add_vector ( log_level, logA, RV ( dimension()*dimension()), prefix );
                         
                 }