Changeset 1321 for applications/pmsm/simulator_zdenek
- Timestamp:
- 04/04/11 14:15:18 (14 years ago)
- Location:
- applications/pmsm/simulator_zdenek
- Files:
-
- 5 modified
-
ekf_example/ekf_obj.cpp (modified) (3 diffs)
-
ekf_example/ekf_obj.h (modified) (1 diff)
-
ekf_example/matrix_vs.cpp (modified) (4 diffs)
-
ekf_example/matrix_vs.h (modified) (2 diffs)
-
test_Ch.cpp (modified) (3 diffs)
Legend:
- Unmodified
- Added
- Removed
-
applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
r1311 r1321 686 686 687 687 // Tuning of matrix R 688 R[0]=prevod(0. 05,15); // 0.05688 R[0]=prevod(0.5,15); // 0.05 689 689 R[3]=R[0]; 690 690 … … 699 699 //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 700 700 // cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < =========== 701 cH=prevod(Tv*Wref* Thetaref*Fmag/Iref/Ls,15); //701 cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 702 702 // cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 703 703 … … 833 833 cG=prevod(Tv*Wref/Thetaref,15); //in q15! 834 834 //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 835 //cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // <=======836 cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); //835 cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // <======= 836 //cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 837 837 // cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 838 838 -
applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
r1311 r1321 310 310 311 311 set_dim(4); 312 dimc = 2; 313 dimy =2; 312 314 E._mu()=zeros(4); 313 315 E._R()=zeros(4,4); -
applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
r1313 r1321 19 19 #include "matrix.h" 20 20 21 #include <stdio.h> 21 22 /* Matrix multiply Full matrix by upper diagonal matrix; */ 22 23 void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) { … … 204 205 // *U_ij += ((int32)lambda*(*b_i))>>15; // puvodni reseni 205 206 *U_ij -= ((int32)(*a_j)*(*b_i))/lambda; // pozadovane optimalni reseni 206 // *U_ij -= ((int32)((int16)((int32)(*a_j)<<15)/lambda)**b_i)>>15; // tohle funguje - problem je s tim pretypovanim na (int16)207 // *U_ij -= (int16)((int32)(*a_j)*(*b_i))/lambda;208 // z_pom = (((int32)(*a_j)*(*b_i))/lambda);209 /* z_pom = (int32)(*U_ij)-(int16)((int32)(*a_j)*(*b_i))/lambda;210 * if (z_pom > 32767) z_pom = 32767;211 * if (z_pom < - 32768) z_pom = -32768;212 *U_ij = z_pom; /**/213 // *U_ij += ((int32)z_pom_int16*(*b_i))>>15; // puvodni reseni - jen jina konstanta214 207 *b_i += ((int32)beta*(*b_j))>>15; 208 printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta); 215 209 } 216 210 } … … 218 212 for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) { 219 213 *x_i += ((int32)difz[iy]*(*b_i))/alpha; // multiply by unscaled Kalman gain 220 221 214 } 222 215 } … … 700 693 } 701 694 695 void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) { 696 int16 alpha,beta,gamma; 697 int16 delta, eta,epsilon,zeta,sigma,tau; 698 int16 i,j,iy; 699 int16 w[5]; 700 int32 tmp_long; 701 702 int16 *Ch_ij, *w_i, *x_i, *C_yi; 703 704 705 for (iy=0; iy<dimy; iy++) 706 { 707 alpha=R[iy]; 708 delta = difz[iy]; 709 710 for (j=0;j<dimx;j++) 711 { 712 C_yi = C+iy*dimx; 713 sigma = 0; 714 Ch_ij=Ch+j; 715 for (i=0;i<=j;i++){ 716 sigma += ((int32)*Ch_ij**C_yi)>>15; //sigma in qCh 717 Ch_ij+=dimx; 718 C_yi++; 719 } 720 721 //sigma=Ch[iy*dimx+j]; 722 beta=alpha; 723 // alpha+=((long)sigma*sigma)>>15; 724 alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15; // vyssi presnost 725 // gamma= qsqrt(((long)alpha*beta)); // verze pro DSP 726 gamma= (int16)(sqrt((double)((int32)alpha*beta))); // verze pro PC 727 728 w[j]=0; 729 730 Ch_ij=Ch+j; 731 w_i=w; 732 733 for (i=0;i<=j;i++) 734 { 735 // tau=Ch[i*dimx+j]; 736 tau=*Ch_ij; 737 // tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma; 738 tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma; 739 740 if (tmp_long>32767) 741 tmp_long=32767; 742 if (tmp_long<-32768) 743 tmp_long=-32768; 744 *Ch_ij=tmp_long; 745 746 // w_i+=((long)tau*sigma)>>15; 747 *w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15; 748 749 w_i++; 750 Ch_ij+=dimx; 751 } 752 } 753 754 x_i=xp; 755 w_i=w; 756 for (i=0;i<dimx;i++) { 757 // xp[i]+=((long)w[i]*delta)/alpha; 758 // *x_i+=((long)*w_i*delta)/alpha; 759 *x_i=((int32)*x_i*alpha+(int32)*w_i*delta)/alpha; // vyssi presnost 760 x_i++; 761 w_i++; 762 } 763 } 764 } 765 -
applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h
r1313 r1321 16 16 #define qCh 14 17 17 18 #define int16 short19 #define int32 int18 #define int16 int //short 19 #define int32 long //int 20 20 21 21 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */ … … 57 57 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ 58 58 extern void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); 59 60 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ 61 extern void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); -
applications/pmsm/simulator_zdenek/test_Ch.cpp
r1241 r1321 27 27 int PSICh[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0}; 28 28 int Chf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0}; 29 int Cf[10]={0,0,0,0,0, 0,0,0,0,0}; 29 30 int multip=1<<15; 30 31 … … 129 130 vec b; 130 131 mat C = zeros(2,5); 131 C(0,0)=1.0;C(1,1)=1.0; 132 C(0,0)=.2;C(1,1)=0.4;C(0,1)=0.3; 133 132 134 for (int iy=0; iy<2; iy++){ 133 135 a = U.T()*C.get_row(iy); // a is not modified, but … … 160 162 161 163 Ch = U*diag(sqrt(D)); 164 mat_to_int16(round_i(C*multip),Cf); 162 165 163 166 cout << "bef Carlson: " << imat(Chf,5,5) << endl; 164 165 carlson (difz,xf, Chf,Rf, 2, 5);167 168 carlson_fastC(difz,xf, Chf, Cf,Rf, 2, 5); 166 169 167 170 cout << endl<<"after Carlson" <<endl;
