root/applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp @ 1440

#include "mpf_double.h"
#include "math.h"
#include <stdio.h>
#include "RArandn.h"
#include "fast_exp.h"

static floatx om[N];
static floatx Pt[N];
static floatx sth[N];
static floatx cth[N];
static floatx isdm[N];
static floatx isqm[N];
static floatx th[N];
static floatx w[N];
static floatx lwi[N];

static floatx qth;
static floatx qom;
static floatx r;


floatx qrandu() {
        if ( randu_i==randu_last )
                randu_i =&RArandu[0];
        else
                randu_i++;

        return *randu_i;
}

floatx qrandn() {
        if ( randn_i==randn_last )
                randn_i =&RArandn[0];
        else
                randn_i++;

        return *randn_i;
}

void resample() {
        int N_babies[N];
        floatx cumdist;
        int i;
        floatx ui;
        int i_from=0;
        int i_to=0;

        floatx u0 = qrandu();

        int j=0;
        N_babies[0]=0;
        cumdist = w[0];
        for ( i = 0; i < N; i++ ) {
                ui  = ( i + u0 ) / N;
                while ( ui > cumdist ) {
                        j++;
                        N_babies[j]=0;
                        cumdist+=w[j];
                }
                N_babies [j] ++;
        }
        while (j++<=(N-1)) { // delete all N_babies after j
                N_babies[j]=0; 
        }

        // COPY

        while ( i_from<N ) {
                while ( N_babies[i_from]>1 ) { // 1 baby stays where it is
                        
                        while ( N_babies[i_to]>=1 ) i_to++; // find first empty slot

                        //copy it
                        Pt[i_to]=Pt[i_from];
                        om[i_to]=om[i_from];
                        th[i_to]=th[i_from];
                        isdm[i_to]=isdm[i_from];
                        isqm[i_to]=isqm[i_from];

                        N_babies[i_to]++;
                        N_babies[i_from]--;
                }
                i_from++;
        }
}

