[491] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Base classes for designers of control strategy |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | ----------------------------------- |
---|
| 7 | BDM++ - C++ library for Bayesian Decision Making under Uncertainty |
---|
| 8 | |
---|
| 9 | Using IT++ for numerical operations |
---|
| 10 | ----------------------------------- |
---|
| 11 | */ |
---|
| 12 | |
---|
[508] | 13 | #include "../base/bdmbase.h" |
---|
[586] | 14 | #include "../estim/kalman.h" |
---|
[491] | 15 | |
---|
[508] | 16 | namespace bdm{ |
---|
| 17 | |
---|
[586] | 18 | //! Low-level class for design of control strategy |
---|
| 19 | class Designer { |
---|
| 20 | |
---|
[491] | 21 | public: |
---|
| 22 | //! Redesign control strategy |
---|
[565] | 23 | virtual void redesign() { |
---|
| 24 | bdm_error("Not implemented"); |
---|
| 25 | } |
---|
| 26 | |
---|
[491] | 27 | //! apply control strategy to obtain control input |
---|
[565] | 28 | virtual vec apply(const vec &cond) { |
---|
| 29 | bdm_error("Not implemented"); |
---|
| 30 | return vec(); |
---|
| 31 | } |
---|
[508] | 32 | }; |
---|
[491] | 33 | |
---|
| 34 | //! Linear Quadratic Gaussian designer for constant penalizations and constant target |
---|
| 35 | //! Its internals are very close to Kalman estimator |
---|
| 36 | class LQG : public Designer { |
---|
| 37 | protected: |
---|
[586] | 38 | //! StateSpace model from which we read data |
---|
| 39 | shared_ptr<StateSpace<fsqmat> > S; |
---|
[491] | 40 | //! required value of the output y at time t (assumed constant) |
---|
| 41 | vec y_req; |
---|
[586] | 42 | //! required value of the output y at time t (assumed constant) |
---|
| 43 | vec u_req; |
---|
[491] | 44 | |
---|
| 45 | //! Control horizon, set to maxint for infinite horizons |
---|
| 46 | int horizon; |
---|
| 47 | //! penalization matrix Qy |
---|
| 48 | mat Qy; |
---|
| 49 | //! penalization matrix Qu |
---|
| 50 | mat Qu; |
---|
| 51 | //! time of the design step - from horizon->0 |
---|
| 52 | int td; |
---|
| 53 | //! controller parameters |
---|
| 54 | mat L; |
---|
| 55 | |
---|
[586] | 56 | //!@{ \name temporary storage for ricatti - use initialize |
---|
| 57 | //! convenience parameters |
---|
| 58 | int dimx; |
---|
| 59 | //! convenience parameters |
---|
| 60 | int dimy; |
---|
| 61 | //! convenience parameters |
---|
| 62 | int dimu; |
---|
| 63 | |
---|
[491] | 64 | //! parameters |
---|
| 65 | mat pr; |
---|
| 66 | //! penalization |
---|
| 67 | mat qux; |
---|
| 68 | //! penalization |
---|
| 69 | mat qyx; |
---|
| 70 | //! internal quadratic form |
---|
| 71 | mat s; |
---|
| 72 | //! penalization |
---|
| 73 | mat qy; |
---|
| 74 | //! pre_qr part |
---|
| 75 | mat hqy; |
---|
| 76 | //! pre qr matrix |
---|
| 77 | mat pre_qr; |
---|
| 78 | //! post qr matrix |
---|
| 79 | mat post_qr; |
---|
| 80 | //!@} |
---|
| 81 | |
---|
| 82 | public: |
---|
| 83 | //! set system parameters from given matrices |
---|
[586] | 84 | void set_system(shared_ptr<StateSpace<fsqmat> > S0); |
---|
[508] | 85 | //! set penalization matrices and control horizon |
---|
[586] | 86 | void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0); |
---|
[491] | 87 | //! set system parameters from Kalman filter |
---|
[508] | 88 | // void set_system_parameters(const Kalman &K); |
---|
| 89 | //! refresh temporary storage - inefficient can be improved |
---|
[586] | 90 | void initialize(); |
---|
| 91 | void validate(); |
---|
| 92 | //! function for future use which is called at each time td; Should call initialize()! |
---|
[508] | 93 | virtual void update_state(){}; |
---|
[491] | 94 | //! redesign one step of the |
---|
| 95 | void ricatti_step(){ |
---|
| 96 | pre_qr.set_submatrix(0,0,s*pr); |
---|
[508] | 97 | pre_qr.set_submatrix(dimx+dimu+dimy, dimu+dimx, -Qy*y_req); |
---|
[598] | 98 | if (!qr(pre_qr,post_qr)){ bdm_warning("QR in LQG unstable");} |
---|
[491] | 99 | triu(post_qr); |
---|
| 100 | // hn(m+1:2*m+n+r,m+1:2*m+n+r); |
---|
[508] | 101 | s=post_qr.get(dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1); |
---|
[491] | 102 | }; |
---|
| 103 | void redesign(){ |
---|
| 104 | for(td=horizon; td>0; td--){ |
---|
| 105 | update_state(); |
---|
| 106 | ricatti_step(); |
---|
| 107 | } |
---|
| 108 | /* ws=hn(1:m,m+1:2*m+n+r); |
---|
| 109 | wsd=hn(1:m,1:m); |
---|
| 110 | Lklq=-inv(wsd)*ws;*/ |
---|
[508] | 111 | L = -inv(post_qr.get(0,dimu-1, 0,dimu-1)) * post_qr.get(0,dimu-1, dimu, 2*dimu+dimx+dimy-1); |
---|
[491] | 112 | } |
---|
[508] | 113 | vec apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;} |
---|
| 114 | } ; |
---|
| 115 | |
---|
[586] | 116 | //! Base class for adaptive controllers |
---|
| 117 | //! The base class is however non-adaptive, method \c adapt() is empty. |
---|
| 118 | //! \note advanced Controllers will probably include estimator as their internal attribute (e.g. dual controllers) |
---|
| 119 | class Controller : public root { |
---|
| 120 | //! identifier of the system output; |
---|
| 121 | RV yrv; |
---|
| 122 | //! identifier of the system input; |
---|
| 123 | RV urv; |
---|
| 124 | //! description of data needed for \c ctrlaction , required for automatic connection to DS |
---|
| 125 | RV drv; |
---|
| 126 | public: |
---|
| 127 | //! function processing new observations and adapting control strategy accordingly |
---|
| 128 | virtual void adapt(const vec &data){}; |
---|
| 129 | //! returns designed control action |
---|
| 130 | virtual vec ctrlaction(const vec &data){return vec(0);} |
---|
| 131 | |
---|
| 132 | void from_setting(const Setting &set){ |
---|
| 133 | UI::get(yrv,set,"yrv",UI::optional); |
---|
| 134 | UI::get(yrv,set,"urv",UI::optional); |
---|
| 135 | } |
---|
| 136 | //! access function |
---|
| 137 | const RV& _urv() {return urv;} |
---|
| 138 | //! access function |
---|
| 139 | const RV& _yrv() {return yrv;} |
---|
| 140 | //! access function |
---|
| 141 | const RV& _drv() {return drv;} |
---|
| 142 | }; |
---|
| 143 | |
---|
[565] | 144 | } // namespace |
---|