root/library/tests/testsuite/LQG_test.cpp @ 723

#define BDMLIB
#include "../mat_checks.h"
#include "design/ctrlbase.h"

using namespace bdm;

TEST(LQG_test) {
        LQG reg;
        shared_ptr<StateSpace<chmat> > stsp=new StateSpace<chmat>;
        // 2 x 1 x 1
        stsp-> set_parameters(eye(2), ones(2,1), ones(1,2), ones(1,1), /* Q,R */ eye(2), eye(1));
        reg.set_system(stsp); // A, B, C
        reg.set_control_parameters(eye(1), eye(1),  vec_1(1.0), 6); //Qy, Qu, horizon
        reg.validate();

        reg.redesign();
        double reg_apply=reg.ctrlaction("0.5, 1.1","0.0")(0); /*convert vec to double*/
        CHECK_CLOSE(reg_apply, -0.248528137234392, 0.0001);
}

TEST(to_state_test) {
        mlnorm<fsqmat> ml;
        mat A="1.1, 2.3";
        ml.set_parameters(A, vec_1(1.3), eye(1));
        RV yr=RV("y",1);
        RV ur=RV("u",1);
        ml.set_rv(yr);
        yr.t_plus(-1);
        ml.set_rvc(concat(yr, ur));
        
        shared_ptr<StateCanonical > Stsp=new StateCanonical;
        Stsp->connect_mlnorm(ml);
        
        /* results from 
        [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
        */
        CHECK_CLOSE_EX(Stsp->_A().get_row(0), vec("1.1"), 0.0001);
        CHECK_CLOSE_EX(Stsp->_C().get_row(0), vec("2.53"), 0.0001);
        CHECK_CLOSE_EX(Stsp->_D().get_row(0), vec("2.30"), 0.0001);
}

TEST(to_state_arx_test) {
        mlnorm<chmat> ml;
        mat A="1.1, 2.3, 3.4";
        ml.set_parameters(A, vec_1(1.3), eye(1));
        RV yr=RV("y",1);
        RV ur=RV("u",1);
        ml.set_rv(yr);
        ml.set_rvc(concat(yr.copy_t(-1), concat(ur, ur.copy_t(-1))));
        
        shared_ptr<StateFromARX> Stsp=new StateFromARX;
        RV xrv; RV urv;
        Stsp->connect_mlnorm(ml,xrv,urv);
        
        /* results from 
        [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
        */
        CHECK_CLOSE_EX(Stsp->_A().get_row(0), vec("1.1, 3.4, 1.3"), 0.0001);
        CHECK_CLOSE_EX(Stsp->_B().get_row(0), vec("2.3"), 0.0001);
        CHECK_CLOSE_EX(Stsp->_C().get_row(0), vec("1, 0, 0"), 0.0001);
}

TEST(arx_LQG_test){
        mlnorm<chmat> ml;
        mat A="1.81, -.81, .00468, .00438";
        ml.set_parameters(A, vec_1(0.0), 0.00001*eye(1));
        RV yr=RV("y",1);
        RV ur=RV("u",1);
        RV rgr = yr.copy_t(-1);
        rgr.add(yr.copy_t(-2));
        rgr.add(yr.copy_t(-2));
        rgr.add(ur.copy_t(-1));
        rgr.add(ur);
        
        ml.set_rv(yr);
        ml.set_rvc(rgr);
        ml.validate();
        
        shared_ptr<StateFromARX> Stsp=new StateFromARX;
        RV xrv; RV urv;
        Stsp->connect_mlnorm(ml,xrv,urv);
        
        LQG L;
        L.set_system(Stsp);
        L.set_control_parameters(eye(1), sqrt(1.0/1000)*eye(1), vec_1(0.0), 100);
        L.validate();
        
        L.redesign();
        cout << L.to_string()<<endl;
}