root/applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp @ 1190

#include <estim/kalman.h>

#include "ekf_obj.h"
#include "../simulator.h"

double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};

void mat_to_int(const imat &M, int *I){
        for (int i=0;i<M.rows(); i++){
                for (int j=0;j<M.cols(); j++){
                        *I++ = M(i,j);
                }
        }
}
void vec_to_int(const ivec &v, int *I){
        for (int i=0;i<v.length(); i++){
                        *I++ = v(i);
        }
}

///////////////
void EKFfixed::bayes(const vec &yt, const vec &ut){
        ekf(yt(0),yt(1),ut(0),ut(1));
        
        vec xhat(4);    
        //UGLY HACK!!! reliance on a predictor!!
        xhat(0)=zprevod(x_est[0],Qm)*Iref;
        xhat(1)=zprevod(x_est[1],Qm)*Iref;
        xhat(2)=zprevod(x_est[2],Qm)*Wref;
        xhat(3)=zprevod(x_est[3],15)*Thetaref;
        
        E.set_mu(xhat);
        
        if ( BM::evalll ) {
                //from enorm
                vec xdif(x,4);//first 4 of x
                //UGLY HACK!!! reliance on a predictor!!
/*              xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref;
                xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref;
                xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref;
                xdif(3)=x[3]-zprevod(x_pred[3],15);*/
                
                xdif -=xhat; //(xdif=x-xhat)
                
                mat Pfull(4,4);
                double* Pp=Pfull._data();
                for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
                
                E._R()._M()=Pfull;
                
                
                BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );
        }
};


void EKFfixed::update_psi(void)
{
  int t_sin,t_cos,tmp;

  // implementace v PC
  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);

  PSI[2]=((long)cB*t_sin)>>15;
  tmp=((long)cH*x_est[2])>>15;
  PSI[3]=((long)tmp*t_cos)>>15;
  PSI[6]=-((long)cB*t_cos)>>15;
  PSI[7]=((long)tmp*t_sin)>>15;
}


void EKFfixed::prediction(int *ux)
{
  int t_sin,t_cos, tmp;

  // implementace v PC
  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);

  tmp=((long)cB*x_est[2])>>15;
  x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
  x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15;
  x_pred[2]=x_est[2];
  x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;

  update_psi();

  mmult(PSI,P_est,temp15a,3,3,3);
//  mtrans(PSI,temp15b,5,5);
  mmultt(temp15a,PSI,P_pred,3,3,3);
  maddD(P_pred,Q,3,3);
}

void EKFfixed::correction(void)
{
  int Y_error[2];
  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */

  choice_P(P_pred,temp15a,3);
  maddD(temp15a,R,1,1);
  minv2(temp15a,temp30a);
        Ry(0,0)=zprevod(temp15a[0],15);
        Ry(0,1)=zprevod(temp15a[1],15);
        Ry(1,0)=zprevod(temp15a[2],15);
        Ry(1,1)=zprevod(temp15a[3],15);
  
  mmultDr(P_pred,temp15a,3,3,1,1);
  mmult1530(temp15a,temp30a,Kalm,3,1,1);


  /* estimate the state system */
  choice_x(x_pred, temp15a);
  msub(Y_mes,temp15a,Y_error,1,0);
  mmult(Kalm,Y_error,temp15a,3,1,0);
  madd(x_pred,temp15a,x_est,3,0);

  /* matrix of covariances - version without MMULTDL() */

/* Version with MMULTDL() */
  mmultDl(P_pred,temp15a,1,3,3,1);

  mmult(Kalm,temp15a,P_est,3,1,3);
  msub(P_pred,P_est,P_est,3,3);
/* END */
}


void EKFfixed::ekf(double ux, double uy, double isxd, double isyd)
{
  // vypocet napeti v systemu (x,y)
  ukalm[0]=prevod(ux/Uref,Qm);
  ukalm[1]=prevod(uy/Uref,Qm);

  // zadani mereni
  Y_mes[0]=prevod(isxd/Iref,Qm);
  Y_mes[1]=prevod(isyd/Iref,Qm);

  ////// vlastni rutina EKF /////////////////////////
  prediction(ukalm);
  correction();

  // navrat estimovanych hodnot regulatoru
  vec& mu = E._mu();
  (mu)(0)=zprevod(x_est[0],Qm)*Iref;
  (mu)(1)=zprevod(x_est[1],Qm)*Iref;
  (mu)(2)=zprevod(x_est[2],Qm)*Wref;
  (mu)(3)=zprevod(x_est[3],15)*Thetaref;
}

