root/applications/pmsm/pmsm_estim.cpp @ 1179

/*!
\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"

using namespace bdm;
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 );

        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, "estimator" );
        int nE = Es.length();   //number of estimators
        int Ndat;                               //number of data records
        F.lookupValue ( "experiment.ndat",Ndat );
                
        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();

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