Changeset 1312 for library

Timestamp:

03/25/11 19:52:13 (13 years ago)

Author:

smidl

Message:

EKF_UD is pro 2d model

Files:

• library/bdm/estim/kalman.cpp (modified) (2 diffs)

library/bdm/estim/kalman.cpp

@@ -520 +520 @@
                 mat tU;
                 mat P= U*diag(D)*U.T();
-                
-                vec xref(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;
+
+                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;
+                        
+                }
                 
                 mat T = diag(1.0/(xref));
@@ -538 +546 @@
                 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)));