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

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

namespace bdm{

//! Base class of designers of control strategy
class Designer : public root {
        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:
                //! 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;
                //! 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 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 prepare_qr();
                //! function for future use which is called at each time td; Should call prepare_qr()!
                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);
                }
                vec apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
        } ;

} // namespace