Changeset 37 for tests/pmsm_unkQ.cpp

Timestamp:

03/14/08 18:11:21 (16 years ago)

Author:

smidl

Message:

Matrix in Cholesky decomposition, Square-root Kalman and many bug fixes

Files:

• tests/pmsm_unkQ.cpp (modified) (3 diffs)

tests/pmsm_unkQ.cpp

@@ -21 +21 @@
 /*
 // PMSM with Q on Ia and Ib given externally
-class EKF_unQ : public EKF<ldmat> , public BMcond {
+class EKF_unQ : public EKF<chmat> , public BMcond {
 public:
-        EKF_unQ( rx,ry,ru,rQ):EKF<ldmat>(rx,ry,ru),BMcond(rQ){};
+        EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){};
         void condition(const vec &Q0){};
 };*/
 
+
 int main() {
         // Kalman filter
-        int Ndat = 10000;
+        int Ndat = 1000;
 
 //      cout << KF;
@@ -40 +41 @@
 
         vec mu0= "100 100 100 1";
-        vec Qdiag ( "0.1 0.1 0.01 0.00001" );
+        vec Qdiag ( "0.1 0.1 0.01 0.01" );
         vec Rdiag ( "0.02 0.02" );
         vec vQ = "0.01:0.01:100";
-        ldmat Q = ldmat ( Qdiag );
-        ldmat R = ldmat ( Rdiag );
-        EKF<ldmat> KFE ( rx,ry,ru );
+        chmat Q ( Qdiag );
+        chmat R ( Rdiag );
+        EKFCh KFE ( rx,ry,ru );
+        KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
         KFE.set_parameters ( &fxu,&hxu,Q,R );
-        KFE.set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) );
 
         mat ll(100,Ndat);
 
-        EKF<ldmat>* kfArray[100];
+        EKFCh* kfArray[100];
 
         for ( int i=0;i<100;i++ ) {
                 vec Qid ( Qdiag );
                 Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i );
-                kfArray[i]= new EKF<ldmat> ( rx,ry,ru );
-                kfArray[i]->set_parameters ( &fxu,&hxu,ldmat ( Qid ),R );
-                kfArray[i]->set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) );
+                kfArray[i]= new EKFCh ( rx,ry,ru );
+                kfArray[i]->set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
+                kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R );
         }
 
@@ -89 +90 @@
                 //estimator
                 KFE.bayes ( concat ( dt,ut ) );
-                for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)+=kfArray[i]->_ll();}
+                for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)=ll(i,t-1) + kfArray[i]->_ll();
+                }
                 
                 Xt.set_col ( t,xt );