root/applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp @ 1228

Revision 1228, 13.0 kB (checked in by smidl, 14 years ago)

UD hopefully correct

Line 
1/************************************
2        Extended Kalman Filter
3        Matrix operations
4
5        V. Smidl
6
7Rev. 30.8.2010
8
930.8.2010      Prvni verze
10
11*************************************/
12#include "fixed.h"
13#include "stdio.h"
14#include <math.h>
15
16#include "matrix_vs.h"
17
18/* Matrix multiply Full matrix by upper diagonal matrix; */
19void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
20    unsigned int i, j, k;
21    long tmp_sum=0L; //in 15+qAU
22    int *m2pom;
23    int *m1pom=m1;
24    int *respom=result;
25
26    for (i=0; i<rows; i++) //rows of result
27    {
28        for (j=0; j<columns; j++) //columns of result
29        {
30            m2pom=up+j;//??
31
32            for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
33            {
34                tmp_sum+=((long)(*(m1pom++))**m2pom)>>(15-qAU);
35                m2pom+=columns;
36            }
37            // add the missing A(i,j)
38            tmp_sum +=(long)(*m1pom)<<qAU; // no need to shift
39            m1pom-=(j); // shift back to first element
40
41            // saturation effect
42            tmp_sum=tmp_sum>>15;
43            if (tmp_sum>32767) {
44                //tmp_sum=32767;
45            }
46            if (tmp_sum<-32768) {
47                //tmp_sum=-32768;
48            }
49//                      printf("Au - saturated\n");
50
51            *respom++=tmp_sum;
52
53            tmp_sum=0;
54        }
55        m1pom+=(columns);
56    }
57};
58
59/* Matrix multiply Full matrix by upper diagonal matrix; */
60void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
61    unsigned int i, j, k;
62    long tmp_sum=0L;
63    int *m2pom;
64    int *m1pom=m1;
65    int *respom=result;
66
67    for (i=0; i<rows; i++) //rows of result
68    {
69        for (j=0; j<columns; j++) //columns of result
70        {
71            m2pom=up+j;//??
72
73            for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1;
74            {
75                tmp_sum+=(long)(*(m1pom++))**m2pom;
76                m2pom+=columns;
77            }
78            m1pom-=(j+1); // shift back to first element
79
80            // saturation effect
81            tmp_sum=tmp_sum>>15;
82            if (tmp_sum>32767) {
83                if (i!=3) tmp_sum=32767;
84            }
85            if (tmp_sum<-32768) {
86                tmp_sum=-32768;
87            }
88            //                  printf("Au - saturated\n");
89
90            *respom++=tmp_sum;
91
92            tmp_sum=0;
93        }
94        m1pom+=(columns);
95    }
96};
97
98bool DBG=true;
99
100void show(const char name[10], int *I, int n) {
101    if (!DBG) return;
102
103    printf("%s: ",name);
104    for (int i=0;i<n;i++) {
105        printf("%d ",*(I+i));
106    }
107    printf("\n");
108}
109
110// Thorton procedure - Kalman predictive variance in UD
111void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
112    unsigned int i,j,k;
113    // copy D to Dold
114    int *Dold_pom=Dold;
115    for (i=0;i<rows;i++) {
116        *Dold_pom++=*D++;
117    }
118    D-=rows; // back to first D
119
120    // initialize G = eye()
121    int *G_pom = G;
122    *G_pom++=1<<14;
123    for (i=0;i<rows-1;i++) {
124        // clean elem before diag
125        for (j=0; j<rows; j++) {
126            *G_pom++=0.0;
127        }
128        *G_pom++=1<<14;
129    }
130    // eye created
131
132    long sigma; // in q30!!!!!!
133    for (i=rows-1; true;i--) { // check i==0 at the END!
134        sigma = 0;
135
136        for (j=0;j<rows; j++) {
137            //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
138            long s2=((((long)PSIU[i*rows+j]*PSIU[i*rows+j]))>>(2*qAU-15))*Dold[j];
139//                      printf("%d - %d\n",s1,s2);
140            sigma += s2;
141        }
142        sigma += Q[i*rows+i]<<15;
143        for (j=i+1;j<rows; j++) {
144            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
145//                      sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
146        }
147
148//        if (sigma>16384<<15) sigma = 16384<<15;
149        *(D+i)=sigma>>15;
150        if (D[i]==0) D[i]=1;
151//show("D",D,5);
152
153        for (j=0;j<i;j++) {
154//                      printf("\n%d,%d\n",i,j);
155            sigma =0;
156            for (k=0;k<rows;k++) {
157                                sigma += (((long(PSIU[i*rows+k])*PSIU[j*rows+k]))>>(2*qAU-15))*Dold[k];
158            }
159            for (k=0;k<rows;k++) {
160                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
161            }
162            long z=sigma/D[i]; // shift by 15
163            if (z>32767) z=32767;
164            if (z<-32768) z=-32768;
165
166            U[j*rows+i] = (int)z;
167
168
169            for (k=0;k<rows;k++) {
170                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; //qAU*q15/q15=qAU
171            }
172
173            for (k=0;k<rows;k++) {
174                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
175            }
176
177        }
178        //show("U",U,25);
179        //show("G",G,25);
180        if (i==0) return;
181    }
182}
183
184void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
185    int alpha;
186    int beta,lambda;
187    int b[5]; // ok even for 4-dim state
188    int *a; // in [0,1] -> q15
189    unsigned int iy,j,i;
190
191    int *b_j,*b_i;
192    int *a_j;
193    int *D_j;
194    int *U_ij;
195    int *x_i;
196    a = U; // iyth row of U
197    for (iy=0; iy<dimy; iy++, a+=dimx) {
198        // a is a row
199        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
200            *b_j=((long)(*D_j)*(*a_j))>>15;
201
202        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
203        // R in q15, a in q15, b=q15
204//              gamma = (1<<15)/alpha; //(in q15)
205        //min alpha = R[iy] = 164
206        //max gamma = 0.0061 => gamma_ref = q7
207        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
208            beta   = alpha;
209            lambda = -((long)(*a_j)<<15)/beta;
210            alpha  += ((long)(*a_j)*(*b_j))>>15;
211            D[j] = ((((long)beta<<15)/alpha)*(*D_j))>>15; //gamma is long
212            if (*D_j==0) *D_j=1;
213
214            for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
215                beta   = *U_ij;
216                *U_ij +=  ((long)lambda*(*b_i))>>15;
217                *b_i  +=  ((long)beta*(*b_j))>>15;
218            }
219        }
220        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
221        // no shift due to gamma
222        for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
223            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain
224        }
225
226        //cout << "Ub: " << U << endl;
227        //cout << "Db: " << D << endl <<endl;
228
229    }
230
231}
232
233// Thorton procedure - Kalman predictive variance in UD
234void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
235    unsigned int i,j,k;
236    // copy D to Dold
237    int *Dold_i,*Dold_k;
238    int *D_i;
239    int *PSIU_ij,*PSIU_ik,*PSIU_jk;
240    int *Q_jj,*Q_ii,*Q_kk;
241    int *U_ji;
242    int *G_ik,*G_jk;
243    int irows,jrows;
244    long sigma; // in q30!!
245
246    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
247        *Dold_i=*D_i;
248    }
249
250    // initialize G = eye()
251    G_ik= G;
252    *G_ik++=1<<14;
253    for (i=0;i<rows-1;i++) {
254        // clean elem before diag
255        for (k=0; k<rows; k++) {
256            *G_ik++=0;
257        }
258        *G_ik++=1<<14;
259    }
260    // eye created
261
262    for (i=rows-1, Dold_i=Dold+i, D_i=D+i;
263            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
264        irows=i*rows;
265        sigma = 0;
266        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)**PSIU_ik)>>15)*(*Dold_k);
269        }
270        sigma += *(Q+i+irows)<<15;
271        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
272            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);
273
274        }
275
276        *D_i=sigma>>15;
277        if (*D_i==0) *D_i=1;
278
279
280        for (j=0;j<i;j++) {
281            jrows = j*rows;
282
283            sigma =0;
284            for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold;
285                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
286
287                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
288            }
289
290            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
291                    k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
292                sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk;
293            }
294
295            long z=sigma/(*D_i); // shift by 15
296            if (z>32767) z=32767;
297            if (z<-32768) z=-32768;
298
299            U_ji=U+jrows+i;
300            *U_ji = (int)z;
301
302
303            for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows;
304                    k<rows;k++,PSIU_ik++,PSIU_jk++) {
305                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
306            }
307
308            for (k=0,G_jk=G+jrows,G_ik=G+irows;
309                    k<rows;k++, G_jk++, G_ik++) {
310                *G_jk -=  ((long)*U_ji**G_ik)>>15;
311            }
312
313        }
314        if (i==0) return;
315    }
316}
317
318void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
319    long alpha;
320    long gamma,beta,lambda;
321    int b[5]; // ok even for 4-dim state
322    int *a; // in [0,1] -> q15
323    unsigned int iy,j,i;
324
325    for (iy=0; iy<dimy; iy++) {
326        // a is a row
327        a = U+iy*dimx; // iyth row of U
328        for (j=0;j<dimx;j++) {
329            (j<iy)? b[j]=0: (j==iy)? b[j]=D[j] : b[j]=((long)D[j]*a[j])>>15;
330        }
331
332        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
333        // R in q15, a in q15, b=q15
334//              gamma = (1<<15)/alpha; //(in q15)
335        //min alpha = R[iy] = 164
336        //max gamma = 0.0061 => gamma_ref = q7
337        for (j=0;j<dimx;j++) {
338            beta   = alpha;
339            lambda = -(((long)a[j])<<15)/beta;
340            alpha  += (((long)(a[j])*b[j])>>15);
341            D[j] = (((((long)beta)<<15)/alpha)*D[j])>>15; //gamma is long
342            if (D[j]==0) D[j]=1;
343
344            //                  cout << "a: " << alpha << "g: " << gamma << endl;
345            for (i=0;i<j;i++) {
346                beta   = U[i*dimx+j];
347                U[i*dimx+j] +=  (lambda*b[i])>>15;
348                b[i]   +=  (beta*b[j])>>15;
349            }
350        }
351        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
352        // no shift due to gamma
353        for (i=0; i<dimx; i++) {
354            xp[i]  += ((long)dzs*b[i])>>15; // multiply by unscaled Kalman gain
355        }
356
357        //cout << "Ub: " << U << endl;
358        //cout << "Db: " << D << endl <<endl;
359
360    }
361
362}
363
364/* square root of 0<a<1 using taylor at 0.5 in q15*/
365int int_sqrt(int x) {
366    //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2
367    //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5);
368#define k1 23170 //0.5*sqrt(2)*32768
369#define k2 11585 //0.25*sqrt(2)*32768
370
371    int tmp;
372    if (x>6554) {
373        int xm05=x-16384;
374        tmp = ((long)k1*xm05)>>15;
375        tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
376        tmp +=k1;
377    } else {
378        tmp = 4*x;
379        tmp-=long(8*x)*x>>15;
380    }
381    return tmp;
382}
383
384void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx) {
385    int k,j,i;
386    int sigma,alpha,beta;
387    int B[25];//beware
388    int w[5];
389    int v[5];
390
391    // copy Q to B
392    for (i=0;i<dimx*dimx;i++) {
393        B[i]=Q[i];
394    }
395
396    for (k=dimx-1; k>=0; k--) {
397        sigma=0;
398        for (j=0;j<dimx;j++) {
399            sigma+=(long(B[k*dimx+j])*B[k*dimx+j])>>15;
400        }
401        for (j=0;j<=k;j++) {
402            sigma+=(long(Ch[k*dimx+j])*Ch[k*dimx+j])>>15;
403        }
404        /*               double sigf=double(sigma)/(1<<15);
405                         double alpf = sqrt(sigf);*/
406        alpha=int_sqrt(sigma);
407        // alpha = alpf*(1<<15);
408        //
409        sigma=0;
410        for (j=0;j<dimx;j++) {
411            w[j]=B[k*dimx+j];
412            sigma+=(long(w[j])*w[j])>>15;
413        }
414        for (j=0; j<=k;j++) {
415            if (j==k) {
416                v[j]=Ch[k*dimx+j]-alpha;
417            } else {
418                v[j]=Ch[k*dimx+j];
419            }
420            sigma+=(long(v[j])*v[j])>>15;
421        }
422        alpha=sigma>>1;
423        for (i=0;i<=k;i++) {
424            sigma=0;
425            for (j=0;j<dimx;j++) {
426                sigma+=(long(B[i*dimx+j])*w[j])>>15;
427            }
428            for (j=0;j<=k;j++) {
429                sigma+=(long(Ch[i*dimx+j])*v[j])>>15;
430            }
431            for (j=0;j<dimx;j++) {
432                B[i*dimx+j]-=(long(sigma)*w[j]/alpha);
433            };
434            for (j=0;j<=k;j++) {
435                Ch[i*dimx+j]-=(long(sigma)*v[j]/alpha);
436            }
437        }
438    }
439
440}
441
442void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx ) {
443    int alpha,beta,gamma;
444    int delta, eta,epsilon,zeta,sigma,tau;
445    int i,j,iy;
446    int w[5];
447
448    for (iy=0; iy<dimy; iy++) {
449        alpha=R[iy];
450        delta = difz[iy];
451
452        for (j=0;j<dimx;j++) {
453            sigma=Ch[iy*dimx+j];
454            beta=alpha;
455            alpha+=(long(sigma)*sigma)>>15;
456//                      double ab=(double)alpha*beta/32768./32768.;
457//                      double s_ab=sqrt(ab);
458            gamma=int_sqrt(((long)alpha*beta)>>15);
459            //gamma = round(s_ab*(1<<15));
460            eta=(long (beta)<<15) / gamma;
461            //zeta=(long(sigma)<<15)/ gamma;
462            w[j]=0;
463            for (i=0;i<=j;i++) {
464                tau=Ch[i*dimx+j];
465                Ch[i*dimx+j]=((long(eta)*Ch[i*dimx+j])>>15) -(long(sigma)*w[i])/gamma;
466                w[i]+=(long(tau)*sigma)>>15;
467            }
468        }
469
470        //epsilon=(long(difz)<<15) / (alpha); // q15*q13/q13 = q15
471        for (i=0;i<dimx;i++) {
472            xp[i]+=(long(w[i])*delta)/alpha;
473        }
474    }
475}
Note: See TracBrowser for help on using the browser.