Changeset 586 for library/bdm/design/ctrlbase.cpp

Timestamp:

08/27/09 15:39:35 (15 years ago)

Author:

smidl

Message:

redesign of ctrl LQ control for arx

Files:

• library/bdm/design/ctrlbase.cpp (modified) (2 diffs)

library/bdm/design/ctrlbase.cpp

@@ -3 +3 @@
 namespace bdm{
 
-void LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){
-        dimx = A0.rows();
-        dimy = C0.rows();
-        dimu = B0.cols();
-
-        bdm_assert_debug ( A0.cols() == dimx, "LQG: A is not square" );
-        bdm_assert_debug ( B0.rows() == dimx, "LQG: B is not compatible" );
-        bdm_assert_debug ( C0.cols() == dimx, "LQG: C is not square" );
-
-        A=A0;
-        B=B0;
-        C=C0;
+void LQG::set_system(shared_ptr<StateSpace<fsqmat> > S0){
+        S = S0;
+        dimx= S->_dimx();
+        dimy= S->_dimy();
+        dimu= S->_dimu();
         pr=zeros(dimx+dimu+dimy,  dimu+dimx+dimu+dimy);
         pr.set_submatrix(dimx, dimu+dimx, eye(dimu+dimy));
 }
 
-void LQG::set_control_parameters(const mat &Qy0, const mat &Qu0, const vec y_req0, int horizon0){
-        bdm_assert_debug ( Qy0.cols() == dimy, "LQG: wrong dimensions of Qy " );
-        bdm_assert_debug ( Qu0.cols() == dimu, "LQG: wrong dimensions of Qu " );
-        bdm_assert_debug ( y_req0.length() == dimy, "LQG: wrong dimensions of y_req " );
-
+void LQG::set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0){
         Qy=Qy0;
         Qu=Qu0;
         y_req=y_req0;
         horizon = horizon0;
-        prepare_qr();
+        validate();
+        initialize();
 }
 
-void LQG::prepare_qr(){
+void LQG::validate() {
+        bdm_assert_debug ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " );
+        bdm_assert_debug ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " );
+        bdm_assert_debug ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " );
+}
+
+void LQG::initialize(){
         // set parameter matrix
-        pr.set_submatrix(0,0,B);
-        pr.set_submatrix(0,dimu, A);
+        pr.set_submatrix(0,0,S->_B());
+        pr.set_submatrix(0,dimu, S->_A());
         
         //penalization
@@ -42 +38 @@
 
         qyx=zeros(dimy, dimx+dimy+dimu);
-        qyx.set_submatrix(0,0,C);
+        qyx.set_submatrix(0,0,S->_C());
         qyx.set_submatrix(0,dimx,-eye(dimy));
         
         //
-        s=1e-5*eye(4);//dimx+dimu+dimy);
+        s=1e-5*eye(dimx+dimu+dimy);
         // parts of QR
         hqy=Qy*qyx*pr;