- Main Page
- Related Pages
- Classes
- Files

# work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

Go to the documentation of this file.

    00001
    00013 #ifndef EKFfix_H
    00014 #define EKFfix_H
    00015
    00016 #include <itpp/itbase.h>
    00017 #include <estim/libKF.h>
    00018 #include "fixed.h"
    00019 #include "matrix.h"
    00020 #include "reference.h"
    00021 #include "parametry_motoru.h"
    00022
    00023 using namespace itpp;
    00024
    00025 double minQ(double Q);
    00026
    00032 class EKFfixed : public BM, public BMcond {
    00033 public:
    00034 void init_ekf(double Tv);
    00035 void ekf(double ux, double uy, double isxd, double isyd);
    00036
    00037 /* Declaration of local functions */
    00038 void prediction(int *ux);
    00039 void correction(void);
    00040 void update_psi(void);
    00041
    00042 /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
    00043  int Q[16]; /* matrix [4,4] */
    00044  int R[4]; /* matrix [2,2] */
    00045
    00046  int x_est[4];
    00047  int x_pred[4];
    00048  int P_pred[16]; /* matrix [4,4] */
    00049  int P_est[16]; /* matrix [4,4] */
    00050  int Y_mes[2];
    00051  int ukalm[2];
    00052  int Kalm[8]; /* matrix [5,2] */
    00053
    00054  int PSI[16]; /* matrix [4,4] */
    00055
    00056  int temp15a[16];
    00057
    00058  int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
    00059
    00060  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
    00061  enorm<fsqmat> E;
    00062  mat Ry;
    00063
    00064 public:
    00066         EKFfixed ( RV rvx,RV rvc ):BM(rvx),BMcond(rvc),E(rvx),Ry(2,2){
    00067         int i;
    00068  for(i=0;i<16;i++){Q[i]=0;}
    00069  for(i=0;i<4;i++){R[i]=0;}
    00070
    00071  for(i=0;i<4;i++){x_est[i]=0;}
    00072  for(i=0;i<4;i++){x_pred[i]=0;}
    00073  for(i=0;i<16;i++){P_pred[i]=0;}
    00074  for(i=0;i<16;i++){P_est[i]=0;}
    00075  P_est[0]=0x7FFF;
    00076  P_est[5]=0x7FFF;
    00077  P_est[10]=0x7FFF;
    00078  P_est[15]=0x7FFF;
    00079  for(i=0;i<2;i++){Y_mes[i]=0;}
    00080  for(i=0;i<2;i++){ukalm[i]=0;}
    00081  for(i=0;i<8;i++){Kalm[i]=0;}
    00082
    00083  for(i=0;i<16;i++){PSI[i]=0;}
    00084 };
    00086         void bayes ( const vec &dt );
    00088         epdf& _epdf(){return E;};
    00089         void condition ( const vec &Q0 ) {
    00090
    00091                 Q[0]=prevod(minQ(Q0(0)),15);       // 0.05
    00092                 Q[5]=prevod(minQ(Q0(1)),15);
    00093                 Q[10]=prevod(minQ(Q0(2)),15);      // 1e-3
    00094                 Q[15]=prevod(minQ(Q0(3)),15);      // 1e-3
    00095
    00096         }
    00097 };
    00098
    00099
    00100 #endif // KF_H
    00101
    00102