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

/*!
  \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 <bdmbase.h>

//! Base class of designers of control strategy
class Designer : public root {
        public:
                //! Redesign control strategy 
                virtual redesign(){it_error("Not implemented"); };
                //! apply control strategy to obtain control input
                virtual vec apply(const vec &cond){it_error("Not implemented"); return vec(0);}
}

//! Linear Quadratic Gaussian designer for constant penalizations and constant target
//! Its internals are very close to Kalman estimator
class LQG : public Designer {
        protected:
                //! dimension of state
                int dimx;
                //! dimension of inputs
                int dimu;
                //! dimension of output
                /int dimy;

                //! matrix A of the linear system
                mat A;
                //! matrix B of the linear system
                mat B;
                //! matrix C of the linear system
                mat C;
                //! expected value of x at time t
                vec xt;
                //! required value of the output y at time t (assumed constant)
                vec y_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
                //!  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_parameters(const mat &A, const mat &B, const mat &C);
                //! set system parameters from Kalman filter
                void set_system_parameters(const Kalman &K);
                //! set current state
                void set_state(const vec &xt0){xt=xt0;};
                //! refresh temporary storage 
                //! function for future use which is called at each time td;
                virtual update_state(){};
                //! redesign one step of the 
                void ricatti_step(){
                        pre_qr.set_submatrix(0,0,s*pr);
                        pre_qr.set_submatrix(dimx, dimu+dimx, -Qy*y_req);
                        post_qr=qr(pre_qr);
                        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, dimu, 2*dimu+dimx+dimy);
                };
                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, dimu, 2*dimu+dimx+dimy);
                }
                vec apply(const vec &state, const vec &ukm){vec pom=concat_vertical(state, ones(dimy,1), ukm); return L*pom;}
}