- Timestamp:
- 09/02/10 23:21:51 (14 years ago)
- Files:
-
- 1 modified
Legend:
- Unmodified
- Added
- Removed
-
applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
r1179 r1180 88 88 for (j=0;j<rows; j++) { 89 89 sigma += (((long)PSIU[i*rows+j]*PSIU[i*rows+j])>>15)*(Dold[i]); 90 printf("-- %d,%d --", PSIU[i*rows+j], Dold[i]);91 90 } 92 91 sigma += Q[i*rows+i]; 93 92 for (j=i+1;j<rows; j++) { 94 93 sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j]; 95 printf("-- %d,%d --", G[i*rows+j], Q[j*rows+j]);96 94 } 97 printf("%d\n",sigma);98 95 99 96 *(D+i)=sigma>>15; … … 110 107 sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k]; 111 108 } 112 printf("%d\n",sigma);113 109 for (k=0;k<rows;k++) { 114 110 sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k]; 115 111 } 116 printf("%d\n",sigma);117 112 long z=sigma/D[i]; // shift by 15 118 113 if (z>32767) z=32767; … … 128 123 PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; 129 124 } 130 for(int x=0;x<16;x++) printf("%d,", PSIU[x]);131 printf("\n");132 125 133 126 for (k=0;k<rows;k++) { 134 127 G[j*rows+k] -= ((long)U[j*rows+i]*G[i*rows+k])>>15; 135 128 } 136 for(int x=0;x<16;x++) printf("%d,", G[x]);137 printf("\n");138 129 139 130 /* printf("end\n"); … … 231 222 k<rows; k++, PSIU_ik++) {//Dold_i= 232 223 sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_i); 233 printf("-- %d,%d --", *PSIU_ik, *Dold_i);234 224 } 235 225 sigma += *(Q+i+irows); 236 226 for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) { 237 227 sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows); 238 printf("++ %d,%d ++", *G_ik, *(Q+j+j*rows)); 239 240 } 241 printf("%d\n",sigma); 228 229 } 242 230 243 231 *D_i=sigma>>15; … … 254 242 sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k; 255 243 } 256 printf("%d\n",sigma);257 244 258 245 for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k; … … 260 247 sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk; 261 248 } 262 printf("%d\n",sigma);263 249 264 250 long z=sigma/(*D_i); // shift by 15 … … 274 260 *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15; 275 261 } 276 for(int x=0;x<16;x++) printf("%d,", PSIU[x]);277 printf("\n");278 262 279 263 for (k=0,G_jk=G+jrows,G_ik=G+irows; … … 281 265 *G_jk -= ((long)*U_ji**G_ik)>>15; 282 266 } 283 for(int x=0;x<16;x++) printf("%d,", G[x]);284 printf("\n");285 267 286 268 }