Changeset 37 for tests/pmsm_unkQ.cpp
- Timestamp:
- 03/14/08 18:11:21 (17 years ago)
- Files:
-
- 1 modified
Legend:
- Unmodified
- Added
- Removed
-
tests/pmsm_unkQ.cpp
r33 r37 21 21 /* 22 22 // PMSM with Q on Ia and Ib given externally 23 class EKF_unQ : public EKF< ldmat> , public BMcond {23 class EKF_unQ : public EKF<chmat> , public BMcond { 24 24 public: 25 EKF_unQ( rx,ry,ru,rQ):EKF< ldmat>(rx,ry,ru),BMcond(rQ){};25 EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){}; 26 26 void condition(const vec &Q0){}; 27 27 };*/ 28 28 29 29 30 int main() { 30 31 // Kalman filter 31 int Ndat = 1000 0;32 int Ndat = 1000; 32 33 33 34 // cout << KF; … … 40 41 41 42 vec mu0= "100 100 100 1"; 42 vec Qdiag ( "0.1 0.1 0.01 0.0 0001" );43 vec Qdiag ( "0.1 0.1 0.01 0.01" ); 43 44 vec Rdiag ( "0.02 0.02" ); 44 45 vec vQ = "0.01:0.01:100"; 45 ldmat Q = ldmat ( Qdiag ); 46 ldmat R = ldmat ( Rdiag ); 47 EKF<ldmat> KFE ( rx,ry,ru ); 46 chmat Q ( Qdiag ); 47 chmat R ( Rdiag ); 48 EKFCh KFE ( rx,ry,ru ); 49 KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) ); 48 50 KFE.set_parameters ( &fxu,&hxu,Q,R ); 49 KFE.set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) );50 51 51 52 mat ll(100,Ndat); 52 53 53 EKF <ldmat>* kfArray[100];54 EKFCh* kfArray[100]; 54 55 55 56 for ( int i=0;i<100;i++ ) { 56 57 vec Qid ( Qdiag ); 57 58 Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i ); 58 kfArray[i]= new EKF <ldmat>( rx,ry,ru );59 kfArray[i]->set_ parameters ( &fxu,&hxu,ldmat ( Qid ),R);60 kfArray[i]->set_ est ( mu0, ldmat ( 1000*ones ( 4 ) ));59 kfArray[i]= new EKFCh ( rx,ry,ru ); 60 kfArray[i]->set_est ( mu0, chmat ( 1000*ones ( 4 ) ) ); 61 kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R ); 61 62 } 62 63 … … 89 90 //estimator 90 91 KFE.bayes ( concat ( dt,ut ) ); 91 for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)+=kfArray[i]->_ll();} 92 for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)=ll(i,t-1) + kfArray[i]->_ll(); 93 } 92 94 93 95 Xt.set_col ( t,xt );