Changeset 62
- Timestamp:
- 04/06/08 20:14:56 (17 years ago)
- Files:
-
- 1 added
- 13 modified
Legend:
- Unmodified
- Added
- Removed
-
BDM.kdevelop
r60 r62 5 5 <projectdirectory>/home/smidl/work/mixpp</projectdirectory> 6 6 <absoluteprojectpath>true</absoluteprojectpath> 7 <author ></author>8 <email ></email>7 <author/> 8 <email/> 9 9 <version>$VERSION$</version> 10 10 <primarylanguage>C++</primarylanguage> … … 17 17 </secondaryLanguages> 18 18 <projectname>BDM</projectname> 19 <description ></description>20 <defaultencoding ></defaultencoding>19 <description/> 20 <defaultencoding/> 21 21 <versioncontrol>kdevsubversion</versioncontrol> 22 22 </general> … … 24 24 <filelistdirectory>/home/smidl/work/mixpp</filelistdirectory> 25 25 <run> 26 <mainprogram>/home/smidl/work/mixpp/tests/pmsm_sim 2</mainprogram>26 <mainprogram>/home/smidl/work/mixpp/tests/pmsm_sim3</mainprogram> 27 27 <directoryradio>custom</directoryradio> 28 28 <customdirectory>/home/smidl/work/mixpp</customdirectory> 29 <programargs ></programargs>29 <programargs/> 30 30 <terminal>false</terminal> 31 31 <autocompile>true</autocompile> 32 32 <envvars/> 33 <globaldebugarguments ></globaldebugarguments>33 <globaldebugarguments/> 34 34 <globalcwd>/home/smidl/work/mixpp</globalcwd> 35 35 <useglobalprogram>false</useglobalprogram> … … 45 45 <numberofjobs>0</numberofjobs> 46 46 <dontact>false</dontact> 47 <makebin ></makebin>47 <makebin/> 48 48 <selectedenvironment>default</selectedenvironment> 49 49 <environments> … … 51 51 </environments> 52 52 <prio>0</prio> 53 <defaulttarget ></defaulttarget>54 <makeoptions ></makeoptions>53 <defaulttarget/> 54 <makeoptions/> 55 55 </make> 56 56 <filetypes> … … 73 73 <other> 74 74 <prio>0</prio> 75 <otherbin ></otherbin>76 <defaulttarget ></defaulttarget>77 <otheroptions ></otheroptions>75 <otherbin/> 76 <defaulttarget/> 77 <otheroptions/> 78 78 <selectedenvironment>default</selectedenvironment> 79 79 <environments> … … 147 147 </qt> 148 148 <creategettersetter> 149 <prefixGet ></prefixGet>149 <prefixGet/> 150 150 <prefixSet>set</prefixSet> 151 151 <prefixVariable>m_,_</prefixVariable> … … 180 180 <projectdirectory>/home/smidl/work/mixpp</projectdirectory> 181 181 <absoluteprojectpath>true</absoluteprojectpath> 182 <gdbpath ></gdbpath>183 <dbgshell ></dbgshell>184 <configGdbScript ></configGdbScript>185 <runShellScript ></runShellScript>186 <runGdbScript ></runGdbScript>182 <gdbpath/> 183 <dbgshell/> 184 <configGdbScript/> 185 <runShellScript/> 186 <runGdbScript/> 187 187 <breakonloadinglibs>true</breakonloadinglibs> 188 188 <separatetty>false</separatetty> -
BDM.kdevelop.filelist
r60 r62 1 # KDevelop Custom Project File List 1 2 CMakeLists.txt 2 3 bdm … … 28 29 tests/CMakeLists.txt 29 30 tests/pmsm_sim.cpp 30 tests/pmsm_sim2.cpp31 31 tests/pmsm_unkQ.cpp 32 32 tests/pmsm_unkQpf.cpp … … 39 39 tests/testResample.cpp 40 40 tests/testSmp.cpp 41 tests/testbidiff.cpp -
CMakeLists.txt
r41 r62 45 45 add_subdirectory (tests) 46 46 add_subdirectory (simulator_zdenek) 47 add_subdirectory (simulator_zdenek/ekf_example) -
bdm/estim/libKF.cpp
r51 r62 59 59 } 60 60 61 62 63 /////////////////////////////// EKFS 64 EKFfull::EKFfull ( RV rvx0, RV rvy0, RV rvu0 ) : BM ( rvx0 ) {}; 65 66 void EKFfull::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const mat Q0,const mat R0 ) { 67 pfxu = pfxu0; 68 phxu = phxu0; 69 70 dimx = rv.count(); 71 dimy = phxu0->_dimy(); 72 dimu = phxu0->_dimu(); 73 74 A.set_size(dimx,dimx); 75 C.set_size(dimy,dimx); 76 //initialize matrices A C, later, these will be only updated! 77 pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); 78 B.clear(); 79 phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); 80 D.clear(); 81 82 R = R0; 83 Q = Q0; 84 } 85 86 void EKFfull::bayes ( const vec &dt ) { 87 it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); 88 89 vec u = dt.get ( dimy,dimy+dimu-1 ); 90 vec y = dt.get ( 0,dimy-1 ); 91 92 pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); 93 phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); 94 95 //Time update 96 mu = pfxu->eval(mu,u);// A*mu + B*u; 97 P = A*P*A.transpose() + Q; 98 99 //Data update 100 _Ry = C*P*C.transpose() + R; 101 _iRy = inv ( _Ry ); 102 _K = P*C.transpose() *_iRy; 103 P -= _K*C*P; // P = P -KCP; 104 vec yp = phxu->eval(mu,u); 105 mu += _K* ( y-yp ); 106 107 if ( BM::evalll ) { 108 //from enorm 109 vec x=y-yp; 110 BM::ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) ); 111 } 112 }; 113 114 115 61 116 void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ) { 62 117 … … 138 193 preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() ); 139 194 195 // mat Sttm = _P->to_mat(); 196 140 197 // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); 141 198 if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" ); 142 199 143 200 ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); 144 _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();201 _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); 145 202 ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); 146 203 _Ry->inv ( *_iRy ); 147 204 fy._cached ( true ); 205 206 // mat iRY = inv(_Ry->to_mat()); 207 // mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm; 208 // mat _K2 = Stt*C.T()*inv(R.to_mat()); 209 210 // prediction 211 *_mu = pfxu->eval ( *_mu ,u ); 212 148 213 *_yp = phxu->eval ( *_mu,u ); 149 214 150 *_mu = pfxu->eval ( *_mu ,u ) + ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp ); 215 /* vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ 216 217 //correction //= initial value is already prediction! 218 *_mu += ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp ); 219 151 220 /* _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); 152 221 *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ … … 173 242 R.setD ( R0 ); 174 243 }; 244 245 -
bdm/estim/libKF.h
r51 r62 26 26 27 27 class KalmanFull { 28 protected: 28 29 int dimx, dimy, dimu; 29 30 mat A, B, C, D, R, Q; … … 47 48 //! print elements of KF 48 49 friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); 49 50 //! For EKFfull; 51 KalmanFull(){}; 50 52 }; 51 53 … … 166 168 167 169 /*! 170 \brief Extended Kalman Filter in full matrices 171 172 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 173 */ 174 class EKFfull : public KalmanFull, public BM { 175 //! Internal Model f(x,u) 176 diffbifn* pfxu; 177 //! Observation Model h(x,u) 178 diffbifn* phxu; 179 public: 180 //! Default constructor 181 EKFfull ( RV rvx, RV rvy, RV rvu ); 182 //! Set nonlinear functions for mean values and covariance matrices. 183 void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 ); 184 //! Here dt = [yt;ut] of appropriate dimensions 185 void bayes ( const vec &dt ); 186 //! set estimates 187 void set_est (vec mu0, mat P0){mu=mu0;P=P0;}; 188 //!dummy! 189 epdf& _epdf(){enorm<fsqmat> E(rv); return E;}; 190 }; 191 192 /*! 168 193 \brief Extended Kalman Filter 169 194 … … 186 211 187 212 /*! 188 \brief Extended Kalman Filter in Square roo r213 \brief Extended Kalman Filter in Square root 189 214 190 215 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. … … 330 355 331 356 }; 357 358 332 359 333 360 //TODO why not const pointer?? -
bdm/stat/libBM.h
r51 r62 88 88 int dimy; 89 89 public: 90 //!default constructor 91 fnc(int dy):dimy(dy){}; 90 92 //! function evaluates numerical value of $f(x)$ at $x=cond$ 91 93 virtual vec eval ( const vec &cond ) { -
bdm/stat/libFN.cpp
r33 r62 4 4 using std::endl; 5 5 6 bilinfn::bilinfn ( const RV &rvx0, const RV &rvu0, const mat &A0, const mat &B0 ) : diffbifn ( rvx0,rvu0 )6 bilinfn::bilinfn ( const RV &rvx0, const RV &rvu0, const mat &A0, const mat &B0 ) : diffbifn (A0.rows(), rvx0,rvu0 ) 7 7 { 8 8 //check input -
bdm/stat/libFN.h
r33 r62 29 29 vec eval ( const vec &cond ) {return val;}; 30 30 //!Default constructor 31 constfn ( const vec &val0 ) : val ( val0 ) {};31 constfn ( const vec &val0 ) :fnc(val0.length()), val ( val0 ) {}; 32 32 }; 33 33 … … 46 46 // linfn evalsome ( ivec &rvind ); 47 47 //!default constructor 48 linfn ( const RV &rv0 ) : rv ( rv0 ),A ( eye ( rv0.count() ) ),B ( zeros ( rv0.count() ) ) { };48 linfn ( const RV &rv0 ) : fnc(rv0.count()), rv ( rv0 ),A ( eye ( rv0.count() ) ),B ( zeros ( rv0.count() ) ) { }; 49 49 //! Set values of \c A and \c B 50 50 void set_parameters ( const mat &A0 , const vec &B0 ) {A=A0; B=B0;}; … … 87 87 virtual void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {}; 88 88 //!Default constructor (dimy is not set!) 89 diffbifn ( const RV rvx0, const RV rvu0 ) :rvx ( rvx0 ),rvu ( rvu0 ) {dimx=rvx.count();dimu=rvu.count();};89 diffbifn (int dimy, const RV rvx0, const RV rvu0 ) : fnc(dimy), rvx ( rvx0 ),rvu ( rvu0 ) {dimx=rvx.count();dimu=rvu.count();}; 90 90 //! access function 91 91 int _dimx() const{return dimx;} … … 104 104 105 105 //! Default constructor 106 bilinfn ( const RV &rvx0, const RV &rvu0 ) : diffbifn ( rvx0,rvu0 ) ,A ( eye ( dimx ) ),B ( zeros ( dimx,dimu ) ) {};106 bilinfn ( const RV &rvx0, const RV &rvu0 ) : diffbifn (dimx, rvx0,rvu0 ) ,A ( eye ( dimx ) ),B ( zeros ( dimx,dimu ) ) {}; 107 107 //! Alternative constructor 108 108 bilinfn ( const RV &rvx0, const RV &rvu0, const mat &A0, const mat &B0 ); -
matlab/testKF.m
r37 r62 7 7 C=[1 0];%; 0 1]; 8 8 D=0.1;%[0.1; 0]; 9 R=0. 1;%[1 0; 0 0.1];9 R=0.01;%[1 0; 0 0.1]; 10 10 Q=[0.2 0 ; 0 0.2]; 11 11 … … 74 74 plot(x'); 75 75 hold on 76 plot([ zeros(size(xth,1),1)xth]','--'); % shift the predictions76 plot([xth]','--'); % shift the predictions 77 77 plot(xth2','+'); 78 78 plot(xthE','o'); … … 81 81 exec_times 82 82 exec_matlab./exec_times 83 keyboard 83 84 end -
tests/CMakeLists.txt
r60 r62 45 45 include_directories (${BDM_SOURCE_DIR}/simulator_zdenek) 46 46 link_directories (${BDM_BINARY_DIR}/simulator_zdenek) 47 link_directories (${BDM_BINARY_DIR}/simulator_zdenek/ekf_example) 47 48 add_executable (pmsm_sim pmsm_sim.cpp) 48 49 add_executable (pmsm_sim2 pmsm_sim2.cpp) 50 add_executable (pmsm_sim3 pmsm_sim3.cpp) 49 51 target_link_libraries (pmsm_sim ${BdmLibs} pmsmsim netcdf_c++) 50 52 target_link_libraries (pmsm_sim2 ${BdmLibs} pmsmsim netcdf_c++) 53 target_link_libraries (pmsm_sim3 ${BdmLibs} pmsmsim ekf_obj) -
tests/pmsm.h
r54 r62 10 10 RV ry ( "7 8", "{oia, oib}", ones_i ( 2 ) ,zeros_i ( 2 )); 11 11 12 //! State evolution model for a PMSM drive and its derivative with respect to $x$12 //! State evolution model for a PMSM drive and its derivative with respect to \$x\$ 13 13 class IMpmsm : public diffbifn { 14 14 double Rs, Ls, dt, Ypm, kp, p, J, Mz; 15 15 16 16 public: 17 IMpmsm() :diffbifn ( rx, ru ) {};17 IMpmsm() :diffbifn (rx.count(), rx, ru ) {}; 18 18 //! Set mechanical and electrical variables 19 19 void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;} … … 34 34 xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls; 35 35 //om 36 xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;36 xk ( 2 ) = omm;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz; 37 37 //th 38 38 xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi> … … 52 52 A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) ); 53 53 // d om 54 A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );55 A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );54 A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) ); 55 A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) ); 56 56 A ( 2,2 ) = 1.0; 57 A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );57 A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) ); 58 58 // d th 59 59 A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0; … … 64 64 }; 65 65 66 //! Observation model for PMSM drive and its derivative with respect to $x$66 //! Observation model for PMSM drive and its derivative with respect to \$x\$ 67 67 class OMpmsm: public diffbifn { 68 68 public: 69 OMpmsm() :diffbifn ( rx,ru ) {};69 OMpmsm() :diffbifn (2, rx,ru ) {}; 70 70 71 71 vec eval ( const vec &x0, const vec &u0 ) { -
tests/pmsm_sim2.cpp
r60 r62 19 19 #include "simulator.h" 20 20 21 #include <netcdfcpp.h> 22 void write_to_nc ( NcFile &nc, mat &X, std::string Xn, Array<std::string> A ) { 23 char tmpstr[200]; 24 int Len = X.rows(); 25 26 sprintf ( tmpstr,"%s.length",Xn.c_str() ); 27 NcDim* lengt = nc.add_dim ( tmpstr, ( long ) Len ); 28 for ( int j=0; j<X.cols(); j++ ) { 29 if ( j<A.length() ) 30 sprintf ( tmpstr,"%s_%s",Xn.c_str(), ( A ( j ) ).c_str() ); 31 else 32 sprintf ( tmpstr,"%s_%d",Xn.c_str(),j ); 33 // Create variables and their attributes 34 NcVar* P = nc.add_var ( tmpstr, ncDouble, lengt ); 35 const double* Dp = X._data(); 36 P->put ( &Dp[j*Len],Len ); 37 } 38 } 39 21 #include "iopom.h" 40 22 41 23 using namespace itpp; … … 54 36 void set_simulator_t(double &Ww) { 55 37 56 if (t>0. 0) x[8]=1.2; // 1A //0.2ZP38 if (t>0.2) x[8]=1.2; // 1A //0.2ZP 57 39 if (t>0.4) x[8]=10.8; // 9A 58 40 if (t>0.6) x[8]=25.2; // 21A … … 99 81 if (t>8.3) x[8]=10.8; // 9A 100 82 if (t>8.5) x[8]=25.2; // 21A 83 84 // x[8]=0.0; 101 85 } 102 86 103 87 int main() { 104 88 // Kalman filter 105 int Ndat = 30000;89 int Ndat = 90000; 106 90 double h = 1e-6; 107 91 int Nsimstep = 125; … … 113 97 // internal model 114 98 IMpmsm fxu; 115 // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz)99 // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) 116 100 fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); 117 101 // observation model … … 119 103 120 104 vec mu0= "0.0 0.0 0.0 0.0"; 121 vec Qdiag ( "0.01 0.01 0.0001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001 122 vec Rdiag ( "0.005 0.005" ); //var(diff(xth)) = "0.034 0.034" 105 // vec Qdiag ( "0.01 0.01 0.01 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001 106 vec Qdiag ( "18 18 157.9 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001 107 vec Rdiag ( "90 90" ); //var(diff(xth)) = "0.034 0.034" 123 108 chmat Q ( Qdiag ); 124 109 chmat R ( Rdiag ); 125 110 EKFCh KFE ( rx,ry,ru ); 126 KFE.set_ parameters ( &fxu,&hxu,Q,R);127 KFE.set_ est ( mu0, chmat ( 1*ones ( 4 ) ));111 KFE.set_est ( mu0, ( 1*eye ( 4 ) ) ); 112 KFE.set_parameters ( &fxu,&hxu,Qdiag,Rdiag); 128 113 129 114 RV rQ ( "100","{Q}","4","0" ); 130 115 EKF_unQ KFEp ( rx,ry,ru,rQ ); 116 KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) ); 131 117 KFEp.set_parameters ( &fxu,&hxu,Q,R ); 132 KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );133 118 134 119 mgamma_fix evolQ ( rQ,rQ ); … … 139 124 epdf& pfinit=evolQ._epdf(); 140 125 M.set_est ( pfinit ); 141 evolQ.set_parameters ( 100000.0, Qdiag, 0.5);126 evolQ.set_parameters ( 5000.0, Qdiag, 0.9999 ); 142 127 143 128 // … … 190 175 //Exit program: 191 176 177 char tmpstr[200]; 178 sprintf(tmpstr,"%s/%s","here/","format"); 179 ofstream form(tmpstr); 180 form << "# Experimetal header file"<< endl; 181 dirfile_write(form,"here/",Xt,"X","{isa isb om th }" ); 182 dirfile_write ( form,"here",XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" ); 183 dirfile_write ( form,"here",XtE,"XE","{isa isb om th }" ); 184 dirfile_write ( form,"here",Dt,"Dt","{isa isb ua ub }" ); 185 192 186 //////////////// 193 187 // Just Testing 194 NcFile nc ( "pmsm_sim.nc", NcFile::Replace ); // Create and leave in define mode188 /* NcFile nc ( "pmsm_sim2.nc", NcFile::Replace ); // Create and leave in define mode 195 189 if ( ! nc.is_valid() ) { std::cerr << "creation of NCFile failed."<<endl;} 196 190 197 191 write_to_nc ( nc,Xt,"X","{isa isb om th }" ); 198 write_to_nc ( nc,XtM,"XtM","{q1 q2 isa isb om th }" );192 write_to_nc ( nc,XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" ); 199 193 write_to_nc ( nc,XtE,"XE","{isa isb om th }" ); 200 write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" ); 194 write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" );*/ 201 195 return 0; 202 196 }