Changeset 586 for library/bdm

Timestamp:

08/27/09 15:39:35 (15 years ago)

Author:

smidl

Message:

redesign of ctrl LQ control for arx

Location:

library/bdm

Files:

• CMakeLists.txt (modified) (1 diff)
• base/bdmbase.h (modified) (1 diff)
• design/ctrlbase.cpp (modified) (2 diffs)
• design/ctrlbase.h (modified) (5 diffs)
• estim/kalman.h (modified) (5 diffs)
• itpp_ext.cpp (modified) (1 diff)
• itpp_ext.h (modified) (1 diff)

library/bdm/CMakeLists.txt

@@ -22 +22 @@
 
 # Normal BDM library
-add_library (bdm STATIC ${bdm_support} ${bdm_base} ${bdm_math} ${bdm_stat} ${bdm_estim} ${bdm_mex} ${bdm_user_info})
+add_library (bdm STATIC ${bdm_support} ${bdm_base} ${bdm_math} ${bdm_stat} ${bdm_estim} ${bdm_ctrl} ${bdm_mex} ${bdm_user_info})

library/bdm/base/bdmbase.h

@@ -995 +995 @@
                                 set_drv (*r);
                         }
+                        string opt;
+                        if(!UI::get(opt, set, "options", UI::optional)) {
+                                set_options(opt);
+                        }
                 }
 

library/bdm/design/ctrlbase.cpp

@@ -3 +3 @@
 namespace bdm{
 
-void LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){
-        dimx = A0.rows();
-        dimy = C0.rows();
-        dimu = B0.cols();
-
-        bdm_assert_debug ( A0.cols() == dimx, "LQG: A is not square" );
-        bdm_assert_debug ( B0.rows() == dimx, "LQG: B is not compatible" );
-        bdm_assert_debug ( C0.cols() == dimx, "LQG: C is not square" );
-
-        A=A0;
-        B=B0;
-        C=C0;
+void LQG::set_system(shared_ptr<StateSpace<fsqmat> > S0){
+        S = S0;
+        dimx= S->_dimx();
+        dimy= S->_dimy();
+        dimu= S->_dimu();
         pr=zeros(dimx+dimu+dimy,  dimu+dimx+dimu+dimy);
         pr.set_submatrix(dimx, dimu+dimx, eye(dimu+dimy));
 }
 
-void LQG::set_control_parameters(const mat &Qy0, const mat &Qu0, const vec y_req0, int horizon0){
-        bdm_assert_debug ( Qy0.cols() == dimy, "LQG: wrong dimensions of Qy " );
-        bdm_assert_debug ( Qu0.cols() == dimu, "LQG: wrong dimensions of Qu " );
-        bdm_assert_debug ( y_req0.length() == dimy, "LQG: wrong dimensions of y_req " );
-
+void LQG::set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0){
         Qy=Qy0;
         Qu=Qu0;
         y_req=y_req0;
         horizon = horizon0;
-        prepare_qr();
+        validate();
+        initialize();
 }
 
-void LQG::prepare_qr(){
+void LQG::validate() {
+        bdm_assert_debug ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " );
+        bdm_assert_debug ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " );
+        bdm_assert_debug ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " );
+}
+
+void LQG::initialize(){
         // set parameter matrix
-        pr.set_submatrix(0,0,B);
-        pr.set_submatrix(0,dimu, A);
+        pr.set_submatrix(0,0,S->_B());
+        pr.set_submatrix(0,dimu, S->_A());
         
         //penalization
@@ -42 +38 @@
 
         qyx=zeros(dimy, dimx+dimy+dimu);
-        qyx.set_submatrix(0,0,C);
+        qyx.set_submatrix(0,0,S->_C());
         qyx.set_submatrix(0,dimx,-eye(dimy));
         
         //
-        s=1e-5*eye(4);//dimx+dimu+dimy);
+        s=1e-5*eye(dimx+dimu+dimy);
         // parts of QR
         hqy=Qy*qyx*pr;

library/bdm/design/ctrlbase.h

@@ -12 +12 @@
 
 #include "../base/bdmbase.h"
+#include "../estim/kalman.h"
 
 namespace bdm{
 
-//! Base class of designers of control strategy
-class Designer : public root {
+//! Low-level class for design of control strategy
+class Designer  {
+        
         public:
                 //! Redesign control strategy 
@@ -34 +36 @@
 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;
+                //! 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
@@ -61 +54 @@
                 mat L;
                 
-                //!@{ \name temporary storage for ricatti
+                //!@{ \name temporary storage for ricatti - use initialize
+                //! convenience parameters
+                int dimx;
+                //! convenience parameters
+                int dimy;
+                //! convenience parameters
+                int dimu;
+                
                 //!  parameters
                 mat pr;
@@ -82 +82 @@
         public:
                 //! set system parameters from given matrices
-                void set_system_parameters(const mat &A, const mat &B, const mat &C);
+                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);
+                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()!
+                void initialize();
+                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 
@@ -113 +114 @@
         } ;
 
+        //! 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 {
+                //! 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;
+                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;}
+        };
+                        
 } // namespace

library/bdm/estim/kalman.h

@@ -78 +78 @@
                         validate();
                 }               
