1 | /*! |
---|
2 | \file |
---|
3 | \brief Base classes for designers of control strategy |
---|
4 | \author Vaclav Smidl. |
---|
5 | |
---|
6 | ----------------------------------- |
---|
7 | BDM++ - C++ library for Bayesian Decision Making under Uncertainty |
---|
8 | |
---|
9 | Using IT++ for numerical operations |
---|
10 | ----------------------------------- |
---|
11 | */ |
---|
12 | |
---|
13 | #include "../base/bdmbase.h" |
---|
14 | #include "../estim/kalman.h" |
---|
15 | |
---|
16 | namespace bdm { |
---|
17 | |
---|
18 | //! Base class for adaptive controllers |
---|
19 | //! The base class is, however, non-adaptive, method \c adapt() is empty. |
---|
20 | //! \note advanced Controllers will probably include estimator as their internal attribute (e.g. dual controllers) |
---|
21 | class Controller : public root { |
---|
22 | protected: |
---|
23 | //! identifier of the designed action; |
---|
24 | RV rv; |
---|
25 | //! identifier of the conditioning variables - data needed ; |
---|
26 | RV rvc; |
---|
27 | public: |
---|
28 | //! function processing new observations and adapting control strategy accordingly |
---|
29 | virtual void adapt ( const vec &data ) {}; |
---|
30 | //! function redesigning the control strategy |
---|
31 | virtual void redesign() {}; |
---|
32 | //! returns designed control action |
---|
33 | virtual vec ctrlaction ( const vec &cond ) const { |
---|
34 | return vec ( 0 ); |
---|
35 | } |
---|
36 | |
---|
37 | void from_setting ( const Setting &set ) { |
---|
38 | UI::get ( rv, set, "rv", UI::optional ); |
---|
39 | UI::get ( rvc, set, "rvc", UI::optional ); |
---|
40 | } |
---|
41 | //! access function |
---|
42 | const RV& _rv() { |
---|
43 | return rv; |
---|
44 | } |
---|
45 | //! access function |
---|
46 | const RV& _rvc() { |
---|
47 | return rvc; |
---|
48 | } |
---|
49 | //! register this controller with given datasource under name "name" |
---|
50 | virtual void log_register ( logger &L, const string &prefix ) { } |
---|
51 | //! write requested values into the logger |
---|
52 | virtual void log_write ( ) const { } |
---|
53 | |
---|
54 | }; |
---|
55 | |
---|
56 | //! Linear Quadratic Gaussian designer for constant penalizations and constant target |
---|
57 | //! Its internals are very close to Kalman estimator |
---|
58 | class LQG : public Controller { |
---|
59 | protected: |
---|
60 | //! StateSpace model from which we read data |
---|
61 | shared_ptr<StateSpace<chmat> > S; |
---|
62 | //! required value of the output y at time t (assumed constant) |
---|
63 | vec y_req; |
---|
64 | //! required value of the output y at time t (assumed constant) |
---|
65 | vec u_req; |
---|
66 | |
---|
67 | //! Control horizon, set to maxint for infinite horizons |
---|
68 | int horizon; |
---|
69 | //! penalization matrix Qy |
---|
70 | chmat Qy; |
---|
71 | //! penalization matrix Qu |
---|
72 | chmat Qu; |
---|
73 | //! time of the design step - from horizon->0 |
---|
74 | int td; |
---|
75 | //! controller parameters |
---|
76 | mat L; |
---|
77 | |
---|
78 | //!@{ \name temporary storage for ricatti - use initialize |
---|
79 | //! convenience parameters |
---|
80 | int dimx; |
---|
81 | //! convenience parameters |
---|
82 | int dimy; |
---|
83 | //! convenience parameters |
---|
84 | int dimu; |
---|
85 | |
---|
86 | //! parameters |
---|
87 | mat pr; |
---|
88 | //! penalization |
---|
89 | mat qux; |
---|
90 | //! penalization |
---|
91 | mat qyx; |
---|
92 | //! internal quadratic form |
---|
93 | mat s; |
---|
94 | //! penalization |
---|
95 | mat qy; |
---|
96 | //! pre_qr part |
---|
97 | mat hqy; |
---|
98 | //! pre qr matrix |
---|
99 | mat pre_qr; |
---|
100 | //! post qr matrix |
---|
101 | mat post_qr; |
---|
102 | //!@} |
---|
103 | |
---|
104 | public: |
---|
105 | //! set system parameters from given matrices |
---|
106 | void set_system ( shared_ptr<StateSpace<chmat> > S0 ); |
---|
107 | //! update internal whan system has changed |
---|
108 | void update_system() { |
---|
109 | pr.set_submatrix ( 0, 0, S->_B() ); |
---|
110 | pr.set_submatrix ( 0, dimu, S->_A() ); |
---|
111 | |
---|
112 | //penalization |
---|
113 | qux.set_submatrix ( 0, 0, Qu._Ch() ); |
---|
114 | qux.set_submatrix ( 0, dimx + dimu + dimy, Qu._Ch() ); |
---|
115 | |
---|
116 | qyx.set_submatrix ( 0, 0, S->_C() ); |
---|
117 | qyx.set_submatrix ( 0, dimx, -eye ( dimy ) ); |
---|
118 | |
---|
119 | // parts of QR |
---|
120 | hqy = Qy.to_mat() * qyx * pr; |
---|
121 | |
---|
122 | // pre_qr |
---|
123 | pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) ); |
---|
124 | } |
---|
125 | //! set penalization matrices and control horizon |
---|
126 | void set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ); |
---|
127 | //! set penalization matrices and control horizon |
---|
128 | void set_control_Qy ( const mat &Qy0 ) { |
---|
129 | Qy = Qy0; |
---|
130 | } |
---|
131 | //! refresh temporary storage - inefficient can be improved |
---|
132 | void initial_belmann() { |
---|
133 | s = 1e-5 * eye ( dimx + dimu + dimy ); |
---|
134 | }; |
---|
135 | //! validation procedure |
---|
136 | void validate(); |
---|
137 | //! function for future use which is called at each time td; Should call initialize()! |
---|
138 | //! redesign one step of the |
---|
139 | void ricatti_step() { |
---|
140 | pre_qr.set_submatrix ( 0, 0, s*pr ); |
---|
141 | pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req ); |
---|
142 | if ( !qr ( pre_qr, post_qr ) ) { |
---|
143 | bdm_warning ( "QR in LQG unstable" ); |
---|
144 | } |
---|
145 | triu ( post_qr ); |
---|
146 | // hn(m+1:2*m+n+r,m+1:2*m+n+r); |
---|
147 | s = post_qr.get ( dimu, 2 * dimu + dimx + dimy - 1, dimu, 2 * dimu + dimx + dimy - 1 ); |
---|
148 | }; |
---|
149 | void redesign() { |
---|
150 | for ( td = horizon; td > 0; td-- ) { |
---|
151 | update_system(); |
---|
152 | ricatti_step(); |
---|
153 | } |
---|
154 | /* ws=hn(1:m,m+1:2*m+n+r); |
---|
155 | wsd=hn(1:m,1:m); |
---|
156 | Lklq=-inv(wsd)*ws;*/ |
---|
157 | L = -inv ( post_qr.get ( 0, dimu - 1, 0, dimu - 1 ) ) * post_qr.get ( 0, dimu - 1, dimu, 2 * dimu + dimx + dimy - 1 ); |
---|
158 | } |
---|
159 | //! compute control action |
---|
160 | vec ctrlaction ( const vec &state, const vec &ukm ) const { |
---|
161 | vec pom = concat ( state, ones ( dimy ), ukm ); |
---|
162 | return L*pom; |
---|
163 | } |
---|
164 | //! access function |
---|
165 | mat _L() const { |
---|
166 | return L; |
---|
167 | } |
---|
168 | } ; |
---|
169 | |
---|
170 | |
---|
171 | } // namespace |
---|