/************************************
        Extended Kalman Filter
        Matrix operations

        V. Smidl

Rev. 30.8.2010

30.8.2010      Prvni verze

*************************************/

#define qAU 14
#define qCU 15
#define qD 13
#define qCh 14

#define int16 short
#define int32 int

/* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
extern void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns);

/* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
extern void mmultCU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns);

/* Matrix multiply Full matrix by upper diagonal matrix; */
extern void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns);

/* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/
extern void thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);

/* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
extern void bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx );

/* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/
extern void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);

/* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
extern void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx );

/* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
extern void bierman_fastC(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx );

/* perform Householder update of Ch matrix using PSI*Ch , Q, */
extern void householder(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);

/* perform Givens update of Ch matrix using PSI*Ch , Q, */
extern void givens(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);

/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );

/* perform Givens update of Ch matrix using PSI*Ch , Q, */
extern void givens_fast(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);

/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
extern void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );

/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
extern void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx );