root/pmsm/sim_var.cpp @ 264

/*!
  \file
  \brief Simulation of disturbances in PMSM model, EKF runs with simulated covariances
  \author Vaclav Smidl.

  \ingroup PMSM
  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/


#include <stat/libFN.h>
#include <estim/libKF.h>
//#include <estim/libPF.h>
#include <math/chmat.h>

#include "pmsm.h"
#include "simulator.h"
#include "sim_profiles.h"

#include <stat/loggers.h>

using namespace bdm;

int main() {
        // Kalman filter
        int Ndat = 90000;
        double h = 1e-6;
        int Nsimstep = 125;

        dirfilelog L("exp/sim_var2",1000);
        //memlog L(Ndat);
        
        
        // SET SIMULATOR
        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
        double Ww = 0.0;
        vec dt ( 2 );
        vec ut ( 2 );
        vec utm ( 2 );
        vec dut ( 2 );
        vec dit (2);
        vec x2o(2);
        vec xtm=zeros ( 4 );
        vec xte=zeros ( 4 );
        vec xdif=zeros ( 4 );
        vec xt ( 4 );
        vec ddif=zeros(2);
        IMpmsm2o fxu;
        //                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
        OMpmsm hxu;
        mat Qt=zeros ( 4,4 );
        mat Rt=zeros ( 2,2 );

        // ESTIMATORS
        vec mu0= "0.0 0.0 0.0 0.0";
        vec Qdiag ( "62 66 454 0.03" ); //zdenek: 0.01 0.01 0.0001 0.0001
        vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
        mat Q =diag( Qdiag );
        mat R =diag ( Rdiag );
        EKFfull Efix ( rx,ry,ru );
        Efix.set_est ( mu0, 1*eye ( 4 )  );
        Efix.set_parameters ( &fxu,&hxu,Q,R);

        EKFfull Eop ( rx,ry,ru );
        Eop.set_est ( mu0, 1*eye ( 4 ) );
        Eop.set_parameters ( &fxu,&hxu,Q,R);

        EKFfull Edi ( rx,ry,ru );
        Edi.set_est ( mu0, 1*eye ( 4 ) );
        Edi.set_parameters ( &fxu,&hxu,Q,R);

        const epdf& Efix_ep = Efix._epdf();
        const epdf& Eop_ep = Eop._epdf();
        const epdf& Edi_ep = Edi._epdf();
        
        //LOG
        RV rQ( "{Q }", "16");
        RV rR( "{R }", "4");
        RV rUD( "{u_isa u_isb i_isa i_isb }", ones_i(4));
        RV rDu("{dux duy }",ones_i(2));
        RV rDi("{disa disb }",ones_i(2));
        int X_log = L.add(rx,"X");
        int X2o_log = L.add(rx,"X2o");
        int Xdf_log = L.add(rx,"Xdf");
        int Efix_log = L.add(rx,"XF");
        int Eop_log = L.add(rx,"XO");
        int Edi_log = L.add(rx,"XD");
        int Q_log = L.add(rQ,"Q");
        int R_log = L.add(rR,"R");
        int D_log = L.add(rUD,"D");
        int Du_log = L.add(rDu,"Du");
        int Di_log = L.add(rDi,"Di");
        L.init();

        for ( int tK=1;tK<Ndat;tK++ ) {
                //Number of steps of a simulator for one step of Kalman
                for ( int ii=0; ii<Nsimstep;ii++ ) {
                        sim_profile_steps1 ( Ww , false);
                        pmsmsim_step ( Ww );
                };
                // simulation via deterministic model
                ut ( 0 ) = KalmanObs[4];
                ut ( 1 ) = KalmanObs[5];
                
                x2o = fxu.eval2o(utm-ut);
                utm = ut;
                
                dt ( 0 ) = KalmanObs[2];
                dt ( 1 ) = KalmanObs[3];
                dut ( 0 ) = KalmanObs[4];
                dut ( 1 ) = KalmanObs[5];
                dit ( 0 ) = KalmanObs[8];
                dit ( 1 ) = KalmanObs[9];

                xte = fxu.eval ( xtm,ut );
                //Results:
                // in xt we have simulation according to the model
                // in x we have "reality"
                xt ( 0 ) =x[0];xt ( 1 ) =x[1];xt ( 2 ) =x[2];xt ( 3 ) =x[3];
                xdif = xt-xte; //xtm is a copy of x[]
                if (xdif(0)>3.0){
                        cout << "here" <<endl;
                        }
                if ( xdif ( 3 ) >pi ) xdif ( 3 )-=2*pi;
                if ( xdif ( 3 ) <-pi ) xdif ( 3 ) +=2*pi;
                
                ddif = hxu.eval(xt,ut) - dit;

                //Rt = 0.9*Rt + xdif^2
                Qt*=0.1;
                Qt += 10*outer_product ( xdif,xdif ); //(x-xt)^2

                if (Qt(0,0)>3.0){
                        cout << "here" <<endl;
                        }
                // For future ref.
                xtm = xt;

                Rt*=0.1;
//              Rt += 10*outer_product ( ddif,ddif ); //(x-xt)^2

                //ESTIMATE
                Efix.bayes(concat(dt,ut));
                //
                Eop.set_parameters ( &fxu,&hxu,(Qt+1e-8*eye(4)),(Rt+1e-6*eye(2)));
                Eop.bayes(concat(dt,ut));
                //
                Edi.set_parameters ( &fxu,&hxu,(diag(diag(Qt))+1e-16*eye(4)), (diag(diag(Rt))+1e-3*eye(2)));
                Edi.bayes(concat(dt,ut));
                
                //LOG
                L.logit(X_log,  vec(x,4)); //vec from C-array
                L.logit(X2o_log, x2o); 
                L.logit(Xdf_log, xdif); 
                L.logit(Efix_log, Efix_ep.mean() ); 
                L.logit(Eop_log,        Eop_ep.mean() ); 
                L.logit(Edi_log,        Edi_ep.mean() ); 
                L.logit(Q_log,  vec(Qt._data(),16) ); 
                L.logit(R_log,          vec(Rt._data(),4) ); 
                L.logit(D_log,  vec(KalmanObs,4) ); 
                L.logit(Du_log, dut ); 
                L.logit(Di_log, dit ); 
                
                L.step();
        }

        L.finalize();
        //L.itsave("sim_var.it");       
        

        return 0;
}