ctrlbase.h

Go to the documentation of this file.

#include "../base/bdmbase.h"

namespace bdm{

class Designer : public root {
        public:
                virtual void redesign(){it_error("Not implemented"); };
                virtual vec apply(const vec &cond){it_error("Not implemented"); return vec(0);}
};

class LQG : public Designer {
        protected:
                int dimx;
                int dimu;
                int dimy;

                mat A;
                mat B;
                mat C;
                vec y_req;
                
                int horizon;
                mat Qy;
                mat Qu;
                int td;
                mat L;
                
                mat pr;
                mat qux;
                mat qyx;
                mat s;
                mat qy;
                mat hqy;
                mat pre_qr;
                mat post_qr;
                
        public:
                void set_system_parameters(const mat &A, const mat &B, const mat &C);
                void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec y_req0, int horizon0);
//              void set_system_parameters(const Kalman &K);
                void prepare_qr();
                virtual void update_state(){};
                void ricatti_step(){
                        pre_qr.set_submatrix(0,0,s*pr);
                        pre_qr.set_submatrix(dimx+dimu+dimy, dimu+dimx, -Qy*y_req);
                        if (!qr(pre_qr,post_qr)) it_warning("QR in LQG unstable");
                        triu(post_qr);
        // hn(m+1:2*m+n+r,m+1:2*m+n+r);
                        s=post_qr.get(dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1);
                };
                void redesign(){
                        for(td=horizon; td>0; td--){
                                update_state();
                                ricatti_step();
                        }
/*                      ws=hn(1:m,m+1:2*m+n+r);
                        wsd=hn(1:m,1:m);
                        Lklq=-inv(wsd)*ws;*/
                        L = -inv(post_qr.get(0,dimu-1, 0,dimu-1)) * post_qr.get(0,dimu-1, dimu, 2*dimu+dimx+dimy-1);
                }
                vec apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
        } ;

} // namespace

---