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<fsqmat> > stsp = new StateSpace<fsqmat>; |
---|
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 | |
---|
15 | reg.redesign(); |
---|
16 | double reg_apply = reg.ctrlaction ( "0.5, 1.1", "0.0" ) ( 0 ); /*convert vec to double*/ |
---|
17 | CHECK_CLOSE ( reg_apply, -0.248528137234392, 0.0001 ); |
---|
18 | } |
---|
19 | |
---|
20 | TEST ( to_state_test ) { |
---|
21 | mlnorm<fsqmat> ml; |
---|
22 | mat A = "1.1, 2.3"; |
---|
23 | ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) ); |
---|
24 | RV yr = RV ( "y", 1 ); |
---|
25 | RV ur = RV ( "u", 1 ); |
---|
26 | ml.set_rv ( yr ); |
---|
27 | yr.t_plus ( -1 ); |
---|
28 | ml.set_rvc ( concat ( yr, ur ) ); |
---|
29 | |
---|
30 | shared_ptr<StateCanonical > Stsp = new StateCanonical; |
---|
31 | Stsp->connect_mlnorm ( ml ); |
---|
32 | |
---|
33 | /* results from |
---|
34 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
35 | */ |
---|
36 | CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1" ), 0.0001 ); |
---|
37 | CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "2.53" ), 0.0001 ); |
---|
38 | CHECK_CLOSE_EX ( Stsp->_D().get_row ( 0 ), vec ( "2.30" ), 0.0001 ); |
---|
39 | } |
---|
40 | |
---|
41 | TEST ( to_state_arx_test ) { |
---|
42 | mlnorm<fsqmat> ml; |
---|
43 | mat A = "1.1, 2.3"; |
---|
44 | ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) ); |
---|
45 | RV yr = RV ( "y", 1 ); |
---|
46 | RV ur = RV ( "u", 1 ); |
---|
47 | ml.set_rv ( yr ); |
---|
48 | yr.t_plus ( -1 ); |
---|
49 | ml.set_rvc ( concat ( yr, ur ) ); |
---|
50 | |
---|
51 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
---|
52 | Stsp->connect_mlnorm ( ml ); |
---|
53 | |
---|
54 | /* results from |
---|
55 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
56 | */ |
---|
57 | cout << "---" << endl; |
---|
58 | } |
---|
59 | |
---|
60 | TEST ( arx_LQG_test ) { |
---|
61 | mlnorm<fsqmat> ml; |
---|
62 | mat A = "1.81, -.8189, .00468, .00438"; |
---|
63 | ml.set_parameters ( A, vec_1 ( 0.0 ), 0.00001*eye ( 1 ) ); |
---|
64 | RV yr = RV ( "y", 1 ); |
---|
65 | RV ur = RV ( "u", 1 ); |
---|
66 | RV rgr = yr.copy_t ( -1 ); |
---|
67 | rgr.add ( yr.copy_t ( -2 ) ); |
---|
68 | rgr.add ( yr.copy_t ( -2 ) ); |
---|
69 | rgr.add ( ur.copy_t ( -2 ) ); |
---|
70 | rgr.add ( ur.copy_t ( -1 ) ); |
---|
71 | |
---|
72 | ml.set_rv ( yr ); |
---|
73 | ml.set_rvc ( rgr ); |
---|
74 | ml.validate(); |
---|
75 | |
---|
76 | shared_ptr<StateCanonical > Stsp = new StateCanonical; |
---|
77 | Stsp->connect_mlnorm ( ml ); |
---|
78 | |
---|
79 | LQG L; |
---|
80 | L.set_system ( Stsp ); |
---|
81 | L.set_control_parameters ( eye ( 1 ), eye ( 1 ), vec_1 ( 0.0 ), 100 ); |
---|
82 | L.validate(); |
---|
83 | |
---|
84 | L.redesign(); |
---|
85 | cout << L.to_string() << endl; |
---|
86 | } |
---|