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

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