root/applications/pmsm/filters.h @ 654

#ifndef PMSMFIL_H
#define PMSMFIL_H

#include "pmsm.h"
#include <estim/particles.h>

/*! \defgroup PMSM
@{
*/

using namespace bdm;

//* FAILURE :(((
class MPFpmsm: public MPF {
private:

        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;


//! modelling theta as normal random walk with 2pi correction
        class rwtheta: public mpdf_internal<enorm<fsqmat> > {
                double om_hat_dt;
                double om_var;
                double rth;
                MPFpmsm& p;
        public:
                rwtheta ( MPFpmsm& p0 ) :p ( p0 ) {};
                void set_parameters ( double omh0, double omv0, double rth0 ) {
                        om_hat_dt=omh0*p.dt;
                        om_var=omv0;
                        rth=rth0;

                        mat R ( 1,1 );
                        R ( 0,0 ) =p.dt*p.dt*om_var+rth;
                        vec mu ( 1 );
                        mu ( 0 ) = om_hat_dt;
                        iepdf.set_parameters ( mu, fsqmat ( R ) );
                }
                vec samplecond ( const vec &cond ) {
                        iepdf._mu() =om_hat_dt+cond ( 0 );

                        //
//                      iepdf._mu()= x[3];// ----------
                        vec th=iepdf.sample();
                        if ( th ( 0 ) >pi ) th ( 0 )-=2*pi;
                        if ( th ( 0 ) <-pi ) th ( 0 ) +=2*pi;
                        return th;
                }
        };

        class PMSMlin: public KalmanFull {
        protected:
                MPFpmsm &p;
                double thm;
        public:
                PMSMlin ( MPFpmsm &p0 ) :p ( p0 ) {};
                PMSMlin ( const PMSMlin &P0 ) : KalmanFull ( P0 ), p ( P0.p ), thm ( P0.thm ) {}
                BM* _copy_() const {return new PMSMlin ( *this );}

                void fillA() {
                        //ia
                        //using namespace p;
                        A ( 0,0 ) = ( 1.0- p.Rs/p.Ls*p.dt ) ;
                        A ( 0,2 ) = p.Ypm/p.Ls*p.dt* sin ( thm );
                        //ib
                        A ( 1,1 ) = ( 1.0- p.Rs/p.Ls*p.dt ) ;
                        A ( 1,2 ) = - p.Ypm/p.Ls*p.dt* cos ( thm );
                        //om
                        A ( 2,0 ) = p.kp*p.p*p.p * p.Ypm/p.J*p.dt* ( -sin ( thm ) );
                        A ( 2,1 ) =p.kp*p.p*p.p * p.Ypm/p.J*p.dt* ( cos ( thm ) );
                        A ( 2,2 ) = 1.0;
                }

                void condition ( const vec &cond ) {
                        thm = cond ( 0 );
                        fillA();
                }
        };

        shared_ptr<rwtheta> rwt;

        vec dQ;
        vec dR;
public:
        void from_setting ( const Setting &set ) {
                BM::from_setting ( set ); //reads drv

                const SettingResolver& params_b=set["params"];
                const Setting& params=params_b.result;

                Rs= params["Rs"];
                Ls = params["Ls"];
                dt = 125e-6;
                Ypm=params["Fmag"];
                kp= params["kp"];
                p= params["p"];
                J = params["J"];

                UI::get ( dQ, set, "dQ", UI::compulsory );
                UI::get ( dR, set, "dR", UI::compulsory );

                mat Q = diag ( dQ ( 0,2 ) );
                mat R = diag ( dR ( 0,1 ) );

                mat A=zeros ( 3,3 );
                mat B=concat_vertical ( dt/Ls*eye ( 2 ), zeros ( 1,2 ) );
                mat C=zeros ( 2,3 );
                C ( 0,0 ) =1.0;
                C ( 1,1 ) =1.0;
                mat D= zeros ( 2,2 );

                shared_ptr<PMSMlin> kal=new PMSMlin ( *this );
                kal->set_parameters ( A,B,C,D,Q,R );
                kal->set_statistics ( zeros ( 3 ), eye ( 3 ) );
                kal->condition ( vec_1 ( 0.0 ) );
                kal->validate();

                pf = new PF;
                rwt=new rwtheta ( *this );
                pf->prior_from_set ( set );
                pf->resmethod_from_set ( set );
                pf->set_model ( rwt,new mpdf() );

                shared_ptr<RV> rv = UI::build<RV> ( set, "rv", UI::optional );
                if ( rv ) {
                        set_rv ( *rv );
                }

                if ( posterior()._rv()._dsize() >0 ) {
                        kal->set_rv ( posterior()._rv().subselect ( "0,1,2" ) );
                        pf->set_rv ( posterior()._rv().subselect ( "3" ) );
                }

                this->set_BM ( *kal );

                string opt;
                if ( UI::get ( opt,set,"options",UI::optional ) ) {
                        set_options ( opt );
                }

                validate();
        }
        void bayes ( const vec& data ) {
                const vec &mu = posterior().mean();
                const vec &Var =  posterior().variance();
                rwt->set_parameters ( mu ( 3 ),Var ( 3 ),dQ ( 3 ) );
// COPY from MPF bayes:
                int i;
                int n=pf->__w().length();
                vec &lls = pf->_lls();

// generate samples - time step
                for ( int i = 0; i < n; i++ ) {
                        const vec &mu = BMs ( i )->posterior().mean();
                        const vec &Var =  BMs ( i )->posterior().variance();
                        rwt->set_parameters ( mu ( 2 ),Var ( 2 ),dQ ( 3 ) );

                        Array<vec> &smp=pf->__samples();
                        smp ( i ) = rwt->samplecond ( smp ( i ) );
                        lls ( i ) = 0;
                }
// weight them - data step
#pragma parallel for
                for ( i = 0; i < n; i++ ) {
                        BMs ( i ) -> condition ( pf->posterior()._sample ( i ) );
                        BMs ( i ) -> bayes ( data );
                        lls ( i ) += BMs ( i )->_ll();
                }

                pf->bayes_weights();

                ivec ind;
                if ( pf->do_resampling() ) {
                        pf->resample ( ind );

#pragma omp parallel for
                        for ( i = 0; i < n; i++ ) {
                                if ( ind ( i ) != i ) {//replace the current Bm by a new one
                                        delete BMs ( i );
                                        BMs ( i ) = BMs ( ind ( i ) )->_copy_(); //copy constructor
                                }
                        };
                }


        }
};

UIREGISTER ( MPFpmsm );



/*!@}*/
#endif //PMSMFIL_H