void EKFfixed::init_ekf(double Tv)
{
  // Tuning of matrix Q
  Q[0]=prevod(.01,15);       // 0.05
  Q[5]=Q[0];
  Q[10]=prevod(0.0001,15);      // 1e-3
  Q[15]=prevod(0.0001,15);      // 1e-3

  // Tuning of matrix R
  R[0]=prevod(0.05,15);         // 0.05
  R[3]=R[0];

  // Motor model parameters 
  cA=prevod(1-Tv*Rs/Ls,15);
  cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
  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);
  cG=prevod(Tv*Wref*4/Thetaref,15);
  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);

  /* Init matrix PSI with permanently constant terms */
  PSI[0]=cA;
  PSI[5]=PSI[0];
  PSI[10]=0x7FFF;
  PSI[14]=cG;
  PSI[15]=0x7FFF;
}


void EKF_UDfix::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const vec R0 ) {
        pfxu = pfxu0;
        phxu = phxu0;
        
        set_dim ( pfxu0->_dimx() );
        dimy = phxu0->dimension();
        dimc = pfxu0->_dimu();
        
        vec &_mu = est._mu();
        // if mu is not set, set it to zeros, just for constant terms of A and C
        if ( _mu.length() != dimension() ) _mu = zeros ( dimension() );
        A = zeros ( dimension(), dimension() );
        C = zeros ( dimy, dimension() );
        
        //initialize matrices A C, later, these will be only updated!
        pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true );
        //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
        phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true );
        //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
        
        R = R0;
        Q = Q0;
        
        // 
}
// aux fnc 
void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref){
        mat P= U*diag(D)*U.T();
        mat T = diag(1.0/(xref));
        mat Pf = T*P*T;
        
        ldmat Pld(Pf);
        
        mat Ut=Pld._L().T()*(1<<15); // U is in q15 -- diagonal is 0!!!
        Uf=round_i(Ut);
        Df=round_i(Pld._D()*(1<<15));
        ivec zer=find(Df==0);
        for(int i=0; i<zer.length(); i++) Df(zer(i))=1;
}


