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

#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;
}