root/applications/pmsm/pmsm_estim.cpp @ 1230

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

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

using namespace bdm;
#ifdef MEX
#include <itpp/itmex.h>
#include <mex/mex_BM.h>
#include <mex/mex_logger.h>
#include <mex/mex_datasource.h>
#include <mex/mex_function.h>

void mexFunction ( int n_output, mxArray *output[], int n_input, const mxArray *input[] ) {
        // Check the number of inputs and output arguments
        if ( n_input<2 ) mexErrMsgTxt ( "Usage:\n"
                                                "[Res,estimators,Res2]=estimator(system, estimators, experiment, logger)\n"
                                                "  system     = struct('class','datasource',...);  % Estimated system\n"
                                                "  estimators = {struct('class','estimator',...),  % Estimators\n"
                                                "                struct('class','estimator',...),...} \n"
                                                "  === optional ==="
                                                "  experiment = struct('ndat',10);                 % number of data in experiment, full length of finite datasources, 10 otherwise \n"
                                                "  logger     = struct('class','mexlogger');       % How to store results, default=mexlog, i.e. matlab structure\n\n"
                                                "Output:\n"
                                                "  Res          Matlab structure with logged results, \n"  
                                                "  estimators   Array of estimators updated with data \n"
                                                "  Res2         When logfull log_level is on, this structure is filled with structures of posterior densities\n\n"
                                                "see documentation of classes datasource, BM, and mexlogger and their offsprings in BDM." );

        RV::clear_all();
        //CONFIG
        UImxArray F;
        try {
                F.addGroup ( input[0],"system" );
                F.addList ( input[1],"estimators" );
                if ( n_input>2 ) {
                        F.addGroup ( input[2],"experiment" );
                }
                if ( n_input>3 ) {
                        F.addGroup ( input[3],"logger" );
                }
        } catch ( SettingException e ) {
                it_error ( "error: "+string ( e.getPath() ) );
        }

        //DBG
        F.writeFile ( "pmsm_estim.cfg" );

#else
int main ( int argc, char* argv[] ) {
        const char *fname;
        if ( argc>1 ) {fname = argv[1]; }
        else { cout << "Missing configuration file.\n Usage: \n $> estimator config_file.cfg"<<endl; abort(); }
        UIFile F ( fname );
#endif
        
        shared_ptr<logger> L = UI::build <logger>( F, "logger" );
        shared_ptr<DS>  pDS = UI::build<DS> ( F, "system" );
        Array<shared_ptr<BM> > Es;                      // array of estimators
        UI::get( Es, F, "estimators" );
        int nE = Es.length();   //number of estimators
        int Ndat;                               //number of data records
        F.lookupValue ( "experiment.ndat",Ndat );
                        
        if ( !L ) {
        #ifdef MEX
                        //mex logger has only from_setting constructor - we  have to fill it...
                        L=new mexlog ( Ndat );
        #else
                        L=new stdlog();
        #endif
                }
        pDS->log_register ( *L, "true" );
        string Ename;
        for (int i=0; i<nE; i++){
                try {
                        UI::get ( Ename, F.getRoot()["estimators"][i], "name",UI::optional );
                } catch ( ...) {
                        Ename="Est"+num2str ( i );
                }
                Es(i)->log_register(*L,Ename); // estimate
        }
        L->init();

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

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

        L->finalize();
#ifdef MEX
        mexlog* mL=dynamic_cast<mexlog*> ( L.get() );

        if ( mL ) { // user wants output!!
                if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
                output[0] = mL->toCell();
                if (n_output>1){ // write estimators
                        UImxArray Ests;
                        UI::save(Es, Ests,"estimators");
                        output[1]=UImxArray::create_mxArray(Ests);
                }
                if (n_output>2) {
                        mL->_setting_conf().setAutoConvert(true);
                        output[2]= UImxArray::create_mxArray(mL->_setting_conf().getRoot());
                }
        }
#endif

        for (int i=0; i<nE; i++){
                delete Dls(i);
                delete Dlsc(i);
        }
}