| 1 | #define BDMLIB |
|---|
| 2 | #include "../mat_checks.h" |
|---|
| 3 | #include "design/ctrlbase.h" |
|---|
| 4 | |
|---|
| 5 | using namespace bdm; |
|---|
| 6 | |
|---|
| 7 | TEST ( LQG_test ) { |
|---|
| 8 | LQG reg; |
|---|
| 9 | shared_ptr<StateSpace<chmat> > stsp = new StateSpace<chmat>; |
|---|
| 10 | // 2 x 1 x 1 |
|---|
| 11 | stsp-> set_parameters ( eye ( 2 ), ones ( 2, 1 ), ones ( 1, 2 ), ones ( 1, 1 ), /* Q,R */ eye ( 2 ), eye ( 1 ) ); |
|---|
| 12 | reg.set_system ( stsp ); // A, B, C |
|---|
| 13 | reg.set_control_parameters ( eye ( 1 ), eye ( 1 ), vec_1 ( 1.0 ), 6 ); //Qy, Qu, horizon |
|---|
| 14 | reg.validate(); |
|---|
| 15 | |
|---|
| 16 | reg.redesign(); |
|---|
| 17 | double reg_apply = reg.ctrlaction ( "0.5, 1.1", "0.0" ) ( 0 ); /*convert vec to double*/ |
|---|
| 18 | CHECK_CLOSE ( reg_apply, -0.248528137234392, 0.0001 ); |
|---|
| 19 | } |
|---|
| 20 | |
|---|
| 21 | TEST ( to_state_test ) { |
|---|
| 22 | mlnorm<fsqmat> ml; |
|---|
| 23 | mat A = "1.1, 2.3"; |
|---|
| 24 | ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) ); |
|---|
| 25 | RV yr = RV ( "y", 1 ); |
|---|
| 26 | RV ur = RV ( "u", 1 ); |
|---|
| 27 | ml.set_rv ( yr ); |
|---|
| 28 | yr.t_plus ( -1 ); |
|---|
| 29 | ml.set_rvc ( concat ( yr, ur ) ); |
|---|
| 30 | |
|---|
| 31 | shared_ptr<StateCanonical > Stsp = new StateCanonical; |
|---|
| 32 | Stsp->connect_mlnorm ( ml ); |
|---|
| 33 | |
|---|
| 34 | /* results from |
|---|
| 35 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
|---|
| 36 | */ |
|---|
| 37 | CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1" ), 0.0001 ); |
|---|
| 38 | CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "2.53" ), 0.0001 ); |
|---|
| 39 | CHECK_CLOSE_EX ( Stsp->_D().get_row ( 0 ), vec ( "2.30" ), 0.0001 ); |
|---|
| 40 | } |
|---|
| 41 | |
|---|
| 42 | TEST ( to_state_arx_test ) { |
|---|
| 43 | mlnorm<chmat> ml; |
|---|
| 44 | mat A = "1.1, 2.3, 3.4"; |
|---|
| 45 | ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) ); |
|---|
| 46 | RV yr = RV ( "y", 1 ); |
|---|
| 47 | RV ur = RV ( "u", 1 ); |
|---|
| 48 | ml.set_rv ( yr ); |
|---|
| 49 | ml.set_rvc ( concat ( yr.copy_t ( -1 ), concat ( ur, ur.copy_t ( -1 ) ) ) ); |
|---|
| 50 | |
|---|
| 51 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
|---|
| 52 | RV xrv; |
|---|
| 53 | RV urv; |
|---|
| 54 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
|---|
| 55 | |
|---|
| 56 | /* results from |
|---|
| 57 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
|---|
| 58 | */ |
|---|
| 59 | CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1, 3.4, 1.3" ), 0.0001 ); |
|---|
| 60 | CHECK_CLOSE_EX ( Stsp->_B().get_row ( 0 ), vec ( "2.3" ), 0.0001 ); |
|---|
| 61 | CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "1, 0, 0" ), 0.0001 ); |
|---|
| 62 | } |
|---|
| 63 | |
|---|
| 64 | TEST ( arx_LQG_test ) { |
|---|
| 65 | mlnorm<chmat> ml; |
|---|
| 66 | mat A = "1.81, -.81, .00468, .00438"; |
|---|
| 67 | ml.set_parameters ( A, vec_1 ( 0.0 ), 0.00001*eye ( 1 ) ); |
|---|
| 68 | RV yr = RV ( "y", 1 ); |
|---|
| 69 | RV ur = RV ( "u", 1 ); |
|---|
| 70 | RV rgr = yr.copy_t ( -1 ); |
|---|
| 71 | rgr.add ( yr.copy_t ( -2 ) ); |
|---|
| 72 | rgr.add ( yr.copy_t ( -2 ) ); |
|---|
| 73 | rgr.add ( ur.copy_t ( -1 ) ); |
|---|
| 74 | rgr.add ( ur ); |
|---|
| 75 | |
|---|
| 76 | ml.set_rv ( yr ); |
|---|
| 77 | ml.set_rvc ( rgr ); |
|---|
| 78 | ml.validate(); |
|---|
| 79 | |
|---|
| 80 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
|---|
| 81 | RV xrv; |
|---|
| 82 | RV urv; |
|---|
| 83 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
|---|
| 84 | |
|---|
| 85 | LQG L; |
|---|
| 86 | L.set_system ( Stsp ); |
|---|
| 87 | L.set_control_parameters ( eye ( 1 ), sqrt ( 1.0 / 1000 ) *eye ( 1 ), vec_1 ( 0.0 ), 100 ); |
|---|
| 88 | L.validate(); |
|---|
| 89 | |
|---|
| 90 | L.redesign(); |
|---|
| 91 | cout << L.to_string() << endl; |
|---|
| 92 | } |
|---|
