- 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
    00015 namespace bdm{
    00016
    00018 class Designer : public root {
    00019         public:
    00021                 virtual void redesign(){it_error("Not implemented"); };
    00023                 virtual vec apply(const vec &cond){it_error("Not implemented"); return vec(0);}
    00024 };
    00025
    00028 class LQG : public Designer {
    00029         protected:
    00031                 int dimx;
    00033                 int dimu;
    00035                 int dimy;
    00036
    00038                 mat A;
    00040                 mat B;
    00042                 mat C;
    00044                 vec y_req;
    00045
    00047                 int horizon;
    00049                 mat Qy;
    00051                 mat Qu;
    00053                 int td;
    00055                 mat L;
    00056
    00059                 mat pr;
    00061                 mat qux;
    00063                 mat qyx;
    00065                 mat s;
    00067                 mat qy;
    00069                 mat hqy;
    00071                 mat pre_qr;
    00073                 mat post_qr;
    00075
    00076         public:
    00078                 void set_system_parameters(const mat &A, const mat &B, const mat &C);
    00080                 void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec y_req0, int horizon0);
    00082 //              void set_system_parameters(const Kalman &K);
    00084                 void prepare_qr();
    00086                 virtual void update_state(){};
    00088                 void ricatti_step(){
    00089                         pre_qr.set_submatrix(0,0,s*pr);
    00090                         pre_qr.set_submatrix(dimx+dimu+dimy, dimu+dimx, -Qy*y_req);
    00091                         if (!qr(pre_qr,post_qr)) it_warning("QR in LQG unstable");
    00092                         triu(post_qr);
    00093         // hn(m+1:2*m+n+r,m+1:2*m+n+r);
    00094                         s=post_qr.get(dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1);
    00095                 };
    00096                 void redesign(){
    00097                         for(td=horizon; td>0; td--){
    00098                                 update_state();
    00099                                 ricatti_step();
    00100                         }
    00101 /*                      ws=hn(1:m,m+1:2*m+n+r);
    00102                         wsd=hn(1:m,1:m);
    00103                         Lklq=-inv(wsd)*ws;*/
    00104                         L = -inv(post_qr.get(0,dimu-1, 0,dimu-1)) * post_qr.get(0,dimu-1, dimu, 2*dimu+dimx+dimy-1);
    00105                 }
    00106                 vec apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
    00107         } ;
    00108
    00109 } // namespace