| [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 | *************************************/ |
|---|
| [1225] | 12 | |
|---|
| [1228] | 13 | #define qAU 14 |
|---|
| [1313] | 14 | #define qCU 15 |
|---|
| [1340] | 15 | #define qD 13 |
|---|
| [1245] | 16 | #define qCh 14 |
|---|
| [1225] | 17 | |
|---|
| [1329] | 18 | #define int16 short |
|---|
| 19 | #define int32 int |
|---|
| [1240] | 20 | |
|---|
| [1225] | 21 | /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */ |
|---|
| [1240] | 22 | extern void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns); |
|---|
| [1174] | 23 | |
|---|
| [1313] | 24 | /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */ |
|---|
| 25 | extern void mmultCU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns); |
|---|
| 26 | |
|---|
| [1225] | 27 | /* Matrix multiply Full matrix by upper diagonal matrix; */ |
|---|
| [1240] | 28 | extern void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns); |
|---|
| [1225] | 29 | |
|---|
| [1174] | 30 | /* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/ |
|---|
| [1240] | 31 | extern void thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); |
|---|
| [1174] | 32 | |
|---|
| 33 | /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/ |
|---|
| [1240] | 34 | extern void bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); |
|---|
| [1179] | 35 | |
|---|
| 36 | /* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/ |
|---|
| [1240] | 37 | extern void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); |
|---|
| [1179] | 38 | |
|---|
| 39 | /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/ |
|---|
| [1240] | 40 | extern void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); |
|---|
| [1225] | 41 | |
|---|
| [1313] | 42 | /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/ |
|---|
| 43 | extern void bierman_fastC(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); |
|---|
| 44 | |
|---|
| [1225] | 45 | /* perform Householder update of Ch matrix using PSI*Ch , Q, */ |
|---|
| [1240] | 46 | extern void householder(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx); |
|---|
| [1225] | 47 | |
|---|
| [1230] | 48 | /* perform Givens update of Ch matrix using PSI*Ch , Q, */ |
|---|
| [1240] | 49 | extern void givens(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx); |
|---|
| [1230] | 50 | |
|---|
| [1225] | 51 | /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ |
|---|
| [1240] | 52 | extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); |
|---|
| [1294] | 53 | |
|---|
| 54 | /* perform Givens update of Ch matrix using PSI*Ch , Q, */ |
|---|
| 55 | extern void givens_fast(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx); |
|---|
| 56 | |
|---|
| 57 | /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ |
|---|
| 58 | extern void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); |
|---|
| [1321] | 59 | |
|---|
| 60 | /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ |
|---|
| 61 | extern void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); |
|---|
