Changeset 723 for library/tests/testsuite/LQG_test.cpp

Timestamp:

11/15/09 23:02:02 (15 years ago)

Author:

smidl

Message:

Big commit of LQG stuff

Files:

• library/tests/testsuite/LQG_test.cpp (modified) (1 diff)

library/tests/testsuite/LQG_test.cpp

@@ -5 +5 @@
 using namespace bdm;
 
-TEST ( LQG_test ) {
+TEST(LQG_test) {
         LQG reg;
-        shared_ptr<StateSpace<fsqmat> > stsp = new StateSpace<fsqmat>;
+        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
+        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 );
+        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 ) {
+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
+        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 );
+        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<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<StateFromARX> Stsp = new StateFromARX;
-        Stsp->connect_mlnorm ( ml );
-
-        /* results from
+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])
         */
-        cout << "---" << endl;
+        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<fsqmat> ml;
-        mat A = "1.81, -.8189, .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 ( -2 ) );
-        rgr.add ( ur.copy_t ( -1 ) );
-
-        ml.set_rv ( yr );
-        ml.set_rvc ( rgr );
+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<StateCanonical > Stsp = new StateCanonical;
-        Stsp->connect_mlnorm ( ml );
-
+        
+        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 ), eye ( 1 ), vec_1 ( 0.0 ), 100 );
+        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;
+        cout << L.to_string()<<endl;
 }