root/library/bdm/design/arx_ctrl.h @ 723

Revision 723, 3.4 kB (checked in by smidl, 15 years ago)

Big commit of LQG stuff

Line 
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
19namespace 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
Note: See TracBrowser for help on using the browser.