[586] | 1 | #define BDMLIB |
---|
[717] | 2 | #include "../mat_checks.h" |
---|
[586] | 3 | #include "design/ctrlbase.h" |
---|
| 4 | |
---|
| 5 | using namespace bdm; |
---|
| 6 | |
---|
[723] | 7 | TEST(LQG_test) { |
---|
[586] | 8 | LQG reg; |
---|
[723] | 9 | shared_ptr<StateSpace<chmat> > stsp=new StateSpace<chmat>; |
---|
[586] | 10 | // 2 x 1 x 1 |
---|
[723] | 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 | |
---|
| 16 | reg.redesign(); |
---|
[723] | 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 | |
---|
[723] | 21 | TEST(to_state_test) { |
---|
[605] | 22 | mlnorm<fsqmat> ml; |
---|
[723] | 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 | */ |
---|
[723] | 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 | |
---|
[723] | 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; RV urv; |
---|
| 53 | Stsp->connect_mlnorm(ml,xrv,urv); |
---|
| 54 | |
---|
| 55 | /* results from |
---|
[703] | 56 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
| 57 | */ |
---|
[723] | 58 | CHECK_CLOSE_EX(Stsp->_A().get_row(0), vec("1.1, 3.4, 1.3"), 0.0001); |
---|
| 59 | CHECK_CLOSE_EX(Stsp->_B().get_row(0), vec("2.3"), 0.0001); |
---|
| 60 | CHECK_CLOSE_EX(Stsp->_C().get_row(0), vec("1, 0, 0"), 0.0001); |
---|
[703] | 61 | } |
---|
| 62 | |
---|
[723] | 63 | TEST(arx_LQG_test){ |
---|
| 64 | mlnorm<chmat> ml; |
---|
| 65 | mat A="1.81, -.81, .00468, .00438"; |
---|
| 66 | ml.set_parameters(A, vec_1(0.0), 0.00001*eye(1)); |
---|
| 67 | RV yr=RV("y",1); |
---|
| 68 | RV ur=RV("u",1); |
---|
| 69 | RV rgr = yr.copy_t(-1); |
---|
| 70 | rgr.add(yr.copy_t(-2)); |
---|
| 71 | rgr.add(yr.copy_t(-2)); |
---|
| 72 | rgr.add(ur.copy_t(-1)); |
---|
| 73 | rgr.add(ur); |
---|
| 74 | |
---|
| 75 | ml.set_rv(yr); |
---|
| 76 | ml.set_rvc(rgr); |
---|
[699] | 77 | ml.validate(); |
---|
[723] | 78 | |
---|
| 79 | shared_ptr<StateFromARX> Stsp=new StateFromARX; |
---|
| 80 | RV xrv; RV urv; |
---|
| 81 | Stsp->connect_mlnorm(ml,xrv,urv); |
---|
| 82 | |
---|
[699] | 83 | LQG L; |
---|
[723] | 84 | L.set_system(Stsp); |
---|
| 85 | L.set_control_parameters(eye(1), sqrt(1.0/1000)*eye(1), vec_1(0.0), 100); |
---|
[699] | 86 | L.validate(); |
---|
[723] | 87 | |
---|
[699] | 88 | L.redesign(); |
---|
[723] | 89 | cout << L.to_string()<<endl; |
---|
[699] | 90 | } |
---|