[508] | 1 | #include "ctrlbase.h" |
---|
[491] | 2 | |
---|
[737] | 3 | namespace bdm { |
---|
[508] | 4 | |
---|
[737] | 5 | void LQG::set_system ( shared_ptr<StateSpace<chmat> > S0 ) { |
---|
[1064] | 6 | S = S0; |
---|
[508] | 7 | } |
---|
| 8 | |
---|
[738] | 9 | void LQG::update_system() { |
---|
[1064] | 10 | pr.set_submatrix ( 0, 0, S->_B() ); |
---|
| 11 | pr.set_submatrix ( 0, dimu, S->_A() ); |
---|
[738] | 12 | |
---|
[1064] | 13 | //penalization |
---|
| 14 | qux.set_submatrix ( 0, 0, Qu._Ch() ); |
---|
| 15 | qux.set_submatrix ( 0, dimx + dimu + dimy, -Qu._Ch() ); |
---|
[738] | 16 | |
---|
[1064] | 17 | qyx.set_submatrix ( 0, 0, S->_C() ); |
---|
| 18 | qyx.set_submatrix ( 0, dimx, -eye ( dimy ) ); |
---|
[738] | 19 | |
---|
[1064] | 20 | // parts of QR |
---|
| 21 | hqy = Qy.to_mat() * qyx * pr; |
---|
[738] | 22 | |
---|
[1064] | 23 | // pre_qr |
---|
| 24 | pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) ); |
---|
[738] | 25 | } |
---|
[1064] | 26 | |
---|
[737] | 27 | void LQG::set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ) { |
---|
[1064] | 28 | Qy = Qy0; |
---|
| 29 | Qu = Qu0; |
---|
| 30 | y_req = y_req0; |
---|
| 31 | horizon = horizon0; |
---|
[508] | 32 | } |
---|
| 33 | |
---|
[586] | 34 | void LQG::validate() { |
---|
[1064] | 35 | // set properties from S |
---|
| 36 | dimx = S->_A().rows(); |
---|
| 37 | dimy = S->_C().rows(); |
---|
| 38 | dimu = S->_B().cols(); |
---|
| 39 | pr = zeros ( dimx + dimu + dimy, dimu + dimx + dimu + dimy ); |
---|
| 40 | pr.set_submatrix ( dimx, dimu + dimx, eye ( dimu + dimy ) ); |
---|
[737] | 41 | |
---|
[1064] | 42 | //penalization |
---|
| 43 | bdm_assert ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " ); |
---|
| 44 | bdm_assert ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " ); |
---|
| 45 | bdm_assert ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " ); |
---|
[737] | 46 | |
---|
[1064] | 47 | qux = zeros ( dimu, dimx + 2 * dimu + dimy ); |
---|
| 48 | qyx = zeros ( dimy, dimx + dimy + dimu ); |
---|
[737] | 49 | |
---|
[1064] | 50 | // |
---|
| 51 | initial_belmann(); |
---|
| 52 | // parts of QR |
---|
| 53 | post_qr = zeros ( pre_qr.rows(), pre_qr.cols() ); |
---|
[737] | 54 | |
---|
[1064] | 55 | update_system(); |
---|
[508] | 56 | } |
---|
| 57 | |
---|
[738] | 58 | void LQG::ricatti_step() { |
---|
[744] | 59 | // pre_qr.set_submatrix ( 0, 0, s*pr ); |
---|
| 60 | // pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req ); |
---|
[1064] | 61 | if ( !qr ( pre_qr, post_qr ) ) { |
---|
| 62 | bdm_warning ( "QR in LQG unstable" ); |
---|
| 63 | } |
---|
| 64 | triu ( post_qr ); |
---|
| 65 | // hn(m+1:2*m+n+r,m+1:2*m+n+r); |
---|
| 66 | s = post_qr.get ( dimu, 2 * dimu + dimx + dimy - 1, dimu, 2 * dimu + dimx + dimy - 1 ); |
---|
[738] | 67 | }; |
---|
[723] | 68 | |
---|
[738] | 69 | void LQG::redesign() { |
---|
[1064] | 70 | for ( td = horizon; td > 0; td-- ) { |
---|
| 71 | update_system(); |
---|
| 72 | ricatti_step(); |
---|
| 73 | } |
---|
| 74 | /* ws=hn(1:m,m+1:2*m+n+r); |
---|
| 75 | wsd=hn(1:m,1:m); |
---|
| 76 | Lklq=-inv(wsd)*ws;*/ |
---|
| 77 | L = -inv ( post_qr.get ( 0, dimu - 1, 0, dimu - 1 ) ) * post_qr.get ( 0, dimu - 1, dimu, 2 * dimu + dimx + dimy - 1 ); |
---|
[565] | 78 | } |
---|
[738] | 79 | |
---|
| 80 | |
---|
| 81 | } |
---|