[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 | |
---|
| 19 | #define HIGH_PRECISION 0 |
---|
| 20 | |
---|
| 21 | /* Matrix multiply Full matrix by upper diagonal matrix; */ |
---|
| 22 | void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) |
---|
| 23 | { |
---|
| 24 | unsigned int i, j, k; |
---|
| 25 | long tmp_sum=0L; //in 15+qAU |
---|
| 26 | int *m2pom; |
---|
| 27 | int *m1pom=m1; |
---|
| 28 | int *respom=result; |
---|
| 29 | |
---|
| 30 | for (i=0; i<rows; i++) //rows of result |
---|
| 31 | { |
---|
| 32 | for (j=0; j<columns; j++) //columns of result |
---|
| 33 | { |
---|
| 34 | m2pom=up+j; |
---|
| 35 | |
---|
| 36 | for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1; |
---|
| 37 | { |
---|
| 38 | tmp_sum += ((long)(*(m1pom++))**m2pom)>>(15-qAU); |
---|
| 39 | m2pom+=columns; |
---|
| 40 | } |
---|
| 41 | // add the missing A(i,j) |
---|
| 42 | tmp_sum += (long)(*m1pom)<<qAU; // no need to shift |
---|
| 43 | m1pom-=j; // shift back to first element |
---|
| 44 | |
---|
| 45 | *respom++=tmp_sum>>15; |
---|
| 46 | |
---|
| 47 | tmp_sum=0; |
---|
| 48 | } |
---|
| 49 | m1pom+=columns; |
---|
| 50 | } |
---|
| 51 | }; |
---|
| 52 | |
---|
| 53 | |
---|
| 54 | void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) |
---|
| 55 | { |
---|
| 56 | int alpha; |
---|
| 57 | int beta,lambda; |
---|
| 58 | int b[5]; // ok even for 4-dim state |
---|
| 59 | int *a; // in [0,1] -> q15 |
---|
| 60 | unsigned int iy,j,i; |
---|
| 61 | |
---|
| 62 | int *b_j,*b_i; |
---|
| 63 | int *a_j; |
---|
| 64 | int *D_j; |
---|
| 65 | int *U_ij; |
---|
| 66 | int *x_i; |
---|
| 67 | |
---|
| 68 | a = U; // iyth row of U |
---|
| 69 | for (iy=0; iy<dimy; iy++) |
---|
| 70 | { |
---|
| 71 | // a is a row |
---|
| 72 | a_j=a; |
---|
| 73 | b_j=b; |
---|
| 74 | D_j=D; |
---|
| 75 | |
---|
| 76 | for (j=0; j<dimx; j++) |
---|
| 77 | { |
---|
| 78 | *b_j=((long)(*D_j)*(*a_j))>>15; |
---|
| 79 | b_j++; |
---|
| 80 | D_j++; |
---|
| 81 | a_j++; |
---|
| 82 | } |
---|
| 83 | |
---|
| 84 | alpha = R[iy]; //\alpha = R+vDv = R+a*b |
---|
| 85 | // R in q15, a in q15, b=q15 |
---|
| 86 | |
---|
| 87 | a_j=a; |
---|
| 88 | b_j=b; |
---|
| 89 | D_j=D; |
---|
| 90 | |
---|
| 91 | for (j=0; j<dimx; j++) |
---|
| 92 | { |
---|
| 93 | lambda=alpha; |
---|
| 94 | if (HIGH_PRECISION == 0) |
---|
| 95 | alpha = alpha + (((long)(*a_j)*(*b_j))>>15); |
---|
| 96 | else |
---|
| 97 | alpha = (((long)alpha<<15) + (long)(*a_j)*(*b_j))>>15; // xxxx lepsi presnost |
---|
| 98 | |
---|
| 99 | *D_j = ((long)lambda**D_j)/alpha; |
---|
| 100 | |
---|
| 101 | if (*D_j==0) *D_j=1; |
---|
| 102 | |
---|
| 103 | b_i=b; |
---|
| 104 | U_ij=U+j; |
---|
| 105 | |
---|
| 106 | for (i=0; i<j; i++) |
---|
| 107 | { |
---|
| 108 | beta = *U_ij; |
---|
| 109 | if (HIGH_PRECISION == 0) |
---|
| 110 | { |
---|
| 111 | *U_ij = *U_ij - ((long)(*a_j)*(*b_i))/lambda; |
---|
| 112 | *b_i = *b_i + (((long)beta*(*b_j))>>15); |
---|
| 113 | } |
---|
| 114 | else |
---|
| 115 | { |
---|
| 116 | *U_ij = ((long)lambda**U_ij - (long)(*a_j)*(*b_i))/lambda; |
---|
| 117 | *b_i = (((long)*b_i<<15) + (long)beta**b_j)>>15; |
---|
| 118 | } |
---|
| 119 | b_i++; |
---|
| 120 | U_ij+=dimx; |
---|
| 121 | } |
---|
| 122 | |
---|
| 123 | a_j++; |
---|
| 124 | b_j++; |
---|
| 125 | D_j++; |
---|
| 126 | } |
---|
| 127 | |
---|
| 128 | x_i=xp; |
---|
| 129 | b_i=b; |
---|
| 130 | |
---|
| 131 | for (i=0; i<dimx; i++) |
---|
| 132 | { |
---|
| 133 | if (HIGH_PRECISION == 0) |
---|
| 134 | *x_i += ((long)difz[iy]**b_i)/alpha; // multiply by unscaled Kalman gain |
---|
| 135 | else |
---|
| 136 | *x_i = ((long)alpha**x_i + (long)difz[iy]**b_i)/alpha; |
---|
| 137 | |
---|
| 138 | x_i++; |
---|
| 139 | b_i++; |
---|
| 140 | } |
---|
| 141 | |
---|
| 142 | a+=dimx; |
---|
| 143 | } |
---|
| 144 | } |
---|
| 145 | |
---|
| 146 | /**/ |
---|
| 147 | |
---|
| 148 | // Thorton procedure - Kalman predictive variance in UD |
---|
| 149 | void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) { |
---|
| 150 | unsigned int i,j,k; |
---|
| 151 | // copy D to Dold |
---|
| 152 | int *Dold_i,*Dold_k; |
---|
| 153 | int *D_i; |
---|
| 154 | int *PSIU_ij,*PSIU_ik,*PSIU_jk; |
---|
| 155 | int *Q_jj,*Q_ii,*Q_kk; |
---|
| 156 | int *U_ji; |
---|
| 157 | int *G_ik,*G_jk; |
---|
| 158 | int irows,jrows; |
---|
| 159 | long sigma; // in qAU+15!! |
---|
| 160 | long z; |
---|
| 161 | long z_tmp; |
---|
| 162 | |
---|
| 163 | Dold_i=Dold; |
---|
| 164 | D_i=D; |
---|
| 165 | |
---|
| 166 | for (i=0;i<rows;i++) |
---|
| 167 | { |
---|
| 168 | *Dold_i=*D_i; |
---|
| 169 | Dold_i++; |
---|
| 170 | D_i++; |
---|
| 171 | } |
---|
| 172 | |
---|
| 173 | // initialize G = eye() |
---|
| 174 | G_ik= G; |
---|
| 175 | *G_ik++=32767; |
---|
| 176 | for (i=0;i<rows-1;i++) |
---|
| 177 | { |
---|
| 178 | // clean elem before diag |
---|
| 179 | for (k=0; k<rows; k++) |
---|
| 180 | { |
---|
| 181 | *G_ik++=0; |
---|
| 182 | } |
---|
| 183 | *G_ik++=32767; |
---|
| 184 | } |
---|
| 185 | // eye created |
---|
| 186 | |
---|
| 187 | Dold_i=Dold+rows-1; |
---|
| 188 | D_i=D+rows-1; |
---|
| 189 | |
---|
| 190 | for (i=rows-1; 1; i--) |
---|
| 191 | { // stop if i==0 at the END! |
---|
| 192 | irows=i*rows; |
---|
| 193 | sigma = 0; |
---|
| 194 | |
---|
| 195 | PSIU_ik=PSIU+irows; |
---|
| 196 | Dold_k=Dold; |
---|
| 197 | |
---|
| 198 | for (k=0;k<rows; k++) |
---|
| 199 | { |
---|
| 200 | sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k); |
---|
| 201 | PSIU_ik++; |
---|
| 202 | Dold_k++; |
---|
| 203 | } |
---|
| 204 | |
---|
| 205 | sigma += (long)*(Q+i+irows)<<(qAU+qD-15); |
---|
| 206 | |
---|
| 207 | G_ik=G+irows+i+1; |
---|
| 208 | |
---|
| 209 | for (j=i+1; j<rows; j++) |
---|
| 210 | { |
---|
| 211 | sigma += ((((long)(*G_ik)**G_ik)>>15)**(Q+j+j*rows))>>(30-qAU-qD); |
---|
| 212 | G_ik++; |
---|
| 213 | } |
---|
| 214 | |
---|
| 215 | if (sigma>((long)1<<(15+qAU))) |
---|
| 216 | { |
---|
| 217 | *D_i = 32767; |
---|
| 218 | } else |
---|
| 219 | { |
---|
| 220 | *D_i=sigma>>qAU; |
---|
| 221 | } |
---|
| 222 | |
---|
| 223 | if (*D_i==0) *D_i=1; |
---|
| 224 | |
---|
| 225 | for (j=0;j<i;j++) |
---|
| 226 | { |
---|
| 227 | jrows = j*rows; |
---|
| 228 | |
---|
| 229 | sigma =0; |
---|
| 230 | PSIU_ik=PSIU+irows; |
---|
| 231 | PSIU_jk=PSIU+jrows; |
---|
| 232 | Dold_k=Dold; |
---|
| 233 | |
---|
| 234 | for (k=0;k<rows; k++) |
---|
| 235 | { |
---|
| 236 | |
---|
| 237 | sigma += (((long)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k; |
---|
| 238 | PSIU_ik++; |
---|
| 239 | PSIU_jk++; |
---|
| 240 | Dold_k++; |
---|
| 241 | } |
---|
| 242 | |
---|
| 243 | G_ik=G+irows+i; |
---|
| 244 | G_jk=G+jrows+i; |
---|
| 245 | Q_kk=Q+i*rows+i; |
---|
| 246 | |
---|
| 247 | for (k=i;k<rows;k++) |
---|
| 248 | { |
---|
| 249 | sigma += ((((long)(*G_ik)**G_jk)>>15)**Q_kk)>>(30-qAU-qD); |
---|
| 250 | G_ik++; |
---|
| 251 | G_jk++; |
---|
| 252 | Q_kk+=rows+1; |
---|
| 253 | } |
---|
| 254 | |
---|
| 255 | z=(sigma/(*D_i))<<(15-qAU); // shift to q15 |
---|
| 256 | if (z>32767) z=32767; |
---|
| 257 | if (z<-32768) z=-32768; |
---|
| 258 | |
---|
| 259 | U_ji=U+jrows+i; |
---|
| 260 | *U_ji = z; |
---|
| 261 | |
---|
| 262 | |
---|
| 263 | PSIU_ik=PSIU+irows; |
---|
| 264 | PSIU_jk=PSIU+jrows; |
---|
| 265 | |
---|
| 266 | for (k=0; k<rows;k++) |
---|
| 267 | { |
---|
| 268 | if (HIGH_PRECISION == 0) |
---|
| 269 | *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15; |
---|
| 270 | else |
---|
| 271 | *PSIU_jk = (((long)(*PSIU_jk)<<15) - (long)*U_ji**PSIU_ik)>>15; |
---|
| 272 | |
---|
| 273 | PSIU_ik++; |
---|
| 274 | PSIU_jk++; |
---|
| 275 | } |
---|
| 276 | |
---|
| 277 | G_jk=G+jrows; |
---|
| 278 | G_ik=G+irows; |
---|
| 279 | |
---|
| 280 | for (k=0;k<rows;k++) |
---|
| 281 | { |
---|
| 282 | if (HIGH_PRECISION == 0) |
---|
| 283 | *G_jk -= ((long)*U_ji**G_ik)>>15; |
---|
| 284 | else |
---|
| 285 | *G_jk = (((long)(*G_jk)<<15) - (long)*U_ji**G_ik)>>15; |
---|
| 286 | G_jk++; |
---|
| 287 | G_ik++; |
---|
| 288 | } |
---|
| 289 | } |
---|
| 290 | |
---|
| 291 | Dold_i--; |
---|
| 292 | D_i--; |
---|
| 293 | if (i==0) return; |
---|
| 294 | } |
---|
| 295 | } |
---|