void EKF_UDfix::bayes ( const vec &yt, const vec &cond ) {
        //preparatory
        vec &_mu=est._mu();
        const vec &u=cond;
        int dim = dimension();
        ///// !!!!!!!!!!!!!!!!
        U = est._R()._L().T();
        D =  est._R()._D();
        
        bool DBG=true;
        if (DBG){
                mat L;
                ivec tmp;
                mat X=randn(5,5);
                mat XX=X*X.T();
                mat T=diag(sqrt(1.0/diag(XX)));
                XX= T*XX*T;
                
                ldmat ldd(XX);
                U = ldd._L().T();
                D = ldd._D()*0.9;
        }
                
        ////////////
        
        pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx
        phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx
        
        mat PhiU = A*U;

        //////
        vec xref(5);
        xref(0)= 30.0*1.4142;
        xref(1)= 30.0*1.4142;
        xref(2)= 6.283185*200.;
        xref(3) = 3.141593;
        xref(4) = 34.0;
        
        if (DBG){
                xref = ones(5);
        }
        
        imat Utf;
        ivec Dtf;
        UDtof(U,D,Utf,Dtf,xref);
        mat invxref=diag(1.0/xref);
        imat Af=round_i(invxref*A*diag(xref)*(1<<15));
        mat_to_int(Af, PSI);
        mat_to_int(Utf,Uf);             
        vec_to_int(Dtf, Df); 

//      cout << Dtf << endl;
        
        // A*U == PSI*U
        mmultAU(PSI,Uf,PSIU,5,5);       
/*      cout << ivec(PSIU,25) << endl;
        cout << (Af*Utf)/(1<<15) <<endl;*/
        
        vec Din = D;
        int i,j,k;
        double sigma;
        mat G = eye(dim);
        //////// thorton
        
        //move mean;
        _mu = pfxu->eval(_mu,u);
        
        for (i=dim-1; i>=0;i--){
                sigma = 0.0; 
                for (j=0; j<dim; j++) {
                        sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
                        sigma += G(i,j)*G(i,j) * Q(j,j);
                }
                
/*              double sigma2= 0.0;
                for (j=0; j<dim; j++) {
                        sigma2 += PhiU(i,j)*PhiU(i,j) *Din(j); 
                }
                sigma2 +=Q(i,i);//*G(i,i)=1.0
                for (j=i+1; j<dim; j++) {
                        sigma2 += G(i,j)*G(i,j) * Q(j,j);
                }*/
                D(i) = sigma; 
                
        /*      UDtof(U,D,Utf,Dtf,xref);
                cout << "d=sig"<<endl;
                cout << Dtf << endl;
        */      
                for (j=0;j<i;j++){ 
//                      cout << i << "," << j << endl;
                        sigma = 0.0; 
                        for (k=0;k<dim;k++){ 
                                sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
                        }
                        for (k=0;k<dim;k++){ 
                                sigma += G(i,k)*Q(k,k)*G(j,k); 
                        }
                        //
                        U(j,i) = sigma/D(i); 
                        
/*                      cout << "U=sig/D"<<endl;
                        UDtof(U,D,Utf,Dtf,xref);
                        cout << Utf << endl << Dtf << endl;
                        cout << G << endl << Din << endl<<endl;*/
                        
                        for (k=0;k<dim;k++){ 
                                PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
                        }
                        for (k=0;k<dim;k++){ 
                                G(j,k) = G(j,k) - U(j,i)*G(i,k); 
                        }
                
                }
                UDtof(U,D,Utf,Dtf,xref);
                //      cout << G << endl << Din << endl;
                imat Urw=Utf.T();
                cout << ivec(Urw._data(),25) << endl << Dtf << endl;
        }

        int Qf[25];
        imat Qrnd=round_i(diag(elem_div(diag(Q),pow(xref,2)))*(1<<15));
//      cout << "Qr" << Qrnd<<endl;
        mat_to_int(Qrnd,Qf);

//      cout << Df <<endl;
        thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
        
        if (DBG){
                _mu = vec("0.500 0.200 0.300 0.400 0.1");
        }
        
        cout << "bierman double I" <<endl;
        UDtof(U,D,Utf,Dtf,xref);
        cout << "Uf: " << Utf << endl;
        cout << "Df: " << Dtf << endl;
        cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
        mat_to_int(Utf,Uf);             
        vec_to_int(Dtf, Df); 
        
        // bierman
        
        double dz,alpha,gamma,beta,lambda;
        vec a;
        vec b;
        vec yp = phxu->eval(_mu,u);
        vec xp=_mu; // used in bierman
        
        if (DBG){
                yp = vec("0.40 0.300");
        }
        
        for (int iy=0; iy<dimy; iy++){
                a     = U.T()*C.get_row(iy);    // a is not modified, but 
                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain. 
                dz    = yt(iy) - yp(iy); 
                
                alpha = R(iy); 
                gamma = 1/alpha; 
                for (j=0;j<dim;j++){
                        beta   = alpha; 
                        alpha  = alpha + a(j)*b(j); 
                        lambda = -a(j)*gamma; 
                        gamma  = 1.0/alpha; 
                        D(j) = beta*gamma*D(j); 
                        
                        //                      cout << "a: " << alpha << "g: " << gamma << endl;
                        for (i=0;i<j;i++){
                                beta   = U(i,j); 
                                U(i,j) = beta + b(i)*lambda; 
                                b(i)   = b(i) + b(j)*beta; 
                        }
                }
                double dzs = gamma*dz;  // apply scaling to innovations 
                _mu   = _mu + dzs*b; // multiply by unscaled Kalman gain 
                //cout << "Ub: " << U << endl;
                //cout << "Db: " << D << endl <<endl;
                
        }

UDtof(U,D,Utf,Dtf,xref);
cout << "Uf: " << Utf << endl;
cout << "Df: " << Dtf << endl;
cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;

        int difz[2];
        vec_to_int(round_i((yt-yp)/xref(0)*(1<<15)), difz);
        int xf[5];
        vec_to_int(round_i(elem_div(xp,xref)*(1<<15)), xf);
        int Rf[2];//={1,1};
        vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf);
        //beirman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx 
        
        cout <<endl<< "bierman fixed" <<endl;
        cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;

        cout << "dz: "; for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;

        int xf_old[5];
        vec_to_int(ivec(xf,5),xf_old);
        bierman(difz,xf, Uf, Df, Rf, 2, 5);
        
        UDtof(U,D,Utf,Dtf,xref);
        cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
        cout << endl;
        
        
        
        /////
        est._R().__L()=U.T();
        est._R().__D()=D;
        
        if ( evalll == true ) { //likelihood of observation y
        }
}