+                //! access function
+                int _dimx(){return dimx;}
+                //! access function
+                int _dimy(){return dimy;}
+                //! access function
+                int _dimu(){return dimu;}
+                //! access function
+                mat _A(){return A;}
+                //! access function
+                mat _B(){return B;}
+                //! access function
+                mat _C(){return C;}
+                //! access function
+                mat _D(){return D;}
+                //! access function
+                sq_T _Q(){return Q;}
+                //! access function
+                sq_T _R(){return R;}
 };
 
@@ -135 +153 @@
                 void bayes (const vec &dt);
 };
-
+UIREGISTER(KalmanFull);
 
 
@@ -182 +200 @@
                 void bayes (const vec &dt);
                 
-                void from_settings(const Setting &set){
+                void from_setting(const Setting &set){
                         Kalman<chmat>::from_setting(set);
                         initialize();
                 }
 };
+UIREGISTER(KalmanCh);
 
 /*!
@@ -219 +238 @@
                 }
 };
+UIREGISTER(EKFfull);
+
 
 /*!
@@ -330 +351 @@
 SHAREDPTR (MultiModel);
 
+//! conversion of outer ARX model (mlnorm) to state space model 
+/*!
+The model is constructed as:
+\f[ x_{t+1} = Ax_t + B u_t + R^{1/2} e_t, y_t=Cx_t+Du_t + R^{1/2}w_t, \f]
+For example, for:
+\f[ y_t = a y_{t-1} + b u_{t-1}\f]
+the state \f$ x_t = [y_{t-1}, u_{t-1}] \f$
+*/
+template<class sq_T>
+shared_ptr<StateSpace<fsqmat> > to_state_space(const mlnorm<sq_T > &ml, ivec &theta_in_A, ivec &theta_in_C){
+        //get ids of yrv
+                        
+                        ivec yids= unique(ml._rv()._ids()); //yrv is now stored in _yrv
+                        ivec dids= unique(ml._rvc()._ids()); //yrv is now stored in _yrv
+                        ivec uids=unique_complement(dids, yids);
+        
+                        RV yrv_uni = RV(yids,zeros_i(yids.length()));
+                        RV urv_uni = RV(uids,zeros_i(yids.length()));
+                        //We can do only 1d now... :(
+                        bdm_assert_debug(yrv_uni._dsize()==urv_uni._dsize()==1, "Only for SISO so far..." );
+                                        
+                        RV xrv; //empty
+                        RV Crv; //empty
+                        int td=ml._rvc().mintd();
+                        for (int t=-1;t>=td;t--){
+                                xrv.add(yrv_uni);
+                                Crv.add(urv_uni);
+                        }
+                        
+                        int dimx = xrv._dsize();
+                        
+                        theta_in_A = ml._rv().dataind(xrv);
+                        theta_in_C = ml._rvc().dataind(xrv);
+                        // some chcek of corretness
+                        
+                        vec A1row = zeros(xrv._dsize());
+                        vec C1row = zeros(xrv._dsize());
+                        vec theta = ml._A().get_row(0); // this 
+                        set_subvector( A1row, theta_in_A, theta);
+                        set_subvector( C1row, theta_in_C, theta);
+
+                        StateSpace<fsqmat> stsp=new StateSpace<fsqmat>();
+                        mat A=zeros(dimx,dimx);
+                        A.set_row(0,A1row);
+                        for (int j=1; j<dimx; j++){A(j,j-1)=1.0;} // off diagonal
+                        mat B=zeros(dimx,1);
+                        B(0) = 1.0;
+                        mat C=zeros(1,dimx);
+                        C.set_row(0,C1row);
+                        stsp.set_parameters(A,B,C,zeros(1,1), zeros(dimx,dimx), ml._R());
+                        return stsp;
+}
+
 /////////// INSTANTIATION
 

library/bdm/itpp_ext.cpp

@@ -415 +415 @@
 }
 
-}
+ivec unique_complement (const ivec &in, const ivec &base)
+{
+        // almost a copy of unique
+        ivec uniq (0);
+        int j = 0;
+        bool found = false;
+        for (int i = 0;i < in.length(); i++) {
+                found = false;
+                j = 0;
+                while ( (!found) && (j < uniq.length())) {
+                        if (in (i) == uniq (j)) found = true;
+                        j++;
+                }
+                j=0;
+                while ( (!found) && (j < base.length())) {
+                        if (in (i) == uniq (j)) found = true;
+                        j++;
+                }
+                if (!found) uniq = concat (uniq, in (i));
+        }
+        return uniq;
+}
+
+}

library/bdm/itpp_ext.h

@@ -115 +115 @@
 //! function returns unique entries in input vector \c in
 ivec unique(const ivec &in);
+//! function returns unique entries of vector \c in that are not present in vector \c base
+ivec unique_complement(const ivec &in, const ivec &base);
 }