Changeset 737 for library/bdm/design
- Timestamp:
- 11/25/09 12:14:38 (15 years ago)
- Location:
- library/bdm/design
- Files:
-
- 3 modified
-
arx_ctrl.h (modified) (3 diffs)
-
ctrlbase.cpp (modified) (2 diffs)
-
ctrlbase.h (modified) (4 diffs)
Legend:
- Unmodified
- Added
- Removed
-
library/bdm/design/arx_ctrl.h
r723 r737 17 17 //#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h> 18 18 19 namespace bdm {19 namespace bdm { 20 20 21 //! Controller using ARX model for estimation and LQG designer for control 22 class LQG_ARX : public Controller{ 23 protected: 24 //! Internal ARX estimator 25 shared_ptr<ARX> ar; 26 //! Internal LQG designer 27 LQG lq; 28 //! Intermediate StateSpace model 29 shared_ptr<StateFromARX> Stsp; 30 //! AR predictor 31 shared_ptr<mlnorm<chmat> > pred; 32 //! datalink from rvc to ar.bayes 33 datalink rvc2ar_y; 34 //! datalink from rvc to ar.bayes 35 datalink_buffered rvc2ar_cond; 36 37 //! flag to use iterations spread in time (ist) 38 bool ist; 39 //! flag to use windsurfer approach 40 bool windsurfer; 41 public: 42 //! Default constructor 43 LQG_ARX(): ar(), lq() { } 21 //! Controller using ARX model for estimation and LQG designer for control 22 class LQG_ARX : public Controller { 23 protected: 24 //! Internal ARX estimator 25 shared_ptr<ARX> ar; 26 //! Internal LQG designer 27 LQG lq; 28 //! Intermediate StateSpace model 29 shared_ptr<StateFromARX> Stsp; 30 //! AR predictor 31 shared_ptr<mlnorm<chmat> > pred; 32 //! datalink from rvc to ar.bayes 33 datalink rvc2ar_y; 34 //! datalink from rvc to ar.bayes 35 datalink_buffered rvc2ar_cond; 44 36 45 //! adaptation is to store arx estimates in stsp 46 void adapt(const vec &data){ 47 ar->bayes(rvc2ar_y.pushdown(data), rvc2ar_cond.pushdown(data)); 48 rvc2ar_cond.store_data(data); 49 } 50 void redesign(){ 51 ar->ml_predictor_update(*pred); 52 Stsp->update_from(*pred); 53 if (windsurfer) { 54 mat Ry=pred->_R(); 55 lq.set_control_Qy(inv(Ry)); 56 } 57 if (!ist) { 58 lq.initial_belmann(); 59 } 60 lq.redesign(); 61 } 62 vec ctrlaction(const vec &cond) const{ 63 //cond is xt + ut 64 vec state=cond.left(Stsp->_A().rows()); 65 if (Stsp->_have_constant()){ 66 state(state.length()-1) = 1; 67 } 68 return lq.ctrlaction(state,cond.right(Stsp->_B().cols())); 69 } 70 //! 37 //! flag to use iterations spread in time (ist) 38 bool ist; 39 //! flag to use windsurfer approach 40 bool windsurfer; 41 public: 42 //! Default constructor 43 LQG_ARX() : ar(), lq() { } 44 45 //! adaptation is to store arx estimates in stsp 46 void adapt ( const vec &data ) { 47 ar->bayes ( rvc2ar_y.pushdown ( data ), rvc2ar_cond.pushdown ( data ) ); 48 rvc2ar_cond.store_data ( data ); 49 } 50 void redesign() { 51 ar->ml_predictor_update ( *pred ); 52 Stsp->update_from ( *pred ); 53 if ( windsurfer ) { 54 mat Ry = pred->_R(); 55 lq.set_control_Qy ( inv ( Ry ) ); 56 } 57 if ( !ist ) { 58 lq.initial_belmann(); 59 } 60 lq.redesign(); 61 } 62 vec ctrlaction ( const vec &cond ) const { 63 //cond is xt + ut 64 vec state = cond.left ( Stsp->_A().rows() ); 65 if ( Stsp->_have_constant() ) { 66 state ( state.length() - 1 ) = 1; 67 } 68 return lq.ctrlaction ( state, cond.right ( Stsp->_B().cols() ) ); 69 } 70 //! 71 71 //! LQG is defined by quadratic loss function 72 72 /*! \f[ L(y,u) = (y-y_{req})'Q_y (y-y_{req}) + (u-u_{req})' Q_u (u-u_{req}) \f] … … 74 74 \code 75 75 { class="LQG"; 76 arx = {class="ARX", ...} // internal arx model, see 77 Qy = ("matrix", ...); 78 Qu = ("matrix", ...); 76 arx = {class="ARX", ...} // internal arx model, see 77 Qy = ("matrix", ...); 78 Qu = ("matrix", ...); 79 79 yreq = []; // requested output 80 80 ureq = []; // requested input value … … 82 82 \endcode 83 83 */ 84 void from_setting(const Setting &set){85 ar=UI::build<ARX> (set, "ARX",UI::compulsory);84 void from_setting ( const Setting &set ) { 85 ar = UI::build<ARX> ( set, "ARX", UI::compulsory ); 86 86 87 mat Qu; 88 mat Qy; 89 90 UI::get(Qu, set, "Qu", UI::compulsory); 91 UI::get(Qy, set, "Qy", UI::compulsory); 92 93 vec y_req; 94 if (!UI::get(y_req, set, "yreq", UI::optional)) 95 y_req = zeros(ar->_yrv()._dsize()); 96 97 int horizon; 98 UI::get(horizon, set, "horizon", UI::compulsory); 99 lq.set_control_parameters(Qy,Qu,y_req,horizon); 100 101 int wind; 102 if (UI::get(wind, set, "windsurfer", UI::optional)){ 103 windsurfer=wind>0; 104 } else { 105 windsurfer=false; 106 }; 107 int ist_; 108 if (UI::get(ist_, set, "ist", UI::optional)){ 109 ist=ist_>0; 110 } else { 111 ist=false; 112 }; 113 validate(); 114 } 87 mat Qu; 88 mat Qy; 115 89 116 void validate() { 117 // ar is valid 118 pred = ar->ml_predictor<chmat>(); 119 Stsp = new StateFromARX; 120 RV xrv; 121 RV urvm; //old ut 122 Stsp->connect_mlnorm(*pred, xrv, urvm); 123 lq.set_system(Stsp); 124 lq.validate(); 125 126 rvc =concat(xrv, urvm); 127 rvc2ar_y.set_connection(ar->_yrv(), rvc); 128 rvc2ar_cond.set_connection(ar->_rvc(), rvc); 129 //datalink from ARX to rvc 130 } 131 void log_register (logger &L, const string &prefix ) { 132 ar->log_register(L,prefix); 133 } 134 void log_write ( ) const { 135 ar->log_write(); 136 } 137 138 }; 139 UIREGISTER(LQG_ARX); 90 UI::get ( Qu, set, "Qu", UI::compulsory ); 91 UI::get ( Qy, set, "Qy", UI::compulsory ); 92 93 vec y_req; 94 if ( !UI::get ( y_req, set, "yreq", UI::optional ) ) 95 y_req = zeros ( ar->_yrv()._dsize() ); 96 97 int horizon; 98 UI::get ( horizon, set, "horizon", UI::compulsory ); 99 lq.set_control_parameters ( Qy, Qu, y_req, horizon ); 100 101 int wind; 102 if ( UI::get ( wind, set, "windsurfer", UI::optional ) ) { 103 windsurfer = wind > 0; 104 } else { 105 windsurfer = false; 106 }; 107 int ist_; 108 if ( UI::get ( ist_, set, "ist", UI::optional ) ) { 109 ist = ist_ > 0; 110 } else { 111 ist = false; 112 }; 113 validate(); 114 } 115 116 void validate() { 117 // ar is valid 118 pred = ar->ml_predictor<chmat>(); 119 Stsp = new StateFromARX; 120 RV xrv; 121 RV urvm; //old ut 122 Stsp->connect_mlnorm ( *pred, xrv, urvm ); 123 lq.set_system ( Stsp ); 124 lq.validate(); 125 126 rvc = concat ( xrv, urvm ); 127 rvc2ar_y.set_connection ( ar->_yrv(), rvc ); 128 rvc2ar_cond.set_connection ( ar->_rvc(), rvc ); 129 //datalink from ARX to rvc 130 } 131 void log_register ( logger &L, const string &prefix ) { 132 ar->log_register ( L, prefix ); 133 } 134 void log_write ( ) const { 135 ar->log_write(); 136 } 137 138 }; 139 UIREGISTER ( LQG_ARX ); 140 140 } // namespace -
library/bdm/design/ctrlbase.cpp
r723 r737 1 1 #include "ctrlbase.h" 2 2 3 namespace bdm {3 namespace bdm { 4 4 5 void LQG::set_system (shared_ptr<StateSpace<chmat> > S0){5 void LQG::set_system ( shared_ptr<StateSpace<chmat> > S0 ) { 6 6 S = S0; 7 7 } 8 8 9 void LQG::set_control_parameters (const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0){10 Qy =Qy0;11 Qu =Qu0;12 y_req =y_req0;9 void LQG::set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ) { 10 Qy = Qy0; 11 Qu = Qu0; 12 y_req = y_req0; 13 13 horizon = horizon0; 14 14 } … … 16 16 void LQG::validate() { 17 17 // set properties from S 18 dimx = S->_A().rows();19 dimy = S->_C().rows();20 dimu = S->_B().cols();21 pr =zeros(dimx+dimu+dimy, dimu+dimx+dimu+dimy);22 pr.set_submatrix (dimx, dimu+dimx, eye(dimu+dimy));23 18 dimx = S->_A().rows(); 19 dimy = S->_C().rows(); 20 dimu = S->_B().cols(); 21 pr = zeros ( dimx + dimu + dimy, dimu + dimx + dimu + dimy ); 22 pr.set_submatrix ( dimx, dimu + dimx, eye ( dimu + dimy ) ); 23 24 24 //penalization 25 25 bdm_assert ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " ); 26 26 bdm_assert ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " ); 27 27 bdm_assert ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " ); 28 29 qux =zeros(dimu,dimx+2*dimu+dimy);30 qyx =zeros(dimy, dimx+dimy+dimu);31 28 29 qux = zeros ( dimu, dimx + 2 * dimu + dimy ); 30 qyx = zeros ( dimy, dimx + dimy + dimu ); 31 32 32 // 33 33 initial_belmann(); 34 34 // parts of QR 35 post_qr = zeros (pre_qr.rows(), pre_qr.cols());36 35 post_qr = zeros ( pre_qr.rows(), pre_qr.cols() ); 36 37 37 update_system(); 38 38 } -
library/bdm/design/ctrlbase.h
r733 r737 31 31 virtual void redesign() {}; 32 32 //! returns designed control action 33 virtual vec ctrlaction ( const vec &cond ) const {return vec ( 0 );} 33 virtual vec ctrlaction ( const vec &cond ) const { 34 return vec ( 0 ); 35 } 34 36 35 37 void from_setting ( const Setting &set ) { 36 UI::get ( rv, set,"rv",UI::optional );37 UI::get ( rvc, set,"rvc",UI::optional );38 UI::get ( rv, set, "rv", UI::optional ); 39 UI::get ( rvc, set, "rvc", UI::optional ); 38 40 } 39 41 //! access function 40 const RV& _rv() {return rv;} 42 const RV& _rv() { 43 return rv; 44 } 41 45 //! access function 42 const RV& _rvc() {return rvc;} 46 const RV& _rvc() { 47 return rvc; 48 } 43 49 //! register this controller with given datasource under name "name" 44 50 virtual void log_register ( logger &L, const string &prefix ) { } … … 101 107 //! update internal whan system has changed 102 108 void update_system() { 103 pr.set_submatrix ( 0, 0,S->_B() );104 pr.set_submatrix ( 0, dimu, S->_A() );109 pr.set_submatrix ( 0, 0, S->_B() ); 110 pr.set_submatrix ( 0, dimu, S->_A() ); 105 111 106 112 //penalization 107 qux.set_submatrix ( 0, 0, Qu._Ch() );108 qux.set_submatrix ( 0, dimx+dimu+dimy,Qu._Ch() );113 qux.set_submatrix ( 0, 0, Qu._Ch() ); 114 qux.set_submatrix ( 0, dimx + dimu + dimy, Qu._Ch() ); 109 115 110 qyx.set_submatrix ( 0, 0,S->_C() );111 qyx.set_submatrix ( 0, dimx,-eye ( dimy ) );116 qyx.set_submatrix ( 0, 0, S->_C() ); 117 qyx.set_submatrix ( 0, dimx, -eye ( dimy ) ); 112 118 113 119 // parts of QR 114 hqy =Qy.to_mat()*qyx*pr;120 hqy = Qy.to_mat() * qyx * pr; 115 121 116 122 // pre_qr 117 pre_qr = concat_vertical ( s *pr, concat_vertical ( hqy, qux ) );123 pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) ); 118 124 } 119 125 //! set penalization matrices and control horizon 120 126 void set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ); 121 127 //! set penalization matrices and control horizon 122 void set_control_Qy ( const mat &Qy0 ) {Qy=Qy0;} 128 void set_control_Qy ( const mat &Qy0 ) { 129 Qy = Qy0; 130 } 123 131 //! refresh temporary storage - inefficient can be improved 124 void initial_belmann() {s=1e-5*eye ( dimx+dimu+dimy );}; 132 void initial_belmann() { 133 s = 1e-5 * eye ( dimx + dimu + dimy ); 134 }; 125 135 //! validation procedure 126 136 void validate(); … … 128 138 //! redesign one step of the 129 139 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" );} 140 pre_qr.set_submatrix ( 0, 0, s*pr ); 141 pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req ); 142 if ( !qr ( pre_qr, post_qr ) ) { 143 bdm_warning ( "QR in LQG unstable" ); 144 } 133 145 triu ( post_qr ); 134 146 // 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 );147 s = post_qr.get ( dimu, 2 * dimu + dimx + dimy - 1, dimu, 2 * dimu + dimx + dimy - 1 ); 136 148 }; 137 149 void redesign() { 138 for ( td =horizon; td>0; td-- ) {150 for ( td = horizon; td > 0; td-- ) { 139 151 update_system(); 140 152 ricatti_step(); … … 143 155 wsd=hn(1:m,1:m); 144 156 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 );157 L = -inv ( post_qr.get ( 0, dimu - 1, 0, dimu - 1 ) ) * post_qr.get ( 0, dimu - 1, dimu, 2 * dimu + dimx + dimy - 1 ); 146 158 } 147 159 //! compute control action 148 160 vec ctrlaction ( const vec &state, const vec &ukm ) const { 149 vec pom =concat ( state, ones ( dimy ), ukm );161 vec pom = concat ( state, ones ( dimy ), ukm ); 150 162 return L*pom; 151 163 } 152 164 //! access function 153 mat _L() const {return L;} 165 mat _L() const { 166 return L; 167 } 154 168 } ; 155 169
