root/applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.cpp

#include "ekf_mm.h"
#include "qmath.h"
#include "stdio.h"



void ekfCh2(ekf_data *E, int16 ux, int16 uy, int16 isx, int16 isy, int32 *detS, int16 *rem)
{
        int16 t_sin,t_cos;
        int32 tmp;
        
        //x_est[0]=x_est[0];
        //               q15              + q15*q13
        (E->x_est)[1]=(((int32)(E->x_est[1])<<15)+(int32)(E->cG)*(E->x_est[0]))>>15;
        E->x_pred[1]=E->x_est[1];
        E->x_pred[0]=E->x_est[0];
        
        mmultACh(E->PSI,E->Chf,E->PSICh,2,2);
        
        givens_fast(E->PSICh,E->Q,2); // pracuje inplace
        for (int i=0;i<4;i++) E->Chf[i]=E->PSICh[i];    
        
        // implementace v PC
        t_sin=qsin(E->x_est[1]);
        t_cos=qcos(E->x_est[1]);
        
        {       
                E->C[0]=((int32)(E->cB)*t_sin)>>15;
                tmp=((int32)(E->cH)*(E->x_est[0]))>>15; // ! cH =cB with different scale!!
                E->C[1]=((int32)tmp*t_cos)>>15;
                E->C[2]=-((int32)(E->cB)*t_cos)>>15;
                E->C[3]=((int32)tmp*t_sin)>>15;
        }
        
        
        tmp=((int32)(E->cB)*(E->x_est[0]))>>15; // q15*q13 -> q13
        //             q15*q13 +          q13*q15      + q15*q13??
        E->y_est[0]=((int32)(E->cA)*(E->y_old[0])+(int32)tmp*t_sin+(int32)(E->cC)*(ux))>>15; // in q13
        E->y_est[1]=((int32)(E->cA)*(E->y_old[1])-(int32)tmp*t_cos+(int32)(E->cC)*(uy))>>15; // q13
        
        E->difz[0]=(isx-(E->y_est[0])); // shift to q15!!
        E->difz[1]=(isy-(E->y_est[1]));//<<2;
        
        E->y_old[0] = isx;
        E->y_old[1] = isy;
        
        carlson_fastC(E->difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem);
}

void init_ekfCh2(ekf_data *E, double Tv)
{
        // Tuning of matrix Q
        E->Q[0]=prevod(0.005,15);      // 1e-3
        E->Q[3]=prevod(0.001,15);      // 1e-3
        
        E->Chf[0]=0x1FFF;// !       // 0.05
        E->Chf[1]=E->Chf[2]=0;
        E->Chf[3]=0x1FFF;//!
                
        // Tuning of matrix R
        E->dR[0]=prevod(0.001,15);         // 0.05
        E->dR[1]=E->dR[0];
        
        // Motor model parameters 
        E->cA=prevod(1-Tv*Rs/Ls,15);
        E->cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
        E->cC=prevod(Tv/Ls/Iref*Uref,15);
        //  cD=prevod(1-Tv*Bf/J,15);
        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
        //  cF=prevod(p*Tv*Mref/J/Wref,15);
        E->cG=prevod(Tv*Wref/Thetaref,15); //in q15!
        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
        // cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < ===========
        E->cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 
        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
        
        /* Init matrix PSI with permanently constant terms */
        E->PSI[0]=0x7FFF;
        E->PSI[2]=E->cG;
        E->PSI[3]=0x7FFF;
}

void ekfmm(ekf_data* E1, ekf_data* E2, int16 ux, int16 uy, int16 isx, int16 isy)
{
        int32 det1, det2;
        int16 rem1, rem2;
        ekfCh2(E1, ux,uy,isx,isy,&det1,&rem1);
        ekfCh2(E2, ux,uy,isx,isy,&det2,&rem2);
        
        
}