[1174] | 1 | /************************************ |
---|
| 2 | Extended Kalman Filter |
---|
| 3 | Matrix operations |
---|
| 4 | |
---|
| 5 | V. Smidl |
---|
| 6 | |
---|
| 7 | Rev. 30.8.2010 |
---|
| 8 | |
---|
| 9 | 30.8.2010 Prvni verze |
---|
| 10 | |
---|
| 11 | *************************************/ |
---|
| 12 | #include "fixed.h" |
---|
| 13 | #include "stdio.h" |
---|
| 14 | /* Matrix multiply Full matrix by upper diagonal matrix; */ |
---|
| 15 | void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns){ |
---|
| 16 | unsigned int i, j, k; |
---|
| 17 | long tmp_sum=0L; |
---|
| 18 | int *m2pom; |
---|
| 19 | |
---|
| 20 | for (i=0; i<rows; i++) //rows of result |
---|
| 21 | { |
---|
| 22 | for (j=0; j<columns; j++) //columns of result |
---|
| 23 | { |
---|
| 24 | m2pom=up+j;//?? |
---|
| 25 | |
---|
| 26 | for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1; |
---|
| 27 | { |
---|
| 28 | tmp_sum+=(long)(*m1++)**m2pom; |
---|
| 29 | m2pom+=columns; |
---|
| 30 | } |
---|
| 31 | // add the missing A(i,j) |
---|
| 32 | tmp_sum +=(long)(*m1)<<15; // no need to shift |
---|
| 33 | m1-=(j); // shift back to first element |
---|
| 34 | |
---|
| 35 | // saturation effect |
---|
| 36 | tmp_sum=tmp_sum>>15; |
---|
| 37 | if (tmp_sum>32767) tmp_sum=32767; |
---|
| 38 | if (tmp_sum<-32768) tmp_sum=-32768; |
---|
| 39 | |
---|
| 40 | *result++=tmp_sum; |
---|
| 41 | |
---|
| 42 | tmp_sum=0; |
---|
| 43 | } |
---|
| 44 | m1+=(columns); |
---|
| 45 | } |
---|
| 46 | }; |
---|
| 47 | |
---|
| 48 | void UDprt(int *U, int *D){ |
---|
| 49 | return; |
---|
| 50 | for (int i=0;i<5;i++){ |
---|
| 51 | for (int j=0;j<5;j++){ |
---|
| 52 | printf("%d,",U[i*5+j]); |
---|
| 53 | } |
---|
| 54 | printf("\n"); |
---|
| 55 | } |
---|
| 56 | for (int i=0;i<5;i++){printf("%d,",D[i]);} |
---|
| 57 | printf("\n"); |
---|
| 58 | } |
---|
| 59 | |
---|
| 60 | // Thorton procedure - Kalman predictive variance in UD |
---|
| 61 | void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows){ |
---|
| 62 | unsigned int i,j,k; |
---|
| 63 | // copy D to Dold |
---|
| 64 | int *Dold_pom=Dold; |
---|
| 65 | for (i=0;i<rows;i++){*Dold_pom++=*D++;} |
---|
| 66 | D-=rows; // back to first D |
---|
| 67 | |
---|
| 68 | // initialize G = eye() |
---|
| 69 | int *G_pom = G; |
---|
| 70 | *G_pom++=1<<14; |
---|
| 71 | for (i=0;i<rows-1;i++){ |
---|
| 72 | // clean elem before diag |
---|
| 73 | for (j=0; j<rows; j++){ |
---|
| 74 | *G_pom++=0.0; |
---|
| 75 | } |
---|
| 76 | *G_pom++=1<<14; |
---|
| 77 | } |
---|
| 78 | // eye created |
---|
| 79 | |
---|
| 80 | long sigma; // in q15 |
---|
| 81 | for (i=rows-1; i>=0;i--){ // check i==0 at the END! |
---|
| 82 | sigma = 0; |
---|
| 83 | |
---|
| 84 | for (j=0;j<rows; j++){ |
---|
| 85 | sigma += (((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]); |
---|
| 86 | } |
---|
| 87 | sigma += Q[i+i*rows]; |
---|
| 88 | for (j=i+1;j<rows; j++){ |
---|
| 89 | sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows]; |
---|
| 90 | } |
---|
| 91 | |
---|
| 92 | *(D+i)=sigma>>15; |
---|
| 93 | if (D[i]==0) D[i]=1; |
---|
| 94 | |
---|
| 95 | /* printf("d=sig\n"); |
---|
| 96 | UDprt(U,D); |
---|
| 97 | UDprt(G,Dold);*/ |
---|
| 98 | |
---|
| 99 | for (j=0;j<i;j++){ |
---|
| 100 | // printf("\n%d,%d\n",i,j); |
---|
| 101 | sigma =0; |
---|
| 102 | for (k=0;k<rows;k++){ |
---|
| 103 | sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k]; |
---|
| 104 | } |
---|
| 105 | for (k=0;k<rows;k++){ |
---|
| 106 | sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k]; |
---|
| 107 | } |
---|
| 108 | long z=sigma/D[i]; // shift by 15 |
---|
| 109 | if (z>32767) z=32767; |
---|
| 110 | if (z<-32768) z=-32768; |
---|
| 111 | |
---|
| 112 | U[j*rows+i] = (int)z; |
---|
| 113 | |
---|
| 114 | /* printf("U=sig/D\n"); |
---|
| 115 | UDprt(U,D); |
---|
| 116 | UDprt(G,Dold);*/ |
---|
| 117 | |
---|
| 118 | for (k=0;k<rows;k++){ |
---|
| 119 | PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; |
---|
| 120 | } |
---|
| 121 | for (k=0;k<rows;k++){ |
---|
| 122 | G[j*rows+k] -= ((long)U[j*rows+i]*G[i*rows+k])>>15; |
---|
| 123 | } |
---|
| 124 | |
---|
| 125 | /* printf("end\n"); |
---|
| 126 | UDprt(U,D); |
---|
| 127 | UDprt(G,Dold); |
---|
| 128 | printf("\n"); */ |
---|
| 129 | } |
---|
| 130 | if(i==0) return; |
---|
| 131 | } |
---|
| 132 | } |
---|
| 133 | |
---|
| 134 | void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ){ |
---|
| 135 | long alpha; |
---|
| 136 | long gamma,beta,lambda; |
---|
| 137 | int b[5]; |
---|
| 138 | int *a; // in [0,1] -> q15 |
---|
| 139 | unsigned int iy,j,i; |
---|
| 140 | |
---|
| 141 | for (iy=0; iy<dimy; iy++){ |
---|
| 142 | // a is a row |
---|
| 143 | a = U+iy*dimx; // iyth row of U |
---|
| 144 | for (j=0;j<dimx;j++) |
---|
| 145 | b[j]=((long)D[j]*a[j])>>15; |
---|
| 146 | |
---|
| 147 | alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b |
---|
| 148 | // R in q15, a in q15, b=q15 |
---|
| 149 | gamma = (1<<15)/alpha; //(in q15) |
---|
| 150 | //min alpha = R[iy] = 164 |
---|
| 151 | //max gamma = 0.0061 => gamma_ref = q7 |
---|
| 152 | for (j=0;j<dimx;j++){ |
---|
| 153 | beta = alpha; |
---|
| 154 | alpha += ((long)(a[j])*b[j]>>15); |
---|
| 155 | lambda = -(a[j])*gamma>>15; |
---|
| 156 | gamma = (1<<15)/alpha; // in q15 now |
---|
| 157 | int mpl=((long)beta*gamma); // no-shift, max_gamma=2^15 |
---|
| 158 | D[j] = (long)mpl*D[j]>>15; //gamma is long |
---|
| 159 | if (D[j]==0) D[j]=1; |
---|
| 160 | |
---|
| 161 | // cout << "a: " << alpha << "g: " << gamma << endl; |
---|
| 162 | for (i=0;i<j;i++){ |
---|
| 163 | beta = U[i*dimx+j]; |
---|
| 164 | U[i*dimx+j] += (lambda*b[i])>>15; |
---|
| 165 | b[i] += (beta*b[j])>>15; |
---|
| 166 | } |
---|
| 167 | } |
---|
| 168 | int dzs = (gamma*(difz[iy]))>>15; // apply scaling to innovations |
---|
| 169 | // no shift due to gamma |
---|
| 170 | for (i=0; i<dimx; i++){ |
---|
| 171 | xp[i] += ((long)dzs*b[i])>>15; // multiply by unscaled Kalman gain |
---|
| 172 | } |
---|
| 173 | |
---|
| 174 | //cout << "Ub: " << U << endl; |
---|
| 175 | //cout << "Db: " << D << endl <<endl; |
---|
| 176 | |
---|
| 177 | } |
---|
| 178 | |
---|
| 179 | } |
---|