Changeset 1233 for applications/pmsm/simulator_zdenek
- Timestamp:
- 10/26/10 14:28:53 (14 years ago)
- Files:
-
- 1 modified
Legend:
- Unmodified
- Added
- Removed
-
applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
r1231 r1233 42 42 tmp_sum=tmp_sum>>15; 43 43 if (tmp_sum>32767) { 44 //tmp_sum=32767;44 printf("Au - saturated\n"); 45 45 } 46 46 if (tmp_sum<-32768) { 47 //tmp_sum=-32768;47 printf("Au - saturated\n"); 48 48 } 49 49 // printf("Au - saturated\n"); … … 155 155 sigma =0; 156 156 for (k=0;k<rows;k++) { 157 int tmp= (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15)); 158 if (tmp>32767) printf("!"); 157 159 sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k]; 158 160 } … … 265 267 sigma = 0; 266 268 for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold; 267 k<rows; k++, PSIU_ik++,Dold_k++) { //Dold_i=268 sigma += (((long)(*PSIU_ik)**Dold_k)>>( 2*qAU-15))*(*PSIU_ik);269 k<rows; k++, PSIU_ik++,Dold_k++) { 270 sigma += (((long)(*PSIU_ik)**Dold_k)>>(qAU))*(*PSIU_ik)<<(15-qAU); 269 271 } 270 272 sigma += *(Q+i+irows)<<15; … … 286 288 k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) { 287 289 288 sigma += ((( (long)*PSIU_ik)**Dold_k)>>(2*qAU-15))**PSIU_jk;289 } 290 291 for (k=i ,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;290 sigma += (((long(*PSIU_ik)**Dold_k)>>qAU)**PSIU_jk)<<(15-qAU); 291 } 292 293 for (k=i+1,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k; 292 294 k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) { 293 295 sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk; … … 338 340 for (j=0;j<dimx;j++) { 339 341 beta = alpha; 340 lambda = -( ((long)a[j])<<15)/beta;341 alpha += (( (long)(a[j])*b[j])>>15);342 D[j] = ((( ((long)beta)<<15)/alpha)*D[j])>>15; //gamma is long342 lambda = -(long(a[j])<<15)/beta; 343 alpha += ((long(a[j])*b[j])>>15); 344 D[j] = (((long(beta)<<15)/alpha)*D[j])>>15; //gamma is long 343 345 if (D[j]==0) D[j]=1; 344 346 … … 350 352 } 351 353 } 352 int dzs = ( ((long)difz[iy])<<15)/alpha; // apply scaling to innovations354 int dzs = (long(difz[iy])<<15)/alpha; // apply scaling to innovations 353 355 // no shift due to gamma 354 356 for (i=0; i<dimx; i++) { 355 xp[i] += ( (long)dzs*b[i])>>15; // multiply by unscaled Kalman gain357 xp[i] += (long(dzs*b[i]))>>15; // multiply by unscaled Kalman gain 356 358 } 357 359