/*!
  \file
  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y

  Using IT++ for numerical operations
  -----------------------------------
*/

#ifndef EKF_mm_H
#define EKF_mm_H

#include "fixed.h"
#include "matrix.h"
#include "matrix_vs.h"
#include "reference_Q15.h"
#include "parametry_motoru.h"

struct ekf_data{
	int16 Q[4]; /* matrix [4,4] */
	int16 dR[2]; /* diag of matrix [2,2] */
	
	int16 x_est[2]; /* estimate and prediction */
	int16 x_pred[2]; /* estimate and prediction */
	int16 y_est[2]; /* estimate and prediction */
	int16 y_old[2]; /* estimate and prediction */
	
	int16 PSI[4]; /* matrix [4,4] */
	int16 PSICh[4]; /* matrix PIS*U, [4,4] */
	int16 C[4]; /* matrix [4,4] */
	
	int16 Chf[4]; // upper triangular of covariance (inplace)
	
	int16 difz[2];
	int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
};


void init_ekfCh2( ekf_data *E, double Tv);
void ekfCh2(ekf_data *E, int16 ux, int16 uy, int16 isx, int16 isy, int32 *detS, int16 *rem);

void ekfmm(ekf_data *E1, ekf_data *E2, int16  ux, int16 uy, int16 isx, int16 isy);


#endif // KF_H