[1263] | 1 | /************************************ |
---|
| 2 | Extended Kalman Filter |
---|
| 3 | Matrix operations |
---|
| 4 | |
---|
| 5 | V. Smidl, Z. Peroutka |
---|
| 6 | |
---|
| 7 | Rev. 28.10.2010 (ZP) |
---|
| 8 | |
---|
| 9 | 26.10.2010 Prvni verze (VS) |
---|
| 10 | |
---|
| 11 | 26.10.2010 Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA. |
---|
| 12 | 27.10.2010 Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci. |
---|
| 13 | 28.10.2010 Drobne upravy v kodu. |
---|
| 14 | |
---|
| 15 | *************************************/ |
---|
| 16 | |
---|
| 17 | #include "matrix_vs.h" |
---|
| 18 | #include "qmath.h" |
---|
| 19 | |
---|
| 20 | #define HIGH_PRECISION 1 |
---|
| 21 | |
---|
| 22 | void householder(int16 *Ch, int16 *Q, unsigned int16 dimx) |
---|
| 23 | { |
---|
| 24 | int16 k,j,i; |
---|
| 25 | int16 alpha,beta; |
---|
| 26 | int32 sigma; // 2*qCh |
---|
| 27 | int32 tmp_long; |
---|
| 28 | int16 B[25];//Q in qCh |
---|
| 29 | int16 w[5]; |
---|
| 30 | int16 v[5]; |
---|
| 31 | |
---|
| 32 | int16 *B_ij, *Q_i, *B_kj, *Ch_kj, *Ch_ij, *w_j, *v_j; |
---|
| 33 | |
---|
| 34 | B_ij=B; |
---|
| 35 | Q_i=Q; |
---|
| 36 | // copy Q to B |
---|
| 37 | for (i=0;i<dimx*dimx;i++) |
---|
| 38 | { |
---|
| 39 | *B_ij++=(*Q_i++)>>(15-qCh); |
---|
| 40 | } |
---|
| 41 | |
---|
| 42 | for (k=dimx-1; k>=0; k--) |
---|
| 43 | { |
---|
| 44 | sigma=0; |
---|
| 45 | B_kj=B+k*dimx+k; |
---|
| 46 | Ch_kj=Ch+k*dimx; |
---|
| 47 | |
---|
| 48 | for (j=k;j<dimx ;j++) |
---|
| 49 | { |
---|
| 50 | sigma+=((long)*B_kj**B_kj); |
---|
| 51 | B_kj++; |
---|
| 52 | } |
---|
| 53 | |
---|
| 54 | for (j=0;j<=k;j++) |
---|
| 55 | { |
---|
| 56 | sigma+=((long)*Ch_kj**Ch_kj); |
---|
| 57 | Ch_kj++; |
---|
| 58 | } |
---|
| 59 | |
---|
| 60 | //alpha in qCh |
---|
| 61 | // alpha = (int)(sqrt((double)sigma)+0.5); // verze pro PC |
---|
| 62 | alpha = qsqrt(sigma); // verze pro DSP |
---|
| 63 | |
---|
| 64 | sigma=0; |
---|
| 65 | |
---|
| 66 | w_j=w; |
---|
| 67 | B_kj=B+k*dimx; |
---|
| 68 | |
---|
| 69 | for (j=0;j<dimx;j++) |
---|
| 70 | { |
---|
| 71 | *w_j=*B_kj; |
---|
| 72 | sigma+=(long)*w_j**w_j; |
---|
| 73 | w_j++; |
---|
| 74 | B_kj++; |
---|
| 75 | } |
---|
| 76 | |
---|
| 77 | v_j=v; |
---|
| 78 | Ch_kj=Ch+k*dimx; |
---|
| 79 | |
---|
| 80 | for (j=0; j<=k;j++) |
---|
| 81 | { |
---|
| 82 | if (j==k) { |
---|
| 83 | *v_j=*Ch_kj-alpha; |
---|
| 84 | } else { |
---|
| 85 | *v_j=*Ch_kj; |
---|
| 86 | } |
---|
| 87 | sigma+=(long)*v_j**v_j; |
---|
| 88 | |
---|
| 89 | v_j++; |
---|
| 90 | Ch_kj++; |
---|
| 91 | } |
---|
| 92 | |
---|
| 93 | alpha=sigma>>(qCh+1); // alpha = sigma /2; |
---|
| 94 | if (alpha==0) alpha =1; |
---|
| 95 | |
---|
| 96 | for (i=0;i<=k;i++) |
---|
| 97 | { |
---|
| 98 | sigma=0; |
---|
| 99 | |
---|
| 100 | B_ij=B+i*dimx+i; |
---|
| 101 | w_j=w+i; |
---|
| 102 | |
---|
| 103 | for (j=i;j<dimx;j++) |
---|
| 104 | { |
---|
| 105 | sigma+=((long)*B_ij**w_j); |
---|
| 106 | |
---|
| 107 | B_ij++; |
---|
| 108 | w_j++; |
---|
| 109 | } |
---|
| 110 | |
---|
| 111 | Ch_ij = Ch + i*dimx; |
---|
| 112 | v_j=v; |
---|
| 113 | |
---|
| 114 | for (j=0;j<=k;j++) |
---|
| 115 | { |
---|
| 116 | sigma+=(long)*Ch_ij**v_j; |
---|
| 117 | |
---|
| 118 | Ch_ij++; |
---|
| 119 | v_j++; |
---|
| 120 | } |
---|
| 121 | |
---|
| 122 | sigma = sigma >> 15; // navrat do Q15 |
---|
| 123 | if (sigma>32767) sigma=32767; |
---|
| 124 | |
---|
| 125 | |
---|
| 126 | B_ij=B+i*dimx+i; |
---|
| 127 | w_j=w+i; |
---|
| 128 | |
---|
| 129 | for (j=i;j<dimx;j++) |
---|
| 130 | { |
---|
| 131 | tmp_long=((long)*B_ij*alpha-sigma**w_j)/alpha; |
---|
| 132 | if (tmp_long>32767) |
---|
| 133 | tmp_long=32767; |
---|
| 134 | if (tmp_long<-32768) |
---|
| 135 | tmp_long=-32768; |
---|
| 136 | *B_ij++=tmp_long; |
---|
| 137 | w_j++; |
---|
| 138 | }; |
---|
| 139 | |
---|
| 140 | Ch_ij=Ch+i*dimx; |
---|
| 141 | v_j=v; |
---|
| 142 | |
---|
| 143 | for (j=0;j<=k;j++) |
---|
| 144 | { |
---|
| 145 | tmp_long=((long)*Ch_ij*alpha-sigma**v_j)/alpha; |
---|
| 146 | if (tmp_long>32767) |
---|
| 147 | tmp_long=32767; |
---|
| 148 | if (tmp_long<-32768) |
---|
| 149 | tmp_long=-32768; |
---|
| 150 | *Ch_ij++=tmp_long; |
---|
| 151 | v_j++; |
---|
| 152 | } |
---|
| 153 | } |
---|
| 154 | } |
---|
| 155 | } |
---|
| 156 | |
---|
| 157 | void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) { |
---|
| 158 | int16 alpha,beta,gamma; |
---|
| 159 | int16 delta, eta,epsilon,zeta,sigma,tau; |
---|
| 160 | int16 i,j,iy; |
---|
| 161 | int16 w[5]; |
---|
| 162 | int32 tmp_long; |
---|
| 163 | |
---|
| 164 | int16 *Ch_ij, *w_i, *x_i, *Ch_iyj; |
---|
| 165 | |
---|
| 166 | |
---|
| 167 | for (iy=0; iy<dimy; iy++) |
---|
| 168 | { |
---|
| 169 | alpha=R[iy]; |
---|
| 170 | delta = difz[iy]; |
---|
| 171 | |
---|
| 172 | Ch_iyj=Ch+iy*dimx; |
---|
| 173 | |
---|
| 174 | for (j=0;j<dimx;j++) |
---|
| 175 | { |
---|
| 176 | sigma=*Ch_iyj++; |
---|
| 177 | beta=alpha; |
---|
| 178 | // alpha+=((long)sigma*sigma)>>15; |
---|
| 179 | alpha=(((long)alpha<<15)+(long)sigma*sigma)>>15; // vyssi presnost |
---|
| 180 | gamma= qsqrt(((long)alpha*beta)); // predelat v DSP |
---|
| 181 | w[j]=0; |
---|
| 182 | |
---|
| 183 | Ch_ij=Ch+j; |
---|
| 184 | w_i=w; |
---|
| 185 | |
---|
| 186 | for (i=0;i<=j;i++) |
---|
| 187 | { |
---|
| 188 | tau=*Ch_ij; |
---|
| 189 | tmp_long=((long)beta**Ch_ij -(long)sigma**w_i)/gamma; |
---|
| 190 | |
---|
| 191 | /* *Ch_ij=tmp_long; |
---|
| 192 | if (tmp_long>32767) |
---|
| 193 | *Ch_ij=32767; |
---|
| 194 | if (tmp_long<-32768) |
---|
| 195 | *Ch_ij=-32768; /**/ |
---|
| 196 | if (tmp_long>32767) |
---|
| 197 | tmp_long=32767; |
---|
| 198 | if (tmp_long<-32768) |
---|
| 199 | tmp_long=-32768; |
---|
| 200 | *Ch_ij=tmp_long; |
---|
| 201 | |
---|
| 202 | // w_i+=((long)tau*sigma)>>15; |
---|
| 203 | *w_i=(((long)*w_i<<15)+(long)tau*sigma)>>15; |
---|
| 204 | |
---|
| 205 | w_i++; |
---|
| 206 | Ch_ij+=dimx; |
---|
| 207 | } |
---|
| 208 | } |
---|
| 209 | |
---|
| 210 | x_i=xp; |
---|
| 211 | w_i=w; |
---|
| 212 | for (i=0;i<dimx;i++) { |
---|
| 213 | // xp[i]+=((long)w[i]*delta)/alpha; |
---|
| 214 | // *x_i+=((long)*w_i*delta)/alpha; |
---|
| 215 | *x_i=((long)*x_i*alpha+(long)*w_i*delta)/alpha; // vyssi presnost |
---|
| 216 | x_i++; |
---|
| 217 | w_i++; |
---|
| 218 | } |
---|
| 219 | } |
---|
| 220 | } |
---|
| 221 | |
---|
| 222 | /* Matrix multiply Full matrix by upper diagonal matrix; */ |
---|
| 223 | void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) { |
---|
| 224 | unsigned int16 i, j, k; |
---|
| 225 | int32 tmp_sum=0L; |
---|
| 226 | int16 *m2pom; |
---|
| 227 | int16 *m1pom=m1; |
---|
| 228 | int16 *respom=result; |
---|
| 229 | |
---|
| 230 | for (i=0; i<rows; i++) //rows of result |
---|
| 231 | { |
---|
| 232 | for (j=0; j<columns; j++) //columns of result |
---|
| 233 | { |
---|
| 234 | m2pom=up+j; |
---|
| 235 | |
---|
| 236 | for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1; |
---|
| 237 | { |
---|
| 238 | tmp_sum+=(int32)(*(m1pom++))**m2pom; |
---|
| 239 | m2pom+=columns; |
---|
| 240 | } |
---|
| 241 | m1pom-=(j+1); // shift back to first element |
---|
| 242 | |
---|
| 243 | *respom++=tmp_sum>>15; |
---|
| 244 | |
---|
| 245 | tmp_sum=0; |
---|
| 246 | } |
---|
| 247 | m1pom+=(columns); |
---|
| 248 | } |
---|
| 249 | } |
---|
| 250 | |
---|
| 251 | void givens(int16 *Ch, int16 *Q, unsigned int16 dimx) |
---|
| 252 | { |
---|
| 253 | int16 i,j,k; |
---|
| 254 | int16 rho,s,c,tau; |
---|
| 255 | int32 tmp_long; |
---|
| 256 | |
---|
| 257 | int16 A[25];//beware |
---|
| 258 | |
---|
| 259 | int16 *A_ij, *Q_i, *Ch_ki, *Ch_kj, *Ch_ii, *Ch_ij, *A_kj; |
---|
| 260 | |
---|
| 261 | A_ij=A; |
---|
| 262 | Q_i=Q; |
---|
| 263 | // copy Q to A |
---|
| 264 | for (i=0;i<dimx*dimx;i++) |
---|
| 265 | { |
---|
| 266 | *A_ij++=(*Q_i++)>>(15-qCh); |
---|
| 267 | } |
---|
| 268 | |
---|
| 269 | for (i=dimx-1; i>=0; i--) |
---|
| 270 | { |
---|
| 271 | Ch_ii=Ch+i*dimx+i; |
---|
| 272 | A_ij=A+i*dimx; |
---|
| 273 | |
---|
| 274 | for (j=0; j<dimx; j++) |
---|
| 275 | { |
---|
| 276 | if (*A_ij!=0) |
---|
| 277 | { |
---|
| 278 | tmp_long=(long)*Ch_ii**Ch_ii+(long)*A_ij**A_ij; |
---|
| 279 | |
---|
| 280 | rho=qsqrt(tmp_long); |
---|
| 281 | s=((long)*A_ij<<15)/rho; |
---|
| 282 | c=((long)*Ch_ii<<15)/rho; |
---|
| 283 | |
---|
| 284 | Ch_ki=Ch+i; |
---|
| 285 | A_kj=A+j; |
---|
| 286 | |
---|
| 287 | for (k=0;k<=i; k++) |
---|
| 288 | { |
---|
| 289 | tau=((long)c**A_kj-(long)s**Ch_ki)>>15; |
---|
| 290 | *Ch_ki=((long)s**A_kj+(long)c**Ch_ki)>>15; |
---|
| 291 | *A_kj=tau; |
---|
| 292 | |
---|
| 293 | Ch_ki+=dimx; |
---|
| 294 | A_kj+=dimx; |
---|
| 295 | } |
---|
| 296 | |
---|
| 297 | } |
---|
| 298 | |
---|
| 299 | A_ij++; |
---|
| 300 | } |
---|
| 301 | |
---|
| 302 | Ch_ij = Ch+i*dimx; |
---|
| 303 | |
---|
| 304 | for (j=0; j<i; j++) |
---|
| 305 | { |
---|
| 306 | if (*Ch_ij!=0) |
---|
| 307 | { |
---|
| 308 | tmp_long=(long)*Ch_ii**Ch_ii+(long)*Ch_ij**Ch_ij; |
---|
| 309 | rho=qsqrt(tmp_long); |
---|
| 310 | s=((long)*Ch_ij<<15)/rho; |
---|
| 311 | c=((long)*Ch_ii<<15)/rho; |
---|
| 312 | |
---|
| 313 | Ch_kj = Ch + j; |
---|
| 314 | Ch_ki = Ch + i; |
---|
| 315 | |
---|
| 316 | for (k=0; k<=i; k++) |
---|
| 317 | { |
---|
| 318 | tau=((long)c**Ch_kj-(long)s**Ch_ki)>>15; |
---|
| 319 | *Ch_ki =((long)s**Ch_kj+(long)c**Ch_ki)>>15; |
---|
| 320 | *Ch_kj=tau; |
---|
| 321 | |
---|
| 322 | Ch_kj += dimx; |
---|
| 323 | Ch_ki += dimx; |
---|
| 324 | } |
---|
| 325 | } |
---|
| 326 | |
---|
| 327 | Ch_ij++; |
---|
| 328 | } |
---|
| 329 | } |
---|
| 330 | } |
---|