arx_ctrl.h

00001
00013 #include "../base/bdmbase.h"
00014 #include "ctrlbase.h"
00015 #include "../estim/arx.h"
00016 #include "../estim/kalman.h"
00017 //#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h>
00018
00019 namespace bdm {
00020
00022 class LQG_ARX : public Controller {
00023 protected:
00025     shared_ptr<ARX> ar;
00027     LQG lq;
00029     shared_ptr<StateFromARX> Stsp;
00031     shared_ptr<mlnorm<chmat> > pred;
00033     datalink rvc2ar_y;
00035     datalink_buffered rvc2ar_cond;
00036
00038     bool ist;
00040     bool windsurfer;
00041 public:
00043     LQG_ARX() : ar(), lq() { }
00044
00046     void adapt ( const vec &data ) {
00047         ar->bayes ( rvc2ar_y.pushdown ( data ), rvc2ar_cond.pushdown ( data ) );
00048         rvc2ar_cond.store_data ( data );
00049         ar->ml_predictor_update ( *pred );
00050     }
00051     void redesign() {
00052         Stsp->update_from ( *pred );
00053         if ( windsurfer ) {
00054             mat Ry = pred->_R();
00055             lq.set_control_Qy ( inv ( Ry ) );
00056         }
00057         if ( !ist ) {
00058             lq.initial_belmann();
00059         }
00060         lq.redesign();
00061     }
00062     vec ctrlaction ( const vec &cond )  {
00063         //cond is xt + ut
00064         vec state = cond.left ( Stsp->_A().rows() );
00065         if ( Stsp->_have_constant() ) {
00066             state ( state.length() - 1 ) = 1;
00067         }
00068         vec tmp=lq.ctrlaction ( state, cond.right ( Stsp->_B().cols() ) );
00069         if (!std::isfinite(sum(tmp))) {
00070             cout << "infinite ctrl action";
00071         }
00072         return tmp;
00073     }
00076
00088     void from_setting ( const Setting &set ) {
00089         ar = UI::build<ARX> ( set, "ARX", UI::compulsory );
00090
00091         mat Qu;
00092         mat Qy;
00093
00094         UI::get ( Qu, set, "Qu", UI::compulsory );
00095         UI::get ( Qy, set, "Qy", UI::compulsory );
00096
00097         vec y_req;
00098         if ( !UI::get ( y_req, set, "yreq", UI::optional ) )
00099             y_req = zeros ( ar->_yrv()._dsize() );
00100
00101         int horizon;
00102         UI::get ( horizon, set, "horizon", UI::compulsory );
00103         lq.set_control_parameters ( Qy, Qu, y_req, horizon );
00104
00105         int wind;
00106         if ( UI::get ( wind, set, "windsurfer", UI::optional ) ) {
00107             windsurfer = wind > 0;
00108         } else {
00109             windsurfer = false;
00110         };
00111         int ist_;
00112         if ( UI::get ( ist_, set, "ist", UI::optional ) ) {
00113             ist = ist_ > 0;
00114         } else {
00115             ist = false;
00116         };
00117         validate();
00118     }
00119
00120     void validate() {
00121         // ar is valid
00122         pred = ar->ml_predictor<chmat>();
00123         Stsp = new StateFromARX;
00124         RV xrv;
00125         RV urvm; //old ut
00126         Stsp->connect_mlnorm ( *pred, xrv, urvm );
00127         lq.set_system ( Stsp );
00128         lq.validate();
00129
00130         rv = urvm; // rv is not shifted to t+1!!
00131         rvc = concat ( xrv, urvm );
00132         rvc2ar_y.set_connection ( ar->_yrv(), rvc );
00133         rvc2ar_cond.set_connection ( ar->_rvc(), rvc );
00134         //datalink from ARX to rvc
00135     }
00136     void log_register ( logger &L, const string &prefix ) {
00137         ar->log_register ( L, prefix );
00138     }
00139     void log_write ( ) const {
00140         ar->log_write();
00141     }
00143     const ARX& _ar() {
00144         return *ar.get();
00145     }
00147     mlnorm<chmat>* _pred() {
00148         return pred.get();
00149     }
00150
00151
00152 };
00153 UIREGISTER ( LQG_ARX );
00154 } // namespace