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

Timestamp:

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

Author:

smidl

Message:

Big commit of LQG stuff

Files:

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

library/bdm/design/ctrlbase.cpp

@@ -3 +3 @@
 namespace bdm{
 
-void LQG::set_system(shared_ptr<StateSpace<fsqmat> > S0){
+void LQG::set_system(shared_ptr<StateSpace<chmat> > S0){
         S = S0;
-        dimx= S->_A().rows();
-        dimy= S->_C().rows();
-        dimu= S->_B().cols();
-        pr=zeros(dimx+dimu+dimy,  dimu+dimx+dimu+dimy);
-        pr.set_submatrix(dimx, dimu+dimx, eye(dimu+dimy));
 }
 
@@ -17 +12 @@
         y_req=y_req0;
         horizon = horizon0;
-        validate();
-        initialize();
 }
 
 void LQG::validate() {
+        // set properties from S
+        dimx= S->_A().rows();
+        dimy= S->_C().rows();
+        dimu= S->_B().cols();
+        pr=zeros(dimx+dimu+dimy,  dimu+dimx+dimu+dimy);
+        pr.set_submatrix(dimx, dimu+dimx, eye(dimu+dimy));
+        
+        //penalization
         bdm_assert ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " );
         bdm_assert ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " );
         bdm_assert ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " );
+        
+        qux=zeros(dimu,dimx+2*dimu+dimy);     
+        qyx=zeros(dimy, dimx+dimy+dimu);
+        
+        //
+        initial_belmann();
+        // parts of QR
+        post_qr = zeros(pre_qr.rows(), pre_qr.cols());
+        
+        update_system();
 }
 
-void LQG::initialize(){
-        // set parameter matrix
-        pr.set_submatrix(0,0,S->_B());
-        pr.set_submatrix(0,dimu, S->_A());
-        
-        //penalization
-        qux=zeros(dimu,dimx+2*dimu+dimy);     
-        qux.set_submatrix(0,0,Qu);
-        qux.set_submatrix(0,dimx+dimu+dimy,Qu);
-
-        qyx=zeros(dimy, dimx+dimy+dimu);
-        qyx.set_submatrix(0,0,S->_C());
-        qyx.set_submatrix(0,dimx,-eye(dimy));
-        
-        //
-        s=1e-5*eye(dimx+dimu+dimy);
-        // parts of QR
-        hqy=Qy*qyx*pr;
-        
-        // pre_qr
-        pre_qr = concat_vertical(s*pr, concat_vertical(hqy, qux));
-        post_qr = zeros(pre_qr.rows(), pre_qr.cols());
-}
 
 }