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

Revision 1180, 8.6 kB (checked in by smidl, 14 years ago)

remove printouts

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/* Matrix multiply Full matrix by upper diagonal matrix; */
15void 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
48void 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++) {
57        printf("%d,",D[i]);
58    }
59    printf("\n");
60}
61
62// Thorton procedure - Kalman predictive variance in UD
63void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
64    unsigned int i,j,k;
65    // copy D to Dold
66    int *Dold_pom=Dold;
67    for (i=0;i<rows;i++) {
68        *Dold_pom++=*D++;
69    }
70    D-=rows; // back to first D
71
72    // initialize G = eye()
73    int *G_pom = G;
74    *G_pom++=1<<14;
75    for (i=0;i<rows-1;i++) {
76        // clean elem before diag
77        for (j=0; j<rows; j++) {
78            *G_pom++=0.0;
79        }
80        *G_pom++=1<<14;
81    }
82    // eye created
83
84    long sigma; // in q15
85    for (i=rows-1; i>=0;i--) { // check i==0 at the END!
86        sigma = 0;
87               
88        for (j=0;j<rows; j++) {
89            sigma += (((long)PSIU[i*rows+j]*PSIU[i*rows+j])>>15)*(Dold[i]);
90                }
91                sigma += Q[i*rows+i];
92        for (j=i+1;j<rows; j++) {
93            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
94                }
95               
96        *(D+i)=sigma>>15;
97        if (D[i]==0) D[i]=1;
98
99        /*              printf("d=sig\n");
100                        UDprt(U,D);
101                        UDprt(G,Dold);*/
102
103        for (j=0;j<i;j++) {
104//                      printf("\n%d,%d\n",i,j);
105            sigma =0;
106            for (k=0;k<rows;k++) {
107                sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k];
108                   }
109                        for (k=0;k<rows;k++) {
110                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
111            }
112                        long z=sigma/D[i]; // shift by 15
113            if (z>32767) z=32767;
114            if (z<-32768) z=-32768;
115
116            U[j*rows+i] = (int)z;
117
118            /*                  printf("U=sig/D\n");
119                                UDprt(U,D);
120                                UDprt(G,Dold);*/
121
122            for (k=0;k<rows;k++) {
123                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15;
124            }
125                       
126            for (k=0;k<rows;k++) {
127                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
128            }
129                       
130            /*                  printf("end\n");
131                                UDprt(U,D);
132                                UDprt(G,Dold);
133                                printf("\n");   */
134        }
135        if (i==0) return;
136    }
137}
138
139void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
140    int alpha;
141    int beta,lambda;
142    int b[5]; // ok even for 4-dim state
143    int *a; // in [0,1] -> q15
144    unsigned int iy,j,i;
145
146    int *b_j,*b_i;
147    int *a_j;
148    int *D_j;
149    int *U_ij;
150    int *x_i;
151    a = U; // iyth row of U
152    for (iy=0; iy<dimy; iy++, a+=dimx) {
153        // a is a row
154        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
155            *b_j=((long)(*D_j)*(*a_j))>>15;
156
157        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
158        // R in q15, a in q15, b=q15
159//              gamma = (1<<15)/alpha; //(in q15)
160        //min alpha = R[iy] = 164
161        //max gamma = 0.0061 => gamma_ref = q7
162        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
163            beta   = alpha;
164            lambda = -((long)(*a_j)<<15)/beta;
165            alpha  += ((long)(*a_j)*(*b_j)>>15);
166            D[j] = (((long)beta<<15)/alpha)*(*D_j)>>15; //gamma is long
167            if (*D_j==0) *D_j=1;
168
169            for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
170                beta   = *U_ij;
171                *U_ij +=  (lambda*(*b_i))>>15;
172                *b_i  +=  ((long)beta*(*b_j))>>15;
173            }
174        }
175        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
176        // no shift due to gamma
177        for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
178            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain
179        }
180
181        //cout << "Ub: " << U << endl;
182        //cout << "Db: " << D << endl <<endl;
183
184    }
185
186}
187
188// Thorton procedure - Kalman predictive variance in UD
189void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
190    unsigned int i,j,k;
191    // copy D to Dold
192    int *Dold_i,*Dold_k;
193    int *D_i;
194    int *PSIU_ij,*PSIU_ik,*PSIU_jk;
195    int *Q_jj,*Q_ii,*Q_kk;
196    int *U_ji;
197    int *G_ik,*G_jk;
198    int irows,jrows;
199    long sigma; // in q15
200
201    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
202        *Dold_i=*D_i;
203    }
204
205    // initialize G = eye()
206    G_ik= G;
207    *G_ik++=1<<14;
208    for (i=0;i<rows-1;i++) {
209        // clean elem before diag
210        for (k=0; k<rows; k++) {
211            *G_ik++=0;
212        }
213        *G_ik++=1<<14;
214    }
215    // eye created
216
217    for (i=rows-1, Dold_i=Dold+i, D_i=D+i;
218            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
219        irows=i*rows;
220        sigma = 0;
221        for (k=0, PSIU_ik=PSIU+irows;
222           k<rows; k++, PSIU_ik++) {//Dold_i=
223            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_i);
224                }
225                sigma += *(Q+i+irows);
226        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
227            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);
228                       
229        }
230               
231        *D_i=sigma>>15;
232        if (*D_i==0) *D_i=1;
233
234
235        for (j=0;j<i;j++) {
236            jrows = j*rows;
237
238            sigma =0;
239            for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold;
240                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
241                               
242                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
243            }
244                       
245            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
246            k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
247                sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk;
248            }
249                       
250                        long z=sigma/(*D_i); // shift by 15
251            if (z>32767) z=32767;
252            if (z<-32768) z=-32768;
253
254            U_ji=U+jrows+i;
255            *U_ji = (int)z;
256
257
258            for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows;
259                    k<rows;k++,PSIU_ik++,PSIU_jk++) {
260                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
261            }
262                       
263            for (k=0,G_jk=G+jrows,G_ik=G+irows;
264            k<rows;k++, G_jk++, G_ik++) {
265                *G_jk -=  ((long)*U_ji**G_ik)>>15;
266            }
267                       
268        }
269        if (i==0) return;
270    }
271}
272
273void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
274    long alpha;
275    long gamma,beta,lambda;
276    int b[5]; // ok even for 4-dim state
277    int *a; // in [0,1] -> q15
278    unsigned int iy,j,i;
279
280    for (iy=0; iy<dimy; iy++) {
281        // a is a row
282        a = U+iy*dimx; // iyth row of U
283        for (j=0;j<dimx;j++)
284            b[j]=((long)D[j]*a[j])>>15;
285
286        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
287        // R in q15, a in q15, b=q15
288//              gamma = (1<<15)/alpha; //(in q15)
289        //min alpha = R[iy] = 164
290        //max gamma = 0.0061 => gamma_ref = q7
291        for (j=0;j<dimx;j++) {
292            beta   = alpha;
293            lambda = -((long)a[j]<<15)/beta;
294            alpha  += ((long)(a[j])*b[j]>>15);
295            D[j] = (((long)beta<<15)/alpha)*D[j]>>15; //gamma is long
296            if (D[j]==0) D[j]=1;
297
298            //                  cout << "a: " << alpha << "g: " << gamma << endl;
299            for (i=0;i<j;i++) {
300                beta   = U[i*dimx+j];
301                U[i*dimx+j] +=  (lambda*b[i])>>15;
302                b[i]   +=  (beta*b[j])>>15;
303            }
304        }
305        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
306        // no shift due to gamma
307        for (i=0; i<dimx; i++) {
308            xp[i]  += ((long)dzs*b[i])>>15; // multiply by unscaled Kalman gain
309        }
310
311        //cout << "Ub: " << U << endl;
312        //cout << "Db: " << D << endl <<endl;
313
314    }
315
316}
Note: See TracBrowser for help on using the browser.