ctrlbase.h

Go to the documentation of this file.

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

namespace bdm{

class Designer  {
        
        public:
                virtual void redesign() {
                        bdm_error("Not implemented");
                }

                virtual vec apply(const vec &cond) {
                        bdm_error("Not implemented");
                        return vec();
                }
};

class LQG : public Designer {
        protected:
                shared_ptr<StateSpace<fsqmat> > S;
                vec y_req;
                vec u_req;
                
                int horizon;
                mat Qy;
                mat Qu;
                int td;
                mat L;
                
                int dimx;
                int dimy;
                int dimu;
                
                mat pr;
                mat qux;
                mat qyx;
                mat s;
                mat qy;
                mat hqy;
                mat pre_qr;
                mat post_qr;
                
        public:
                void set_system(shared_ptr<StateSpace<fsqmat> > S0);
                void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0);
//              void set_system_parameters(const Kalman &K);
                void initialize();
                void validate();
                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)){ bdm_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;}
        } ;

        class Controller : public root {
                RV yrv;
                RV urv;
                RV drv;
                public:
                        virtual void adapt(const vec &data){};
                        virtual vec ctrlaction(const vec &data){return vec(0);}
                        
                        void from_setting(const Setting &set){
                                UI::get(yrv,set,"yrv",UI::optional);
                                UI::get(yrv,set,"urv",UI::optional);
                        }
                        const RV& _urv() {return urv;}
                        const RV& _yrv() {return yrv;}
                        const RV& _drv() {return drv;}
        };
                        
} // namespace

---