|
Revision 491, 0.8 kB
(checked in by smidl, 15 years ago)
|
|
First attempt at #3 and #11
|
| Line | |
|---|
| 1 | #include <ctrlbase.h> |
|---|
| 2 | |
|---|
| 3 | LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){ |
|---|
| 4 | dimx = A0.rows(); |
|---|
| 5 | dimy = C0.rows(); |
|---|
| 6 | dimu = B0.cols(); |
|---|
| 7 | |
|---|
| 8 | it_assert_debug ( A0.cols() == dimx, "LQG: A is not square" ); |
|---|
| 9 | it_assert_debug ( B0.rows() == dimx, "LQG: B is not compatible" ); |
|---|
| 10 | it_assert_debug ( C0.cols() == dimx, "LQG: C is not square" ); |
|---|
| 11 | |
|---|
| 12 | A=A0; |
|---|
| 13 | B=B0; |
|---|
| 14 | C=C0; |
|---|
| 15 | |
|---|
| 16 | // set temporary stuff; |
|---|
| 17 | pr = concat_vertical( |
|---|
| 18 | concat_horizontal(B,A, zeros(dimx, dimu+dimy)), |
|---|
| 19 | concat_horizontal(zeros(dimu+dimy,dimu+dimx), eye(dimu+dimy))); |
|---|
| 20 | |
|---|
| 21 | //penalization |
|---|
| 22 | qux=zeros(dimu,dimx+2*dimu+dimy); |
|---|
| 23 | qux.set_submatrix(0,0,Qu); |
|---|
| 24 | qux.set_submatrix(0,dimx+dimu+dimy,Qu); |
|---|
| 25 | |
|---|
| 26 | qyx=concat_horizontal(C,-eye(dimy),zeros(dimy,dimu)); |
|---|
| 27 | |
|---|
| 28 | // |
|---|
| 29 | s=1e-5*eye(dimx+dimu+dimy); |
|---|
| 30 | // parts of QR |
|---|
| 31 | hqy=Qy*qyx*pr; |
|---|
| 32 | |
|---|
| 33 | // pre_qr |
|---|
| 34 | pre_qr = concat_vertical(s*pr, hqy, qux); |
|---|
| 35 | |
|---|
| 36 | } |
|---|
