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

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

//! Low-level class for design of control strategy
class Designer  {
        
        public:
                //! Redesign control strategy 
                virtual void redesign() {
                        bdm_error("Not implemented");
                }

                //! apply control strategy to obtain control input
                virtual vec apply(const vec &cond) {
                        bdm_error("Not implemented");
                        return vec();
                }
};

//! Linear Quadratic Gaussian designer for constant penalizations and constant target
//! Its internals are very close to Kalman estimator
class LQG : public Designer {
        protected:
                //! StateSpace model from which we read data
                shared_ptr<StateSpace<fsqmat> > 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
                mat Qy;
                //! penalization matrix Qu
                mat 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<fsqmat> > S0);
                //! set penalization matrices and control horizon
                void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0);
                //! set system parameters from Kalman filter
//              void set_system_parameters(const Kalman &K);
                //! refresh temporary storage - inefficient can be improved
                void initialize();
                //! validation procedure
                void validate();
                //! function for future use which is called at each time td; Should call initialize()!
                virtual void update_state(){};
                //! 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*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_state();
                                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 apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
        } ;

        //! 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 system output;
                RV yrv;
                //! identifier of the system input;
                RV urv;
                //! description of data needed for \c ctrlaction , required for automatic connection to DS
                RV drv;
                //! vector of logger IDs
                ivec LIDs;
                public:
                        //! function processing new observations and adapting control strategy accordingly
                        virtual void adapt(const vec &data){};
                        //! returns designed control action
                        virtual vec ctrlaction(const vec &data){return vec(0);}
                        
                        void from_setting(const Setting &set){
                                UI::get(yrv,set,"yrv",UI::optional);
                                UI::get(yrv,set,"urv",UI::optional);
                        }
                        //! access function
                        const RV& _urv() {return urv;}
                        //! access function
                        const RV& _yrv() {return yrv;}
                        //! access function
                        const RV& _drv() {return drv;}
                        //! register this controller with given datasource under name "name"
                        virtual void log_add ( logger &L, const string &name = "" ) { }
                        //! write requested values into the logger
                        virtual void logit ( logger &L ) { }
                        
        };
                        
} // namespace