root/library/bdm/design/arx_ctrl.h @ 1187

/*!
  \file
  \brief Controllers for linear Gaussian systems
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

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

#include "../base/bdmbase.h"
#include "ctrlbase.h"
#include "../estim/arx.h"
#include "../estim/kalman.h"
//#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h>

namespace bdm {

//! Controller using ARX model for estimation and LQG designer for control
class LQG_ARX : public Controller {
protected:
    //! Internal ARX estimator
    shared_ptr<ARX> ar;
    //! Internal LQG designer
    LQG lq;
    //! Intermediate StateSpace model
    shared_ptr<StateFromARX> Stsp;
    //! AR predictor
    shared_ptr<mlnorm<chmat> > pred;
    //! datalink from rvc to ar.bayes
    datalink rvc2ar_y;
    //! datalink from rvc to ar.bayes
    datalink_buffered rvc2ar_cond;

    //! flag to use iterations spread in time (ist)
    bool ist;
    //! flag to use windsurfer approach
    bool windsurfer;
public:
    //! Default constructor
    LQG_ARX() : ar(), lq() { }

    //! adaptation is to store arx estimates in stsp
    void adapt ( const vec &data ) {
        ar->bayes ( rvc2ar_y.pushdown ( data ), rvc2ar_cond.pushdown ( data ) );
        rvc2ar_cond.store_data ( data );
        ar->ml_predictor_update ( *pred );
    }
    void redesign() {
        Stsp->update_from ( *pred );
        if ( windsurfer ) {
            mat Ry = pred->_R();
            lq.set_control_Qy ( inv ( Ry ) );
        }
        if ( !ist ) {
            lq.initial_belmann();
        }
        lq.redesign();
    }
    vec ctrlaction ( const vec &cond ) const {
        //cond is xt + ut
        vec state = cond.left ( Stsp->_A().rows() );
        if ( Stsp->_have_constant() ) {
            state ( state.length() - 1 ) = 1;
        }
        vec tmp=lq.ctrlaction ( state, cond.right ( Stsp->_B().cols() ) );
        if (!std::isfinite(sum(tmp))) {
            cout << "infinite ctrl action";
        }
        return tmp;
    }
    //!
    //! LQG is defined by quadratic loss function
    /*! \f[ L(y,u) = (y-y_{req})'Q_y (y-y_{req}) + (u-u_{req})' Q_u (u-u_{req}) \f]
                expected input
                \code
        { class="LQG";
                arx = {class="ARX", ...} // internal arx model, see
                Qy = ("matrix", ...);
                Qu = ("matrix", ...);
                yreq = [];               // requested output
                ureq = [];               // requested input value
        }
                \endcode
     */
    void from_setting ( const Setting &set ) {
        ar = UI::build<ARX> ( set, "ARX", UI::compulsory );

        mat Qu;
        mat Qy;

        UI::get ( Qu, set, "Qu", UI::compulsory );
        UI::get ( Qy, set, "Qy", UI::compulsory );

        vec y_req;
        if ( !UI::get ( y_req, set, "yreq", UI::optional ) )
            y_req = zeros ( ar->_yrv()._dsize() );

        int horizon;
        UI::get ( horizon, set, "horizon", UI::compulsory );
        lq.set_control_parameters ( Qy, Qu, y_req, horizon );

        int wind;
        if ( UI::get ( wind, set, "windsurfer", UI::optional ) ) {
            windsurfer = wind > 0;
        } else {
            windsurfer = false;
        };
        int ist_;
        if ( UI::get ( ist_, set, "ist", UI::optional ) ) {
            ist = ist_ > 0;
        } else {
            ist = false;
        };
        validate();
    }

    void validate() {
        // ar is valid
        pred = ar->ml_predictor<chmat>();
        Stsp = new StateFromARX;
        RV xrv;
        RV urvm; //old ut
        Stsp->connect_mlnorm ( *pred, xrv, urvm );
        lq.set_system ( Stsp );
        lq.validate();

        rv = urvm; // rv is not shifted to t+1!!
        rvc = concat ( xrv, urvm );
        rvc2ar_y.set_connection ( ar->_yrv(), rvc );
        rvc2ar_cond.set_connection ( ar->_rvc(), rvc );
        //datalink from ARX to rvc
    }
    void log_register ( logger &L, const string &prefix ) {
        ar->log_register ( L, prefix );
    }
    void log_write ( ) const {
        ar->log_write();
    }
    //! access function
    const ARX& _ar() {
        return *ar.get();
    }
    //! access function
    mlnorm<chmat>* _pred() {
        return pred.get();
    }


};
UIREGISTER ( LQG_ARX );
} // namespace