[586] | 1 | #define BDMLIB |
---|
[717] | 2 | #include "../mat_checks.h" |
---|
[1130] | 3 | #include "design/lq_ctrl.h" |
---|
[586] | 4 | |
---|
| 5 | using namespace bdm; |
---|
| 6 | |
---|
[737] | 7 | TEST ( LQG_test ) { |
---|
[1064] | 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(); |
---|
[586] | 15 | |
---|
[1064] | 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 ); |
---|
[586] | 19 | } |
---|
| 20 | |
---|
[737] | 21 | TEST ( to_state_test ) { |
---|
[1064] | 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 ) ); |
---|
[737] | 30 | |
---|
[1064] | 31 | shared_ptr<StateCanonical > Stsp = new StateCanonical; |
---|
| 32 | Stsp->connect_mlnorm ( ml ); |
---|
[737] | 33 | |
---|
[1064] | 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 ); |
---|
[605] | 40 | } |
---|
| 41 | |
---|
[737] | 42 | TEST ( to_state_arx_test ) { |
---|
[1064] | 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 ) ) ) ); |
---|
[737] | 50 | |
---|
[1064] | 51 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
---|
| 52 | RV xrv; |
---|
| 53 | RV urv; |
---|
| 54 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
---|
[737] | 55 | |
---|
[1064] | 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 ); |
---|
[703] | 62 | } |
---|
| 63 | |
---|
[737] | 64 | TEST ( arx_LQG_test ) { |
---|
[1064] | 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 ); |
---|
[737] | 75 | |
---|
[1064] | 76 | ml.set_rv ( yr ); |
---|
| 77 | ml.set_rvc ( rgr ); |
---|
| 78 | ml.validate(); |
---|
[737] | 79 | |
---|
[1064] | 80 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
---|
| 81 | RV xrv; |
---|
| 82 | RV urv; |
---|
| 83 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
---|
[737] | 84 | |
---|
[1064] | 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(); |
---|
[737] | 89 | |
---|
[1064] | 90 | L.redesign(); |
---|
| 91 | cout << L.to_string() << endl; |
---|
[699] | 92 | } |
---|
[1130] | 93 | |
---|
| 94 | TEST (quadratic_test){ |
---|
| 95 | quadraticfn qf; |
---|
| 96 | qf.Q = chmat(2); |
---|
| 97 | qf.Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0"); |
---|
| 98 | CHECK_CLOSE_EX(qf.eval(vec("1 2")), vec_1(1.0), 0.0000000000000001); |
---|
| 99 | |
---|
| 100 | LQG_universal lq; |
---|
| 101 | lq.Losses = Array<quadraticfn>(1); |
---|
| 102 | lq.Losses(0) = quadraticfn(); |
---|
| 103 | lq.Losses(0).Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0"); |
---|
| 104 | lq.Losses(0).rv = RV("{u up }"); |
---|
| 105 | |
---|
| 106 | lq.Models = Array<linfn>(1); |
---|
| 107 | lq.Models(0) = linfn(mat("1"),vec("1")); |
---|
| 108 | lq.Models(0).rv = RV("{x }"); |
---|
| 109 | |
---|
| 110 | lq.rv = RV("u",1); |
---|
| 111 | |
---|
| 112 | lq.redesign(); |
---|
| 113 | CHECK_CLOSE_EX(lq.ctrlaction(vec("1,0")), vec("1.24, -5.6"), 0.0000001); |
---|
| 114 | } |
---|