[586] | 1 | #define BDMLIB |
---|
[717] | 2 | #include "../mat_checks.h" |
---|
[586] | 3 | #include "design/ctrlbase.h" |
---|
| 4 | |
---|
| 5 | using namespace bdm; |
---|
| 6 | |
---|
[737] | 7 | TEST ( LQG_test ) { |
---|
[586] | 8 | LQG reg; |
---|
[737] | 9 | shared_ptr<StateSpace<chmat> > stsp = new StateSpace<chmat>; |
---|
[586] | 10 | // 2 x 1 x 1 |
---|
[737] | 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 |
---|
[723] | 14 | reg.validate(); |
---|
[586] | 15 | |
---|
| 16 | reg.redesign(); |
---|
[737] | 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 ); |
---|
[586] | 19 | } |
---|
| 20 | |
---|
[737] | 21 | TEST ( to_state_test ) { |
---|
[605] | 22 | mlnorm<fsqmat> ml; |
---|
[737] | 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 |
---|
[605] | 35 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
| 36 | */ |
---|
[737] | 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 ); |
---|
[605] | 40 | } |
---|
| 41 | |
---|
[737] | 42 | TEST ( to_state_arx_test ) { |
---|
[723] | 43 | mlnorm<chmat> ml; |
---|
[737] | 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 |
---|
[703] | 57 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
| 58 | */ |
---|
[737] | 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 ); |
---|
[703] | 62 | } |
---|
| 63 | |
---|
[737] | 64 | TEST ( arx_LQG_test ) { |
---|
[723] | 65 | mlnorm<chmat> ml; |
---|
[737] | 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 ); |
---|
[699] | 78 | ml.validate(); |
---|
[737] | 79 | |
---|
| 80 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
---|
| 81 | RV xrv; |
---|
| 82 | RV urv; |
---|
| 83 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
---|
| 84 | |
---|
[699] | 85 | LQG L; |
---|
[737] | 86 | L.set_system ( Stsp ); |
---|
| 87 | L.set_control_parameters ( eye ( 1 ), sqrt ( 1.0 / 1000 ) *eye ( 1 ), vec_1 ( 0.0 ), 100 ); |
---|
[699] | 88 | L.validate(); |
---|
[737] | 89 | |
---|
[699] | 90 | L.redesign(); |
---|
[737] | 91 | cout << L.to_string() << endl; |
---|
[699] | 92 | } |
---|