Changeset 1328 for library

Timestamp:

04/12/11 22:31:34 (13 years ago)

Author:

smidl

Message:

ekf_ud assumes that model to be on scaled variables

Location:

library/bdm/estim

Files:

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

library/bdm/estim/kalman.cpp

@@ -494 +494 @@
                 }
                 D(i) = sigma; 
+                if (D(i)>(1<<17)) D(i)=(1<<17);
                 /////////////// !!!!!!!!!!!!!
-                if (D(i) > 1579000) D(i) =1579000;
                 
                 for (j=0;j<i;j++){ 
@@ -515 +515 @@
                 }
         }
-        
+
         if ( log_level[logU] ){
-                // transformed U
-                mat tU;
-                mat P= U*diag(D)*U.T();
-
-                vec xref;
-                if (dim==4){
-                        xref.set_length(4);
-                        xref(0)= 30.0*1.4142 *4.;
-                        xref(1)= 30.0*1.4142 *4.;
-                        xref(2)= 6.283185*200.*4.;
-                        xref(3) = 3.141593;
-                //xref(4) = 34.0;
-                }else{
-                        xref.set_length(2);
-                        xref(0)= 6.283185*200.*4.;
-                        xref(1) = 3.141593;
-                        
-                }
+                //vec tmp=vec(U._data(),dimension()*dimension());
+                log_level.store(logU, vec(U._data(),dimension()*dimension()));
                 
-                mat T = diag(1.0/(xref));
-                mat Pf = T*P*T;
+                log_level.store(logD, D);
                 
-                mat TQT = T*Q*T;
-                
-                ldmat Pld(Pf);
-                
-                //vec tmp=vec(U._data(),dimension()*dimension());
-                vec tmp=vec(Pld._L()._data(),dimension()*dimension());
-                log_level.store(logU,round(((int)1<<15)*tmp));
-                vec vPf(Pf._data(),Pf._datasize());
-                log_level.store(logP,round(((int)1<<15)*vPf));
-                log_level.store(logD,(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);
+                log_level.store(logA, vec(A._data(), dimension()*dimension()));
+
+                log_level.store(logC, vec(C._data(), dimension()*dimensiony()));
                 
         }
         if ( log_level[logG] ){
-                vec tmp=vec(G._data(),dimension()*dimension());
-                log_level.store(logG,tmp);
+                log_level.store(logG,vec(G._data(),dimension()*dimension()));
         }
         //cout << "Ut: " << U << endl;
@@ -596 +566 @@
                 
         }
+        
+        // artificial round of U
+        U = round(U*(1<<15))/(1<<15);
                 
 /////

library/bdm/estim/kalman.h

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