root/applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h @ 1168

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

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

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

#ifndef EKFfix_H
#define EKFfix_H


#include <estim/kalman.h>
#include "fixed.h"
#include "matrix.h"
#include "reference.h"
#include "parametry_motoru.h"

using namespace bdm;

double minQ(double Q);

/*!
\brief Extended Kalman Filter with full matrices in fixed point arithmetic

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/
class EKFfixed : public BM {
public:
void init_ekf(double Tv);
void ekf(double ux, double uy, double isxd, double isyd);

/* Declaration of local functions */
void prediction(int *ux);
void correction(void);
void update_psi(void);

/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
 int Q[16]; /* matrix [4,4] */
 int R[4]; /* matrix [2,2] */

 int x_est[4];
 int x_pred[4];
 int P_pred[16]; /* matrix [4,4] */
 int P_est[16]; /* matrix [4,4] */
 int Y_mes[2];
 int ukalm[2];
 int Kalm[8]; /* matrix [5,2] */

 int PSI[16]; /* matrix [4,4] */

 int temp15a[16];

 int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane

 long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
 enorm<fsqmat> E;
 mat Ry;
  
public:
        //! Default constructor
        EKFfixed ():BM(),E(),Ry(2,2){
                int i;
                for(i=0;i<16;i++){Q[i]=0;}
                for(i=0;i<4;i++){R[i]=0;}

                for(i=0;i<4;i++){x_est[i]=0;}
                for(i=0;i<4;i++){x_pred[i]=0;}
                for(i=0;i<16;i++){P_pred[i]=0;}
                for(i=0;i<16;i++){P_est[i]=0;}
                P_est[0]=0x7FFF;
                P_est[5]=0x7FFF;
                P_est[10]=0x7FFF;
                P_est[15]=0x7FFF;
                for(i=0;i<2;i++){Y_mes[i]=0;}
                for(i=0;i<2;i++){ukalm[i]=0;}
                for(i=0;i<8;i++){Kalm[i]=0;}

                for(i=0;i<16;i++){PSI[i]=0;}

                set_dim(4);
                E._mu()=zeros(4);
                E._R()=zeros(4,4);
                init_ekf(0.000125);
        };
        //! Here dt = [yt;ut] of appropriate dimensions
        void bayes ( const vec &yt, const vec &ut );
        //!dummy!
        const epdf& posterior() const {return E;};
        
};

UIREGISTER(EKFfixed);

#endif // KF_H