1 | /*! |
---|
2 | \file |
---|
3 | \brief Controllers for linear Gaussian systems |
---|
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 "ctrlbase.h" |
---|
15 | #include "../estim/arx.h" |
---|
16 | #include "../estim/kalman.h" |
---|
17 | //#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h> |
---|
18 | |
---|
19 | namespace bdm{ |
---|
20 | |
---|
21 | //! Controller using ARX model for estimation and LQG designer for control |
---|
22 | class LQG_ARX : public Controller{ |
---|
23 | protected: |
---|
24 | //! Internal ARX estimator |
---|
25 | shared_ptr<ARX> ar; |
---|
26 | //! Internal LQG designer |
---|
27 | LQG lq; |
---|
28 | //! Intermediate StateSpace model |
---|
29 | shared_ptr<StateFromARX> Stsp; |
---|
30 | //! AR predictor |
---|
31 | shared_ptr<mlnorm<chmat> > pred; |
---|
32 | //! datalink from rvc to ar.bayes |
---|
33 | datalink rvc2ar_y; |
---|
34 | //! datalink from rvc to ar.bayes |
---|
35 | datalink_buffered rvc2ar_cond; |
---|
36 | |
---|
37 | //! flag to use iterations spread in time (ist) |
---|
38 | bool ist; |
---|
39 | //! flag to use windsurfer approach |
---|
40 | bool windsurfer; |
---|
41 | public: |
---|
42 | //! Default constructor |
---|
43 | LQG_ARX(): ar(), lq() { } |
---|
44 | |
---|
45 | //! adaptation is to store arx estimates in stsp |
---|
46 | void adapt(const vec &data){ |
---|
47 | ar->bayes(rvc2ar_y.pushdown(data), rvc2ar_cond.pushdown(data)); |
---|
48 | rvc2ar_cond.store_data(data); |
---|
49 | } |
---|
50 | void redesign(){ |
---|
51 | ar->ml_predictor_update(*pred); |
---|
52 | Stsp->update_from(*pred); |
---|
53 | if (windsurfer) { |
---|
54 | mat Ry=pred->_R(); |
---|
55 | lq.set_control_Qy(inv(Ry)); |
---|
56 | } |
---|
57 | if (!ist) { |
---|
58 | lq.initial_belmann(); |
---|
59 | } |
---|
60 | lq.redesign(); |
---|
61 | } |
---|
62 | vec ctrlaction(const vec &cond) const{ |
---|
63 | //cond is xt + ut |
---|
64 | vec state=cond.left(Stsp->_A().rows()); |
---|
65 | if (Stsp->_have_constant()){ |
---|
66 | state(state.length()-1) = 1; |
---|
67 | } |
---|
68 | return lq.ctrlaction(state,cond.right(Stsp->_B().cols())); |
---|
69 | } |
---|
70 | //! |
---|
71 | //! LQG is defined by quadratic loss function |
---|
72 | /*! \f[ L(y,u) = (y-y_{req})'Q_y (y-y_{req}) + (u-u_{req})' Q_u (u-u_{req}) \f] |
---|
73 | expected input |
---|
74 | \code |
---|
75 | { class="LQG"; |
---|
76 | arx = {class="ARX", ...} // internal arx model, see |
---|
77 | Qy = ("matrix", ...); |
---|
78 | Qu = ("matrix", ...); |
---|
79 | yreq = []; // requested output |
---|
80 | ureq = []; // requested input value |
---|
81 | } |
---|
82 | \endcode |
---|
83 | */ |
---|
84 | void from_setting(const Setting &set){ |
---|
85 | ar=UI::build<ARX> (set, "ARX",UI::compulsory); |
---|
86 | |
---|
87 | mat Qu; |
---|
88 | mat Qy; |
---|
89 | |
---|
90 | UI::get(Qu, set, "Qu", UI::compulsory); |
---|
91 | UI::get(Qy, set, "Qy", UI::compulsory); |
---|
92 | |
---|
93 | vec y_req; |
---|
94 | if (!UI::get(y_req, set, "yreq", UI::optional)) |
---|
95 | y_req = zeros(ar->_yrv()._dsize()); |
---|
96 | |
---|
97 | int horizon; |
---|
98 | UI::get(horizon, set, "horizon", UI::compulsory); |
---|
99 | lq.set_control_parameters(Qy,Qu,y_req,horizon); |
---|
100 | |
---|
101 | int wind; |
---|
102 | if (UI::get(wind, set, "windsurfer", UI::optional)){ |
---|
103 | windsurfer=wind>0; |
---|
104 | } else { |
---|
105 | windsurfer=false; |
---|
106 | }; |
---|
107 | int ist_; |
---|
108 | if (UI::get(ist_, set, "ist", UI::optional)){ |
---|
109 | ist=ist_>0; |
---|
110 | } else { |
---|
111 | ist=false; |
---|
112 | }; |
---|
113 | validate(); |
---|
114 | } |
---|
115 | |
---|
116 | void validate() { |
---|
117 | // ar is valid |
---|
118 | pred = ar->ml_predictor<chmat>(); |
---|
119 | Stsp = new StateFromARX; |
---|
120 | RV xrv; |
---|
121 | RV urvm; //old ut |
---|
122 | Stsp->connect_mlnorm(*pred, xrv, urvm); |
---|
123 | lq.set_system(Stsp); |
---|
124 | lq.validate(); |
---|
125 | |
---|
126 | rvc =concat(xrv, urvm); |
---|
127 | rvc2ar_y.set_connection(ar->_yrv(), rvc); |
---|
128 | rvc2ar_cond.set_connection(ar->_rvc(), rvc); |
---|
129 | //datalink from ARX to rvc |
---|
130 | } |
---|
131 | void log_register (logger &L, const string &prefix ) { |
---|
132 | ar->log_register(L,prefix); |
---|
133 | } |
---|
134 | void log_write ( ) const { |
---|
135 | ar->log_write(); |
---|
136 | } |
---|
137 | |
---|
138 | }; |
---|
139 | UIREGISTER(LQG_ARX); |
---|
140 | } // namespace |
---|