- Main Page
- Related Pages
- Classes
- Files

- File List
- File Members

# ctrlbase.h

Go to the documentation of this file.

    00001
    00013 #include "../base/bdmbase.h"
    00014 #include "../estim/kalman.h"
    00015
    00016 namespace bdm{
    00017
    00019 class Designer  {
    00020
    00021         public:
    00023                 virtual void redesign() {
    00024                         bdm_error("Not implemented");
    00025                 }
    00026
    00028                 virtual vec apply(const vec &cond) {
    00029                         bdm_error("Not implemented");
    00030                         return vec();
    00031                 }
    00032 };
    00033
    00036 class LQG : public Designer {
    00037         protected:
    00039                 shared_ptr<StateSpace<fsqmat> > S;
    00041                 vec y_req;
    00043                 vec u_req;
    00044
    00046                 int horizon;
    00048                 mat Qy;
    00050                 mat Qu;
    00052                 int td;
    00054                 mat L;
    00055
    00058                 int dimx;
    00060                 int dimy;
    00062                 int dimu;
    00063
    00065                 mat pr;
    00067                 mat qux;
    00069                 mat qyx;
    00071                 mat s;
    00073                 mat qy;
    00075                 mat hqy;
    00077                 mat pre_qr;
    00079                 mat post_qr;
    00081
    00082         public:
    00084                 void set_system(shared_ptr<StateSpace<fsqmat> > S0);
    00086                 void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0);
    00088 //              void set_system_parameters(const Kalman &K);
    00090                 void initialize();
    00091                 void validate();
    00093                 virtual void update_state(){};
    00095                 void ricatti_step(){
    00096                         pre_qr.set_submatrix(0,0,s*pr);
    00097                         pre_qr.set_submatrix(dimx+dimu+dimy, dimu+dimx, -Qy*y_req);
    00098                         if (!qr(pre_qr,post_qr)) bdm_warning("QR in LQG unstable");
    00099                         triu(post_qr);
    00100         // hn(m+1:2*m+n+r,m+1:2*m+n+r);
    00101                         s=post_qr.get(dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1);
    00102                 };
    00103                 void redesign(){
    00104                         for(td=horizon; td>0; td--){
    00105                                 update_state();
    00106                                 ricatti_step();
    00107                         }
    00108 /*                      ws=hn(1:m,m+1:2*m+n+r);
    00109                         wsd=hn(1:m,1:m);
    00110                         Lklq=-inv(wsd)*ws;*/
    00111                         L = -inv(post_qr.get(0,dimu-1, 0,dimu-1)) * post_qr.get(0,dimu-1, dimu, 2*dimu+dimx+dimy-1);
    00112                 }
    00113                 vec apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
    00114         } ;
    00115
    00119         class Controller : public root {
    00121                 RV yrv;
    00123                 RV urv;
    00125                 RV drv;
    00126                 public:
    00128                         virtual void adapt(const vec &data){};
    00130                         virtual vec ctrlaction(const vec &data){return vec(0);}
    00131
    00132                         void from_setting(const Setting &set){
    00133                                 UI::get(yrv,set,"yrv",UI::optional);
    00134                                 UI::get(yrv,set,"urv",UI::optional);
    00135                         }
    00137                         const RV& _urv() {return urv;}
    00139                         const RV& _yrv() {return yrv;}
    00141                         const RV& _drv() {return drv;}
    00142         };
    00143
    00144 } // namespace