root/pmsm/simulator_zdenek/ekf_example/matrix.cpp @ 111

Revision 61, 11.3 kB (checked in by smidl, 18 years ago)

Zalozeni fixed EKF jako BM pro PF

Line 
1/************************************
2        Extended Kalman Filter
3        Matrix operations
4
5        Z. Peroutka
6
7Rev. 15.3.2008
8
915.3. 2008      Kompletni kontrola vypoctu + zamena q15->int a q30->long
10
11*************************************/
12
13#include "matrix.h"
14
15/* Vsechny meze se udavaji ve tvaru (rozmer_matice - 1), abych mel ve FOR konstantni horni mez) */
16
17/* Matrix addition in q15: m1 + m2 = result[rows, columns] */
18void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);
19
20/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
21void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns);
22
23/* Matrix substraction in q15: m1 - m2 = result[rows, columns] */
24void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);
25
26/* Matrix multiply in q15: m1[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] */
27void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
28columns);
29
30/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columnsx,columns]transpose) = result[rows,columns] */
31void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
32columns);
33
34/* matrix multiplication in q15: sum is in q15 */
35void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
36columns);
37
38
39/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
40void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
41columns);
42
43
44/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
45void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
46columns);
47
48/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
49minrowcol=minimum(rows, columnsx) */
50void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
51columns, unsigned int minrowcol);
52
53/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
54minrowcol=minimum(rows, columnsx) */
55void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
56columns, unsigned int minrowcol);
57
58/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
59minrowcol=minimum(columnsx,columns) */
60void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
61columns, unsigned int minrowcol);
62
63/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
64minrowcol=minimum(columnsx,columns) */
65void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
66columns, unsigned int minrowcol);
67
68/* Matrix transposition in q15: m1.' = result[rows, columns] */
69void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns);
70
71/* Matrix [2,2] inversion in q15: inv(m1) = result[rows, columns] */
72void minv2(int *matrix, long *result);
73
74void choice_P(int *m, int *result, unsigned int columns);
75void choice_x(int *m, int *result);
76
77
78/* matrix addition in q15 */
79void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns)
80{
81  unsigned int i,j;
82
83  for (i=0; i<=rows; i++)
84    for (j=0; j<=columns; j++)
85      *result++ = *m1++ + *m2++;
86}
87
88
89/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
90void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns)
91{
92  unsigned int i;
93
94  for (i=0; i<=minrowcol; i++)
95/*    *(m1+i*(columns+1)+i) += *(m2+i*(columns+1)+i); */
96  {
97    *m1 += *m2;
98    m1+=(columns+2);
99    m2+=(columns+2);
100  }
101
102}
103
104/* Matrix substraction in q15 */
105void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns)
106{
107  unsigned int i,j;
108
109  for (i=0; i<=rows; i++)
110    for (j=0; j<=columns; j++)
111      *result++ = *m1++ - *m2++;
112}
113
114
115/* matrix multiplication in q15 */
116void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
117columns)
118{
119  unsigned int i, j, k;
120  long tmp_sum=0;
121  int *m2pom;
122
123  for (i=0; i<=rows; i++)
124  {
125    for (j=0; j<=columns; j++)
126    {
127      m2pom=m2+j;
128
129      for (k=0; k<=columnsx; k++)
130      {
131        tmp_sum+=(long)(*m1++)**m2pom;
132        m2pom+=columns+1;
133      }
134
135      // saturation effect
136      tmp_sum=tmp_sum>>15;
137      if (tmp_sum>32767) tmp_sum=32767;
138      if (tmp_sum<-32768) tmp_sum=-32768;
139
140      *result++=tmp_sum;
141
142      tmp_sum=0;
143      m1-=(columnsx+1);
144    }
145    m1+=(columnsx+1);
146  }
147}
148
149
150/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
151void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
152columns)
153{
154  unsigned int i, j, k;
155  long tmp_sum=0;
156  int *m2pom=m2;
157
158  for (i=0; i<=rows; i++)
159  {
160    for (j=0; j<=columns; j++)
161    {
162      for (k=0; k<=columnsx; k++)
163        tmp_sum+=(long)(*m1++)*(*m2pom++);
164
165      // saturation effect
166      tmp_sum=tmp_sum>>15;
167      if (tmp_sum>32767) tmp_sum=32767;
168      if (tmp_sum<-32768) tmp_sum=-32768;
169
170      *result++=tmp_sum;
171
172      tmp_sum=0;
173      m1-=(columnsx+1);
174    }
175    m2pom=m2;
176    m1+=(columnsx+1);
177  }
178}
179
180
181/* matrix multiplication in q15: sum is in q15 */
182void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
183columns)
184{
185  unsigned int i, j, k;
186  long tmp_sum=0;
187  int *m2pom;
188
189  for (i=0; i<=rows; i++)
190  {
191    for (j=0; j<=columns; j++)
192    {
193      m2pom=m2+j;
194
195      for (k=0; k<=columnsx; k++)
196      {
197        tmp_sum+=((long)(*m1++)**m2pom)>>15;
198        m2pom+=columns+1;
199      }
200
201      // saturation effect
202      if (tmp_sum>32767) tmp_sum=32767;
203      if (tmp_sum<-32768) tmp_sum=-32768;
204
205      *result++=tmp_sum;
206
207      tmp_sum=0;
208      m1-=(columnsx+1);
209    }
210    m1+=(columnsx+1);
211  }
212}
213
214
215/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
216void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
217columns)
218{
219  unsigned int i, j, k;
220  long tmp_sum=0;
221  int *m2pom=m2;
222
223  for (i=0; i<=rows; i++)
224  {
225    for (j=0; j<=columns; j++)
226    {
227      for (k=0; k<=columnsx; k++)
228        tmp_sum+=((long)(*m1++)*(*m2pom++))>>15;
229
230      // saturation effect
231      if (tmp_sum>32767) tmp_sum=32767;
232      if (tmp_sum<-32768) tmp_sum=-32768;
233
234      *result++=tmp_sum;
235
236      tmp_sum=0;
237      m1-=(columnsx+1);
238    }
239    m2pom=m2;
240    m1+=(columnsx+1);
241  }
242}
243
244
245/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
246void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
247columns)
248{
249  unsigned int i, j, k;
250  long tmp_sum=0;
251  long *m2pom;
252
253  for (i=0; i<=rows; i++)
254  {
255    for (j=0; j<=columns; j++)
256    {
257      m2pom=m2+j;
258
259      for (k=0; k<=columnsx; k++)
260      {
261        tmp_sum+=(long)(*m1++)**m2pom;
262        m2pom+=columns+1;
263      }
264
265      // saturation effect
266      tmp_sum=tmp_sum>>15;
267      if (tmp_sum>32767) tmp_sum=32767;
268      if (tmp_sum<-32768) tmp_sum=-32768;
269
270      *result++=tmp_sum;
271
272      tmp_sum=0;
273      m1-=(columnsx+1);
274    }
275    m1+=(columnsx+1);
276  }
277}
278 
279
280/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
281minrowcol=minimum(rows, columnsx) */
282void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
283columns, unsigned int minrowcol)
284{
285  unsigned int i,j;
286/*  int *res_tmp=result; */
287 
288/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */
289/*  for (i=0;i<=rows;i++)       
290    for (j=0;j<=columns;j++)
291      *res_tmp++=0;
292/* END OF ERASE SEQUENCE */
293
294  for (i=0;i<=minrowcol;i++)
295    for (j=0;j<=columns;j++)
296      *result++=*m2++;
297}
298
299
300/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
301minrowcol=minimum(rows, columnsx) */
302void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
303columns, unsigned int minrowcol)
304{
305  unsigned int i,j;
306/*  int *res_tmp=result; */
307
308/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */ 
309/*  for (i=0;i<=rows;i++)               
310    for (j=0;j<=columns;j++)
311      *res_tmp++=0;
312/* END OF ERASE SEQUENCE */
313
314  for (i=0;i<=minrowcol;i++)
315  {
316    for (j=0;j<=columns;j++)
317      *result++=((long)(*DIAG)*(*m2++))>>15;
318   
319    DIAG+=(columnsx+1);
320  }
321}
322
323
324/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
325minrowcol=minimum(columnsx,columns) */
326void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
327columns, unsigned int minrowcol)
328{
329  unsigned int i,j;
330/*  int *res_tmp=result; */
331
332/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */
333/*  for (i=0;i<=rows;i++)
334    for (j=0;j<=columns;j++)
335      *res_tmp++=0;
336/* END OF ERASE SEQUENCE */
337
338  for (i=0;i<=rows;i++)
339  {
340    for (j=0;j<=minrowcol;j++)
341      *result++=*m1++;
342   
343    m1+=(columnsx-minrowcol);
344    result+=(columns-minrowcol);
345  }
346} 
347
348
349
350/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
351minrowcol=minimum(columnsx,columns) */
352void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
353columns, unsigned int minrowcol)
354{
355  unsigned int i,j;
356  int *DIAG_tmp=DIAG;
357/*  int *res_tmp=result; */
358
359/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */ 
360/*  for (i=0;i<=rows;i++)               
361    for (j=0;j<=columns;j++)
362      *res_tmp++=0;
363/* END OF ERASE SEQUENCE */
364
365  for (i=0;i<=rows;i++)
366  {
367    for (j=0;j<=minrowcol;j++)
368    {
369      *result++=((long)(*m1++)**DIAG_tmp)>>15;
370      DIAG_tmp+=columns+2;
371    }
372     
373    DIAG_tmp=DIAG;
374    m1+=(columnsx-minrowcol);
375    result+=(columns-minrowcol);
376  }
377} 
378
379
380/* matrix transposition - neni optimalizovana, protoze se nepouziva */
381void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns)
382{
383  unsigned int i, j;
384 
385  for (i=0; i<=rows; i++)
386    for (j=0; j<=columns; j++)
387      *result++=*(m1+(columns+1)*j+i);
388}
389
390/* matrix [2,2] inversion */
391void minv2(int *matrix, long *result)
392{
393  int det;
394  unsigned int i;
395
396  det=((long)*matrix**(matrix+3)-(long)*(matrix+1)**(matrix+2))>>15;
397  if (det==0) det=1;
398
399  *result++=(((long)*(matrix+3))<<15)/det;
400  *result++=(((long)*(matrix+1))<<15)/det;
401  *result++=(((long)*(matrix+2))<<15)/det;
402  *result++=(((long)*matrix)<<15)/det;
403}
404
405
406void choice_P(int *m, int *result, unsigned int columns)
407{
408  *result++=*m;
409  *result++=*(m+1);
410  *result++=*(m+columns+1);
411  *result++=*(m+columns+2);
412}
413
414
415void choice_x(int *m, int *result)
416{
417  *result++=*m++;
418  *result=*m;
419}
Note: See TracBrowser for help on using the browser.