Show
Ignore:
Timestamp:
11/15/09 23:02:02 (15 years ago)
Author:
smidl
Message:

Big commit of LQG stuff

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/bdm/design/ctrlbase.h

    r706 r723  
    1414#include "../estim/kalman.h" 
    1515 
    16 namespace bdm{ 
     16namespace bdm { 
    1717 
    18         //! Base class for adaptive controllers  
    19         //! The base class is, however, non-adaptive, method \c adapt() is empty. 
    20         //! \note advanced Controllers will probably include estimator as their internal attribute (e.g. dual controllers) 
    21         class Controller : public root { 
    22                 protected: 
    23                         //! identifier of the designed action; 
    24                         RV rv; 
    25                         //! identifier of the conditioning variables; 
    26                         RV rvc; 
    27                 public: 
    28                         //! function processing new observations and adapting control strategy accordingly 
    29                         virtual void adapt(const vec &data){}; 
    30                         //! function redesigning the control strategy  
    31                         virtual void redesign(){}; 
    32                         //! returns designed control action 
    33                         virtual vec ctrlaction(const vec &cond){return vec(0);} 
    34                          
    35                         void from_setting(const Setting &set){ 
    36                                 UI::get(rv,set,"rv",UI::optional); 
    37                                 UI::get(rvc,set,"rvc",UI::optional); 
    38                         } 
    39                         //! access function 
    40                         const RV& _rv() {return rv;} 
    41                         //! access function 
    42                         const RV& _rvc() {return rvc;} 
    43                         //! register this controller with given datasource under name "name" 
    44                         virtual void log_register (logger &L, const string &prefix ) { } 
    45                         //! write requested values into the logger 
    46                         virtual void log_write ( ) const { } 
    47                          
    48         }; 
    49          
     18//! Base class for adaptive controllers 
     19//! The base class is, however, non-adaptive, method \c adapt() is empty. 
     20//! \note advanced Controllers will probably include estimator as their internal attribute (e.g. dual controllers) 
     21class Controller : public root { 
     22protected: 
     23        //! identifier of the designed action; 
     24        RV rv; 
     25        //! identifier of the conditioning variables - data needed ; 
     26        RV rvc; 
     27public: 
     28        //! function processing new observations and adapting control strategy accordingly 
     29        virtual void adapt ( const vec &data ) {}; 
     30        //! function redesigning the control strategy 
     31        virtual void redesign() {}; 
     32        //! returns designed control action 
     33        virtual vec ctrlaction ( const vec &cond ) const {return vec ( 0 );} 
     34 
     35        void from_setting ( const Setting &set ) { 
     36                UI::get ( rv,set,"rv",UI::optional ); 
     37                UI::get ( rvc,set,"rvc",UI::optional ); 
     38        } 
     39        //! access function 
     40        const RV& _rv() {return rv;} 
     41        //! access function 
     42        const RV& _rvc() {return rvc;} 
     43        //! register this controller with given datasource under name "name" 
     44        virtual void log_register ( logger &L, const string &prefix ) { } 
     45        //! write requested values into the logger 
     46        virtual void log_write ( ) const { } 
     47 
     48}; 
     49 
    5050//! Linear Quadratic Gaussian designer for constant penalizations and constant target 
    5151//! Its internals are very close to Kalman estimator 
    5252class LQG : public Controller { 
    53         protected: 
    54                 //! StateSpace model from which we read data 
    55                 shared_ptr<StateSpace<fsqmat> > S; 
    56                 //! required value of the output y at time t (assumed constant) 
    57                 vec y_req; 
    58                 //! required value of the output y at time t (assumed constant) 
    59                 vec u_req; 
    60                  
    61                 //! Control horizon, set to maxint for infinite horizons 
    62                 int horizon; 
    63                 //! penalization matrix Qy 
    64                 mat Qy; 
    65                 //! penalization matrix Qu 
    66                 mat Qu; 
    67                 //! time of the design step - from horizon->0 
    68                 int td; 
    69                 //! controller parameters 
    70                 mat L; 
    71                  
    72                 //!@{ \name temporary storage for ricatti - use initialize 
    73                 //! convenience parameters 
    74                 int dimx; 
    75                 //! convenience parameters 
    76                 int dimy; 
    77                 //! convenience parameters 
    78                 int dimu; 
    79                  
    80                 //!  parameters 
    81                 mat pr; 
    82                 //! penalization  
    83                 mat qux; 
    84                 //! penalization 
    85                 mat qyx; 
    86                 //! internal quadratic form  
    87                 mat s; 
    88                 //! penalization 
    89                 mat qy; 
    90                 //! pre_qr part 
    91                 mat hqy; 
    92                 //! pre qr matrix 
    93                 mat pre_qr; 
    94                 //! post qr matrix 
    95                 mat post_qr; 
    96                 //!@} 
    97                  
    98         public: 
    99                 //! set system parameters from given matrices 
    100                 void set_system(shared_ptr<StateSpace<fsqmat> > S0); 
    101                 //! set penalization matrices and control horizon 
    102                 void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0); 
    103                 //! set system parameters from Kalman filter 
    104 //              void set_system_parameters(const Kalman &K); 
    105                 //! refresh temporary storage - inefficient can be improved 
    106                 void initialize(); 
    107                 //! validation procedure 
    108                 void validate(); 
    109                 //! function for future use which is called at each time td; Should call initialize()! 
    110                 virtual void update_state(){}; 
    111                 //! redesign one step of the  
    112                 void ricatti_step(){ 
    113                         pre_qr.set_submatrix(0,0,s*pr); 
    114                         pre_qr.set_submatrix(dimx+dimu+dimy, dimu+dimx, -Qy*y_req); 
    115                         if (!qr(pre_qr,post_qr)){ bdm_warning("QR in LQG unstable");} 
    116                         triu(post_qr); 
    117         // hn(m+1:2*m+n+r,m+1:2*m+n+r); 
    118                         s=post_qr.get(dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1); 
    119                 }; 
    120                 void redesign(){ 
    121                         for(td=horizon; td>0; td--){ 
    122                                 update_state(); 
    123                                 ricatti_step(); 
    124                         } 
    125 /*                      ws=hn(1:m,m+1:2*m+n+r); 
    126                         wsd=hn(1:m,1:m); 
    127                         Lklq=-inv(wsd)*ws;*/ 
    128                         L = -inv(post_qr.get(0,dimu-1, 0,dimu-1)) * post_qr.get(0,dimu-1, dimu, 2*dimu+dimx+dimy-1); 
     53protected: 
     54        //! StateSpace model from which we read data 
     55        shared_ptr<StateSpace<chmat> > S; 
     56        //! required value of the output y at time t (assumed constant) 
     57        vec y_req; 
     58        //! required value of the output y at time t (assumed constant) 
     59        vec u_req; 
     60 
     61        //! Control horizon, set to maxint for infinite horizons 
     62        int horizon; 
     63        //! penalization matrix Qy 
     64        chmat Qy; 
     65        //! penalization matrix Qu 
     66        chmat Qu; 
     67        //! time of the design step - from horizon->0 
     68        int td; 
     69        //! controller parameters 
     70        mat L; 
     71 
     72        //!@{ \name temporary storage for ricatti - use initialize 
     73        //! convenience parameters 
     74        int dimx; 
     75        //! convenience parameters 
     76        int dimy; 
     77        //! convenience parameters 
     78        int dimu; 
     79 
     80        //!  parameters 
     81        mat pr; 
     82        //! penalization 
     83        mat qux; 
     84        //! penalization 
     85        mat qyx; 
     86        //! internal quadratic form 
     87        mat s; 
     88        //! penalization 
     89        mat qy; 
     90        //! pre_qr part 
     91        mat hqy; 
     92        //! pre qr matrix 
     93        mat pre_qr; 
     94        //! post qr matrix 
     95        mat post_qr; 
     96        //!@} 
     97 
     98public: 
     99        //! set system parameters from given matrices 
     100        void set_system ( shared_ptr<StateSpace<chmat> > S0 ); 
     101        //! update internal whan system has changed 
     102        void update_system() { 
     103                pr.set_submatrix ( 0,0,S->_B() ); 
     104                pr.set_submatrix ( 0,dimu, S->_A() ); 
     105 
     106                //penalization 
     107                qux.set_submatrix ( 0,0, Qu._Ch() ); 
     108                qux.set_submatrix ( 0,dimx+dimu+dimy,Qu._Ch() ); 
     109 
     110                qyx.set_submatrix ( 0,0,S->_C() ); 
     111                qyx.set_submatrix ( 0,dimx,-eye ( dimy ) ); 
     112 
     113                // parts of QR 
     114                hqy=Qy.to_mat()*qyx*pr; 
     115 
     116                // pre_qr 
     117                pre_qr = concat_vertical ( s*pr, concat_vertical ( hqy, qux ) ); 
     118        } 
     119        //! set penalization matrices and control horizon 
     120        void set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ); 
     121        //! set penalization matrices and control horizon 
     122        void set_control_Qy ( const mat &Qy0 ) {Qy=Qy0;} 
     123        //! refresh temporary storage - inefficient can be improved 
     124        void initial_belmann() {s=1e-5*eye ( dimx+dimu+dimy );}; 
     125        //! validation procedure 
     126        void validate(); 
     127        //! function for future use which is called at each time td; Should call initialize()! 
     128        //! redesign one step of the 
     129        void ricatti_step() { 
     130                pre_qr.set_submatrix ( 0,0,s*pr ); 
     131                pre_qr.set_submatrix ( dimx+dimu+dimy, dimu+dimx, -Qy.to_mat()*y_req ); 
     132                if ( !qr ( pre_qr,post_qr ) ) { bdm_warning ( "QR in LQG unstable" );} 
     133                triu ( post_qr ); 
     134                // hn(m+1:2*m+n+r,m+1:2*m+n+r); 
     135                s=post_qr.get ( dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1 ); 
     136        }; 
     137        void redesign() { 
     138                for ( td=horizon; td>0; td-- ) { 
     139                        update_system(); 
     140                        ricatti_step(); 
    129141                } 
    130                 //! compute control action 
    131                 vec ctrlaction(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;} 
    132         } ; 
     142                /*                      ws=hn(1:m,m+1:2*m+n+r); 
     143                                        wsd=hn(1:m,1:m); 
     144                                        Lklq=-inv(wsd)*ws;*/ 
     145                L = -inv ( post_qr.get ( 0,dimu-1, 0,dimu-1 ) ) * post_qr.get ( 0,dimu-1, dimu, 2*dimu+dimx+dimy-1 ); 
     146        } 
     147        //! compute control action 
     148        vec ctrlaction ( const vec &state, const vec &ukm ) const { 
     149                vec pom=concat ( state, ones ( dimy ), ukm ); 
     150                return L*pom; 
     151        } 
     152} ; 
    133153 
    134                          
     154 
    135155} // namespace