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 | ar->ml_predictor_update ( *pred ); |
---|
50 | } |
---|
51 | void redesign() { |
---|
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 | vec tmp=lq.ctrlaction ( state, cond.right ( Stsp->_B().cols() ) ); |
---|
69 | if (!std::isfinite(sum(tmp))) { |
---|
70 | cout << "infinite ctrl action"; |
---|
71 | } |
---|
72 | return tmp; |
---|
73 | } |
---|
74 | //! |
---|
75 | //! LQG is defined by quadratic loss function |
---|
76 | /*! \f[ L(y,u) = (y-y_{req})'Q_y (y-y_{req}) + (u-u_{req})' Q_u (u-u_{req}) \f] |
---|
77 | expected input |
---|
78 | \code |
---|
79 | { class="LQG"; |
---|
80 | arx = {class="ARX", ...} // internal arx model, see |
---|
81 | Qy = ("matrix", ...); |
---|
82 | Qu = ("matrix", ...); |
---|
83 | yreq = []; // requested output |
---|
84 | ureq = []; // requested input value |
---|
85 | } |
---|
86 | \endcode |
---|
87 | */ |
---|
88 | void from_setting ( const Setting &set ) { |
---|
89 | ar = UI::build<ARX> ( set, "ARX", UI::compulsory ); |
---|
90 | |
---|
91 | mat Qu; |
---|
92 | mat Qy; |
---|
93 | |
---|
94 | UI::get ( Qu, set, "Qu", UI::compulsory ); |
---|
95 | UI::get ( Qy, set, "Qy", UI::compulsory ); |
---|
96 | |
---|
97 | vec y_req; |
---|
98 | if ( !UI::get ( y_req, set, "yreq", UI::optional ) ) |
---|
99 | y_req = zeros ( ar->_yrv()._dsize() ); |
---|
100 | |
---|
101 | int horizon; |
---|
102 | UI::get ( horizon, set, "horizon", UI::compulsory ); |
---|
103 | lq.set_control_parameters ( Qy, Qu, y_req, horizon ); |
---|
104 | |
---|
105 | int wind; |
---|
106 | if ( UI::get ( wind, set, "windsurfer", UI::optional ) ) { |
---|
107 | windsurfer = wind > 0; |
---|
108 | } else { |
---|
109 | windsurfer = false; |
---|
110 | }; |
---|
111 | int ist_; |
---|
112 | if ( UI::get ( ist_, set, "ist", UI::optional ) ) { |
---|
113 | ist = ist_ > 0; |
---|
114 | } else { |
---|
115 | ist = false; |
---|
116 | }; |
---|
117 | validate(); |
---|
118 | } |
---|
119 | |
---|
120 | void validate() { |
---|
121 | // ar is valid |
---|
122 | pred = ar->ml_predictor<chmat>(); |
---|
123 | Stsp = new StateFromARX; |
---|
124 | RV xrv; |
---|
125 | RV urvm; //old ut |
---|
126 | Stsp->connect_mlnorm ( *pred, xrv, urvm ); |
---|
127 | lq.set_system ( Stsp ); |
---|
128 | lq.validate(); |
---|
129 | |
---|
130 | rv = urvm; // rv is not shifted to t+1!! |
---|
131 | rvc = concat ( xrv, urvm ); |
---|
132 | rvc2ar_y.set_connection ( ar->_yrv(), rvc ); |
---|
133 | rvc2ar_cond.set_connection ( ar->_rvc(), rvc ); |
---|
134 | //datalink from ARX to rvc |
---|
135 | } |
---|
136 | void log_register ( logger &L, const string &prefix ) { |
---|
137 | ar->log_register ( L, prefix ); |
---|
138 | } |
---|
139 | void log_write ( ) const { |
---|
140 | ar->log_write(); |
---|
141 | } |
---|
142 | //! access function |
---|
143 | const ARX& _ar() { |
---|
144 | return *ar.get(); |
---|
145 | } |
---|
146 | //! access function |
---|
147 | mlnorm<chmat>* _pred() { |
---|
148 | return pred.get(); |
---|
149 | } |
---|
150 | |
---|
151 | |
---|
152 | }; |
---|
153 | UIREGISTER ( LQG_ARX ); |
---|
154 | } // namespace |
---|