root/library/bdm/design/ctrlbase.h @ 733

/*!
  \file
  \brief Base classes for designers of control strategy
  \author Vaclav Smidl.

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

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

#include "../base/bdmbase.h"
#include "../estim/kalman.h"

namespace bdm {

//! Base class for adaptive controllers
//! The base class is, however, non-adaptive, method \c adapt() is empty.
//! \note advanced Controllers will probably include estimator as their internal attribute (e.g. dual controllers)
class Controller : public root {
protected:
        //! identifier of the designed action;
        RV rv;
        //! identifier of the conditioning variables - data needed ;
        RV rvc;
public:
        //! function processing new observations and adapting control strategy accordingly
        virtual void adapt ( const vec &data ) {};
        //! function redesigning the control strategy
        virtual void redesign() {};
        //! returns designed control action
        virtual vec ctrlaction ( const vec &cond ) const {return vec ( 0 );}

        void from_setting ( const Setting &set ) {
                UI::get ( rv,set,"rv",UI::optional );
                UI::get ( rvc,set,"rvc",UI::optional );
        }
        //! access function
        const RV& _rv() {return rv;}
        //! access function
        const RV& _rvc() {return rvc;}
        //! register this controller with given datasource under name "name"
        virtual void log_register ( logger &L, const string &prefix ) { }
        //! write requested values into the logger
        virtual void log_write ( ) const { }

};

//! Linear Quadratic Gaussian designer for constant penalizations and constant target
//! Its internals are very close to Kalman estimator
class LQG : public Controller {
protected:
        //! StateSpace model from which we read data
        shared_ptr<StateSpace<chmat> > S;
        //! required value of the output y at time t (assumed constant)
        vec y_req;
        //! required value of the output y at time t (assumed constant)
        vec u_req;

        //! Control horizon, set to maxint for infinite horizons
        int horizon;
        //! penalization matrix Qy
        chmat Qy;
        //! penalization matrix Qu
        chmat Qu;
        //! time of the design step - from horizon->0
        int td;
        //! controller parameters
        mat L;

        //!@{ \name temporary storage for ricatti - use initialize
        //! convenience parameters
        int dimx;
        //! convenience parameters
        int dimy;
        //! convenience parameters
        int dimu;

        //!  parameters
        mat pr;
        //! penalization
        mat qux;
        //! penalization
        mat qyx;
        //! internal quadratic form
        mat s;
        //! penalization
        mat qy;
        //! pre_qr part
        mat hqy;
        //! pre qr matrix
        mat pre_qr;
        //! post qr matrix
        mat post_qr;
        //!@}

public:
        //! set system parameters from given matrices
        void set_system ( shared_ptr<StateSpace<chmat> > S0 );
        //! update internal whan system has changed
        void update_system() {
                pr.set_submatrix ( 0,0,S->_B() );
                pr.set_submatrix ( 0,dimu, S->_A() );

                //penalization
                qux.set_submatrix ( 0,0, Qu._Ch() );
                qux.set_submatrix ( 0,dimx+dimu+dimy,Qu._Ch() );

                qyx.set_submatrix ( 0,0,S->_C() );
                qyx.set_submatrix ( 0,dimx,-eye ( dimy ) );

                // parts of QR
                hqy=Qy.to_mat()*qyx*pr;

                // pre_qr
                pre_qr = concat_vertical ( s*pr, concat_vertical ( hqy, qux ) );
        }
        //! set penalization matrices and control horizon
        void set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 );
        //! set penalization matrices and control horizon
        void set_control_Qy ( const mat &Qy0 ) {Qy=Qy0;}
        //! refresh temporary storage - inefficient can be improved
        void initial_belmann() {s=1e-5*eye ( dimx+dimu+dimy );};
        //! validation procedure
        void validate();
        //! function for future use which is called at each time td; Should call initialize()!
        //! redesign one step of the
        void ricatti_step() {
                pre_qr.set_submatrix ( 0,0,s*pr );
                pre_qr.set_submatrix ( dimx+dimu+dimy, dimu+dimx, -Qy.to_mat()*y_req );
                if ( !qr ( pre_qr,post_qr ) ) { bdm_warning ( "QR in LQG unstable" );}
                triu ( post_qr );
                // hn(m+1:2*m+n+r,m+1:2*m+n+r);
                s=post_qr.get ( dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1 );
        };
        void redesign() {
                for ( td=horizon; td>0; td-- ) {
                        update_system();
                        ricatti_step();
                }
                /*                      ws=hn(1:m,m+1:2*m+n+r);
                                        wsd=hn(1:m,1:m);
                                        Lklq=-inv(wsd)*ws;*/
                L = -inv ( post_qr.get ( 0,dimu-1, 0,dimu-1 ) ) * post_qr.get ( 0,dimu-1, dimu, 2*dimu+dimx+dimy-1 );
        }
        //! compute control action
        vec ctrlaction ( const vec &state, const vec &ukm ) const {
                vec pom=concat ( state, ones ( dimy ), ukm );
                return L*pom;
        }
        //! access function
        mat _L() const {return L;}
} ;


} // namespace