Changeset 586

Timestamp:

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

Author:

smidl

Message:

redesign of ctrl LQ control for arx

Files:

• applications/bdmtoolbox/tutorial/estimation/arx_test.m (modified) (2 diffs)
• library/bdm/CMakeLists.txt (modified) (1 diff)
• library/bdm/base/bdmbase.h (modified) (1 diff)
• library/bdm/design/ctrlbase.cpp (modified) (2 diffs)
• library/bdm/design/ctrlbase.h (modified) (5 diffs)
• library/bdm/estim/kalman.h (modified) (5 diffs)
• library/bdm/itpp_ext.cpp (modified) (1 diff)
• library/bdm/itpp_ext.h (modified) (1 diff)
• library/tests/CMakeLists.txt (modified) (1 diff)
• library/tests/LQG_test.cpp (added)
• library/tests/rv_test.cpp (modified) (1 diff)

applications/bdmtoolbox/tutorial/estimation/arx_test.m

@@ +1 @@
+theta = [0.8, -0.3, 0.4, 1.0;...
+         0.0, 0.0, 0.0, 0.0];
+R = [0.1, 0.0; 0.0, 1.0];
+
+
 y = struct('class','RV','names',{{'y'}}); 
+y.class = 'RV';
+y.names= {'y'};
+
 u = struct('class','RV','names',{{'u'}}); 
-yu = struct('class','RV','names',{{'y','u'}}); 
+%yu = struct('class','RV','names',{{'y','u'}}); 
 rgr= struct('class','RV',...
         'names',{{'y', 'y','y','u'}},...
@@ -12 +20 @@
         'u',  struct('class','RV','names',{{}}),...
         'rgr', rgr,...
-        'theta', [0.8, -0.3, 0.4, 1.0;
-              0.0, 0.0, 0.0, 0.0],...
+        'theta', theta,...
         'offset',  [0.0, 0.0],...
-        'r', [0.1, 0.0; 0.0, 1.0],...
+        'r', R,...
         'opt', 'L_theta');
 
-%store results
-logger = struct(...
-        'class','dirfilelog',...
-        'dirname','arx_test',...
-        'maxlen',  1000);
+% store results
+% logger = struct(...
+%       'class','dirfilelog',...
+%       'dirname','arx_test',...
+%       'maxlen',  1000);
 
-estimators = {
-        struct('class','ARX',...
-        'y', y,...
-        'rgr', rgr)
-};
+A1.class = 'ARX';
+A1.y = y;
+A1.rgr = rgr;
+A1.options ='logbounds,logll';
+
+estimators = { A1};
 
 %experiment description
-experiment.ndat = 9000;
+ndat = 90;
+experiment.ndat = ndat;
 
 M=estimator(system,estimators,experiment);%,logger);
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% plot results
+
+subplot(1,2,1);
+hold off
+plot([M.tth(1:ndat,1:2:end) ],'-.');
+title(' Regression parameters \theta')
+hold on
+plot(M.mean_theta(1:ndat,:));
+co = get(gca,'ColorOrder');
+for i=1:4
+    ind =1:10:ndat;
+    h=errorbar(ind,M.mean_theta(ind,i),...
+    M.mean_theta(ind,i)-M.lb_theta(ind,i),M.mean_theta(ind,i)-M.ub_theta(ind,i),'.');
+    set(h,'color',co(i,:));
+end
+
+set(gca,'YLim',[-0.7,2]);
+
+subplot(1,2,2);
+hold off
+plot([M.rR(1:ndat) ],'-.');
+title('Variance parameters r')
+hold on
+plot(M.mean_r(1:ndat,:));
+co = get(gca,'ColorOrder');
+ind =1:10:ndat;
+for i=1:size(M.mean_r, 2)
+    h=errorbar(ind,M.mean_r(ind,i),...
+    M.mean_r(ind,i)-M.lb_r(ind,i),M.mean_r(ind,i)-M.ub_r(ind,i),'.');
+    set(h,'color',co(i,:));
+end

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);
 }
 

library/tests/CMakeLists.txt

@@ -39 +39 @@
 
 # using UnitTest++
-add_executable(testsuite datalink_test.cpp egiw_test.cpp emix_test.cpp epdf_test.cpp logger_test.cpp merger_test.cpp 
+add_executable(testsuite datalink_test.cpp egiw_test.cpp emix_test.cpp epdf_test.cpp logger_test.cpp LQG_test.cpp merger_test.cpp 
         mpdf_test.cpp randun_test.cpp rectangular_support_test.cpp rv_test.cpp shared_ptr_test.cpp square_mat_test.cpp testsuite.cpp user_info_test.cpp)
 target_link_libraries(testsuite testutil unittest)

library/tests/rv_test.cpp

@@ -142 +142 @@
         join.add(tmp);
         
-        CHECK_EQUAL(unique(join._ids()), vec_2(a.id(0), b.id(0)));
+        CHECK_EQUAL(unique(join._ids()), vec_2(a.id(0), b.id(0))); // find only ids of a and b
+        CHECK_EQUAL(unique_complement(join._ids(), vec_1(a.id(0))), vec_1(b.id(0))); // complemnet of a in previous is b
 }