Changeset 738 for library/bdm/design

Timestamp:

11/25/09 12:46:08 (15 years ago)

Author:

mido

Message:

a few moves of code from h to cpp, however, only part of the whole library is done

Location:

library/bdm/design

Files:

• ctrlbase.cpp (modified) (2 diffs)
• ctrlbase.h (modified) (2 diffs)

library/bdm/design/ctrlbase.cpp

@@ -7 +7 @@
 }
 
+void LQG::update_system() {
+        pr.set_submatrix ( 0, 0, S->_B() );
+        pr.set_submatrix ( 0, dimu, S->_A() );
+
+        //penalization
+        qux.set_submatrix ( 0, 0, Qu._Ch() );
+        qux.set_submatrix ( 0, dimx + dimu + dimy, Qu._Ch() );
+
+        qyx.set_submatrix ( 0, 0, S->_C() );
+        qyx.set_submatrix ( 0, dimx, -eye ( dimy ) );
+
+        // parts of QR
+        hqy = Qy.to_mat() * qyx * pr;
+
+        // pre_qr
+        pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) );
+}
+        
 void LQG::set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ) {
         Qy = Qy0;
@@ -38 +56 @@
 }
 
+void LQG::ricatti_step() {
+        pre_qr.set_submatrix ( 0, 0, s*pr );
+        pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req );
+        if ( !qr ( pre_qr, post_qr ) ) {
+                bdm_warning ( "QR in LQG unstable" );
+        }
+        triu ( post_qr );
+        // hn(m+1:2*m+n+r,m+1:2*m+n+r);
+        s = post_qr.get ( dimu, 2 * dimu + dimx + dimy - 1, dimu, 2 * dimu + dimx + dimy - 1 );
+};
+
+void LQG::redesign() {
+        for ( td = horizon; td > 0; td-- ) {
+                update_system();
+                ricatti_step();
+        }
+        /*                      ws=hn(1:m,m+1:2*m+n+r);
+                                wsd=hn(1:m,1:m);
+                                Lklq=-inv(wsd)*ws;*/
+        L = -inv ( post_qr.get ( 0, dimu - 1, 0, dimu - 1 ) ) * post_qr.get ( 0, dimu - 1, dimu, 2 * dimu + dimx + dimy - 1 );
+}
+
 
 }

library/bdm/design/ctrlbase.h

@@ -106 +106 @@
         void set_system ( shared_ptr<StateSpace<chmat> > S0 );
         //! update internal whan system has changed
-        void update_system() {
-                pr.set_submatrix ( 0, 0, S->_B() );
-                pr.set_submatrix ( 0, dimu, S->_A() );
-
-                //penalization
-                qux.set_submatrix ( 0, 0, Qu._Ch() );
-                qux.set_submatrix ( 0, dimx + dimu + dimy, Qu._Ch() );
-
-                qyx.set_submatrix ( 0, 0, S->_C() );
-                qyx.set_submatrix ( 0, dimx, -eye ( dimy ) );
-
-                // parts of QR
-                hqy = Qy.to_mat() * qyx * pr;
-
-                // pre_qr
-                pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) );
-        }
+        void update_system();
         //! set penalization matrices and control horizon
         void set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 );
@@ -137 +121 @@
         //! function for future use which is called at each time td; Should call initialize()!
         //! redesign one step of the
-        void ricatti_step() {
-                pre_qr.set_submatrix ( 0, 0, s*pr );
-                pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req );
-                if ( !qr ( pre_qr, post_qr ) ) {
-                        bdm_warning ( "QR in LQG unstable" );
-                }
-                triu ( post_qr );
-                // hn(m+1:2*m+n+r,m+1:2*m+n+r);
-                s = post_qr.get ( dimu, 2 * dimu + dimx + dimy - 1, dimu, 2 * dimu + dimx + dimy - 1 );
-        };
-        void redesign() {
-                for ( td = horizon; td > 0; td-- ) {
-                        update_system();
-                        ricatti_step();
-                }
-                /*                      ws=hn(1:m,m+1:2*m+n+r);
-                                        wsd=hn(1:m,1:m);
-                                        Lklq=-inv(wsd)*ws;*/
-                L = -inv ( post_qr.get ( 0, dimu - 1, 0, dimu - 1 ) ) * post_qr.get ( 0, dimu - 1, dimu, 2 * dimu + dimx + dimy - 1 );
-        }
+        void ricatti_step();
+        
+        void redesign();
+
         //! compute control action
         vec ctrlaction ( const vec &state, const vec &ukm ) const {