root/library/tests/LQG_test.cpp @ 622

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

using namespace bdm;

TEST(test_LQG) {
        LQG reg;
        shared_ptr<StateSpace<fsqmat> > stsp=new StateSpace<fsqmat>;
        // 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.redesign();
        double reg_apply=reg.apply("0.5, 1.1","0.0")(0); /*convert vec to double*/
        CHECK_CLOSE(reg_apply, -0.248528137234392, 0.0001);
}

TEST(test_to_state) {
        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);
}