void EKF_UDfix::from_setting ( const Setting &set ) {
        BM::from_setting ( set );
        shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory );
        shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory );
        
        //statistics
        int dim = IM->dimension();
        vec mu0;
        if ( !UI::get ( mu0, set, "mu0" ) )
                mu0 = zeros ( dim );
        
        mat P0;
        vec dP0;
        if ( UI::get ( dP0, set, "dP0" ) )
                P0 = diag ( dP0 );
        else if ( !UI::get ( P0, set, "P0" ) )
                P0 = eye ( dim );
        
        est._mu()=mu0;
        est._R()=ldmat(P0);
        
        //parameters
        vec dQ, dR;
        UI::get ( dQ, set, "dQ", UI::compulsory );
        UI::get ( dR, set, "dR", UI::compulsory );
        set_parameters ( IM, OM, diag ( dQ ), dR  );
        
        UI::get(log_level, set, "log_level", UI::optional);
}


void EKFfixedUD::bayes(const itpp::vec& yt, const itpp::vec& ut)
{
ekf(ut[0],ut[1],yt[0],yt[1]);
}


void EKFfixedUD::ekf(double ux, double uy, double isxd, double isyd)
{
        // vypocet napeti v systemu (x,y)
        int uf[2];
        uf[0]=prevod(ux/Uref,Qm);
        uf[1]=prevod(uy/Uref,Qm);
        
        int Y_mes[2];
        // zadani mereni
        Y_mes[0]=prevod(isxd/Iref,Qm);
        Y_mes[1]=prevod(isyd/Iref,Qm);
        
        ////// vlastni rutina EKF -- /////////////////////////
        int t_sin,t_cos, tmp;
        
        // implementace v PC
        t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
        
        tmp=((long)cB*x_est[2])>>15;
        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*uf[0])>>15;
        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*uf[1])>>15;
        x_est[2]=x_est[2];
        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
        
        //void EKFfixed::update_psi(void)
        {               
                PSI[2]=((long)cB*t_sin)>>15;
                tmp=((long)cH*x_est[2])>>15;
                PSI[3]=((long)tmp*t_cos)>>15;
                PSI[6]=-((long)cB*t_cos)>>15;
                PSI[7]=((long)tmp*t_sin)>>15;
        }
        
        ///////// end of copy ///////////////
        mmultAU(PSI,Uf,PSIU,4,4);
        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
        
        int difz[2];
        difz[0]=Y_mes[0]-x_est[0];
        difz[1]=Y_mes[1]-x_est[1];
        //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
        int dR[2];dR[0]=R[0];dR[1]=R[3];
        bierman(difz,x_est,Uf,Df,dR,2,4);
        
        // navrat estimovanych hodnot regulatoru
        vec& mu = E._mu();
        (mu)(0)=zprevod(x_est[0],Qm)*Iref;
        (mu)(1)=zprevod(x_est[1],Qm)*Iref;
        (mu)(2)=zprevod(x_est[2],Qm)*Wref;
        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
        
//      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
}

void EKFfixedUD::init_ekf(double Tv)
{
        // Tuning of matrix Q
        Q[0]=prevod(.01,15);       // 0.05
        Q[5]=Q[0];
        Q[10]=prevod(0.0001,15);      // 1e-3
        Q[15]=prevod(0.0001,15);      // 1e-3

        Uf[0]=0x7FFF;       // 0.05
        Uf[5]=0x7FFF;
        Uf[10]=0x7FFF;      // 1e-3
        Uf[15]=0x7FFF;      // 1e-3
        
        Df[0]=0x7FFF;
        Df[1]=0x7FFF;
        Df[2]=0x7FFF;
        Df[3]=0x7FFF;
        
        // Tuning of matrix R
        R[0]=prevod(0.05,15);         // 0.05
        R[3]=R[0];
        
        // Motor model parameters 
        cA=prevod(1-Tv*Rs/Ls,15);
        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
        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);
        cG=prevod(Tv*Wref*4/Thetaref,15);
        cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
        
        /* Init matrix PSI with permanently constant terms */
        PSI[0]=cA;
        PSI[5]=PSI[0];
        PSI[10]=0x7FFF;
        PSI[14]=cG;
        PSI[15]=0x7FFF;
        
        //////////////////////// =================
        ///// TEST thorton vs. thorton_fast
        
/*      int Ut[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
        int Dt[4]={100,200,300,400};
        int PSIu[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
        int Dold[4];
        
        thorton(Ut,Dt,PSIu,Q,G,Dold,4);
        int Ut2[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
        int Dt2[4]={100,200,300,400};
        int PSIu2[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
        thorton_fast(Ut2,Dt2,PSIu2,Q,G,Dold,4);
        cout<< Q<<endl;*/
}