root/applications/pmsm/pmsm_estim_mex.cpp @ 1280

/*!
\file
\brief Multi-Estimator (developped for PMSM)


 */

#include "base/user_info.h"
#include "base/loggers.h"
#include "estim/kalman.h"
#include "pmsmDS.h"
#include "filters.h"
#include "base/datasources.h"
#include "simulator_zdenek/ekf_example/ekf_obj.h"

#include <itpp/itmex.h>
#include <mex/mex_BM.h>
#include <mex/mex_logger.h>
#include <mex/mex_datasource.h>
#include <mex/mex_function.h>

using namespace bdm;

void mexFunction ( int n_output, mxArray *output[], int n_input, const mxArray *input[] ) {
        // Check the number of inputs and output arguments
        string fname;
        if (n_input>0){
                fname=mxArray2string(input[0]);
        } else {
                printf("file name missing");
        }
        // ------------------ 
        
        UIFile F ( fname.c_str());

        logger* L;
        DS * DS; 
        Array<BM*> Es;                  // array of estimators
        int Ndat;                               //number of data records
        int nE;                                 //number of estimators

        try {
                UIbuild ( F.lookup ( "logger" ),L );
                UIbuild ( F.lookup ( "system" ),DS );
                F.lookupValue ( "experiment.ndat",Ndat );
                Setting& S=F.lookup ( "estimator" );
                nE = S.getLength();
                Es.set_size(nE);
                for(int i=0;i<nE;i++){          
                        UIbuild (S[i] ,Es(i) );
                }
        }
        catch UICATCH;

        DS->log_add ( *L );
        string nic="";
        for (int i=0; i<nE; i++){
                Es(i)->log_add(*L,nic); // estimate
        }
        L->init();

        vec dt=zeros ( DS->_drv()._dsize() );   //data variable
        Array<datalink*> Dls(nE); 
        for (int i=0; i<nE; i++){
                Dls(i)=new datalink( Es(i)->_drv(),DS->_drv() ); //datalink between a datasource and estimator
        }
        
        // Main cycle
        for ( int tK=1;tK<Ndat;tK++ ) {
                // Data Source
                DS->step();                                                     // simulator step
                DS->getdata ( dt );                                     // read data
                DS->logit ( *L );
                
                // Estimators
                for (int i=0; i<nE; i++){
                        Es(i)->bayes ( Dls(i)->pushdown ( dt ) );               // update estimates

                        Es(i)->logit (*L);
                }
                // Regulators
                L->step();
        }

        L->finalize();

        // ------------------ End of routine -----------------------------
        
        mex_logger* mL=dynamic_cast<mex_logger*>(L);

        if (mL) { // user wants output!!
                if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
                output[0] = mL->toCell();
        }
        
        
        delete L;
        delete DS;
        for (int i=0; i<nE; i++){
                delete Dls(i);
                delete Es(i);
        }
}