| 1 | /************************************ |
|---|
| 2 | Extended Kalman Filter |
|---|
| 3 | Matrix operations |
|---|
| 4 | |
|---|
| 5 | Z. Peroutka |
|---|
| 6 | |
|---|
| 7 | Rev. 15.3.2008 |
|---|
| 8 | |
|---|
| 9 | 15.3. 2008 Kompletni kontrola vypoctu + zamena q15->int a q30->long |
|---|
| 10 | |
|---|
| 11 | *************************************/ |
|---|
| 12 | |
|---|
| 13 | /* Vsechny meze se udavaji ve tvaru (rozmer_matice - 1), abych mel ve FOR konstantni horni mez) */ |
|---|
| 14 | |
|---|
| 15 | /* Matrix addition in q15: m1 + m2 = result[rows, columns] */ |
|---|
| 16 | extern void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns); |
|---|
| 17 | |
|---|
| 18 | /* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */ |
|---|
| 19 | extern void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns); |
|---|
| 20 | |
|---|
| 21 | /* Matrix substraction in q15: m1 - m2 = result[rows, columns] */ |
|---|
| 22 | extern void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns); |
|---|
| 23 | |
|---|
| 24 | /* Matrix multiply in q15: m1[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] */ |
|---|
| 25 | extern void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 26 | columns); |
|---|
| 27 | |
|---|
| 28 | /* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columnsx,columns]transpose) = result[rows,columns] */ |
|---|
| 29 | extern void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 30 | columns); |
|---|
| 31 | |
|---|
| 32 | /* matrix multiplication in q15: sum is in q15 */ |
|---|
| 33 | extern void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 34 | columns); |
|---|
| 35 | |
|---|
| 36 | /* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */ |
|---|
| 37 | extern void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 38 | columns); |
|---|
| 39 | |
|---|
| 40 | /* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */ |
|---|
| 41 | extern void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 42 | columns); |
|---|
| 43 | |
|---|
| 44 | /* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] |
|---|
| 45 | minrowcol=minimum(rows, columnsx) */ |
|---|
| 46 | extern void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 47 | columns, unsigned int minrowcol); |
|---|
| 48 | |
|---|
| 49 | /* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] |
|---|
| 50 | minrowcol=minimum(rows, columnsx) */ |
|---|
| 51 | extern void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 52 | columns, unsigned int minrowcol); |
|---|
| 53 | |
|---|
| 54 | /* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns] |
|---|
| 55 | minrowcol=minimum(columnsx,columns) */ |
|---|
| 56 | extern void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 57 | columns, unsigned int minrowcol); |
|---|
| 58 | |
|---|
| 59 | /* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns] |
|---|
| 60 | minrowcol=minimum(columnsx,columns) */ |
|---|
| 61 | extern void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int |
|---|
| 62 | columns, unsigned int minrowcol); |
|---|
| 63 | |
|---|
| 64 | /* Matrix transposition in q15: m1.' = result[rows, columns] */ |
|---|
| 65 | extern void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns); |
|---|
| 66 | |
|---|
| 67 | /* Matrix [2,2] inversion in q15: inv(m1) = result[rows, columns] */ |
|---|
| 68 | extern void minv2(int *matrix, long *result); |
|---|
| 69 | |
|---|
| 70 | extern void choice_P(int *m, int *result, unsigned int columns); |
|---|
| 71 | extern void choice_x(int *m, int *result); |
|---|