void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb ) {

        floatx isd;
        floatx isq;
        floatx usd;
        floatx usq;

        floatx Cd;
        floatx Cq;
        floatx CC;
        floatx oCC;

        floatx difid;
        floatx difiq;

        floatx zeta;
        floatx Kd;
        floatx Kq;
        floatx ro;
        floatx ypd;
        floatx ypq;
        floatx detRy;
        floatx ydiffd;
        floatx ydiffq;
        floatx ydC;

        floatx maxlwi, sumlwi;
        int i;
        
        floatx *th_i, *Pt_i, *om_i, *isdm_i, *isqm_i;
        floatx *w_i, *lw_i;
        floatx *sth_i, *cth_i;
// implementation starts here
        th_i = &th[0];
        Pt_i = &Pt[0];
        om_i = &om[0];
        isdm_i = &isdm[0];
        isqm_i = &isqm[0];
        sth_i = &sth[0];
        cth_i = &cth[0];
        for ( i=0; i<N; i++ ) {

                // time update
                *Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance
                *th_i += *om_i*_dt+qth *  qrandn() ;
                while ( *th_i>M_PI ) *th_i -=2*M_PI;
                while ( *th_i<-M_PI ) *th_i+=2*M_PI;

                *sth_i=sin ( *th_i );
                *cth_i=cos ( *th_i );
                
                isd = *cth_i*isa+*sth_i*isb;
                isq = -*sth_i*isa+*cth_i*isb;
                // process old voltage
                usd = (*cth_i)*usa+(*sth_i)*usb;
                usq = -(*sth_i)*usa+(*cth_i)*usb;

                Cd = isq*_dt;
                Cq = -_b - isd* _dt;

                difid=isd- _ad  * (*isdm_i) - _cd *(usd);
                difiq=isq- _aq  * (*isqm_i) - _cq *(usq);

                CC=Cd*Cd+Cq*Cq;
                zeta = *Pt_i/ ( r+*Pt_i*CC );
                oCC = ( 1-zeta*CC );
                ro = oCC/r;

                Kd = *Pt_i*Cd*ro;
                Kq = *Pt_i*Cq*ro;

                (*Pt_i)*=( 1.0- ( Kd*Cd+Kq*Cq ) );

                ypd = Cd**om_i;
                ypq = Cq**om_i;

                //detRy = ro/r;
                *om_i +=  Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
                ydiffd = ( ypd-difid );
                ydiffq = ( ypq-difiq );
                ydC = ydiffd*Cd + ydiffq*Cq;

                //lwi[i] = 0.5* ( log ( detRy ) + ( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ) ;
                lwi[i] = 0.5*( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ;
                
                *isdm_i=isd;
                *isqm_i=isq;
                
                // shift all counters
                th_i++;
                Pt_i++;
                om_i++;
                isdm_i++;
                isqm_i++;
                sth_i++;
                cth_i++;

        }
/*      maxlwi=-1e10;
        lw_i = &lwi[0];
        for ( i=0;i<N;i++,lw_i++ ) {
                if ( *lw_i>maxlwi ) maxlwi=*lw_i;
        }*/
        lw_i = &lwi[0];
        w_i = &w[0];
        for ( i=0;i<N;i++,lw_i++,w_i++ ) {
//              *lw_i-=maxlwi;
                (*w_i)=EXP ( *lw_i ); // always resample -> wmin=1/n;
        }

        sumlwi=0.0;
        w_i = &w[0];
        for ( i=0;i<N;i++,w_i++ ) sumlwi+=*w_i;
        w_i = &w[0];
        for ( i=0;i<N;i++,w_i++ ) *w_i/=sumlwi;
        
        // NEFF
        floatx sw2=0.0;
        for ( i=0;i<N;i++ ) sw2+=w[i]*w[i];
        printf("%f\n",1./sw2*N);
        
        resample();
}


void mpf_init(floatx qom0, floatx qth0, floatx r0) {
        int i;
        r=r0;
        qth=qth0;
        qom=qom0;

        for ( i=0; i<N; i++ ) {
                th[i]=qrandu() *2*M_PI - M_PI;
                sth[i]=sin ( th[i] );
                cth[i]=cos ( th[i] );
                
                om[i]=0;
                Pt[i]=10.;
                isdm[i]=0;
                isqm[i]=0;
                w[i]=1.0/N;
        }

}
void mpf_mean ( floatx *Ecosth, floatx *Esinth, floatx *Eome ) {
        int i;
        floatx *w_i=&w[0];
        floatx *cth_i=&cth[0];
        floatx *sth_i=&sth[0];
        floatx *om_i=&om[0];
        *Ecosth=0.0;
        *Esinth=0.0;
        *Eome=0.0;
        for ( i=0;i<N;i++,w_i++,cth_i++,sth_i++,om_i++ ) {
                *Ecosth+=(*w_i)*(*cth_i);
                *Esinth+=(*w_i)*(*sth_i);
                *Eome+=(*w_i)*(*om_i);
        }

}
void mpf_th(floatx th1[N]){
        int i;
        for (i=0;i<N;i++) th1[i]=th[i];
}

floatx kalman_om( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th ) {

        floatx isd;
        floatx isq;
        floatx usd;
        floatx usq;

        floatx Cd;
        floatx Cq;
        floatx CC;
        floatx oCC;

        floatx difid;
        floatx difiq;

        floatx zeta;
        floatx Kd;
        floatx Kq;
        floatx ro;
        floatx ypd;
        floatx ypq;
        floatx detRy;
        floatx ydiffd;
        floatx ydiffq;
        floatx ydC;

        static floatx Pt;
        static floatx om;
        floatx cth, sth;
        static floatx isdm, isqm;

        
                Pt = 1.0*1.0*Pt+qom; // Pt is now predictive variance
                //while ( th>M_PI ) th=th-2*M_PI;
                //while ( th<-M_PI ) th=th+2*M_PI;

                sth=sin ( th );
                cth=cos ( th );

                isd = cth*isa+sth*isb;
                isq = -sth*isa+cth*isb;
                usd = cth*usa+sth*usb;
                usq = -sth*usa+cth*usb;

                Cd = isq*_dt;
                Cq = -_b - isd* _dt;

                difid=isd- _ad  *isdm - _cd *usd;
                difiq=isq- _aq  *isqm - _cq *usq;

                CC=Cd*Cd+Cq*Cq;
                zeta = Pt/ ( r+Pt*CC );
                oCC = ( 1-zeta*CC );
                ro = oCC/r;

                Kd = Pt*Cd*ro;
                Kq = Pt*Cq*ro;

                Pt=Pt* ( 1- ( Kd*Cd+Kq*Cq ) );

                ypd = Cd*om;
                ypq = Cq*om;

                detRy = ro/r;
                om = om + Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
                ydiffd = ( ypd-difid );
                ydiffq = ( ypq-difiq );
                ydC = ydiffd*Cd + ydiffq*Cq;

                isdm=isd;
                isqm=isq;
                
                return om;
}