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

Timestamp:

08/12/09 16:47:33 (15 years ago)

Author:

smidl

Message:

LQG basic implementation

Files:

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

library/bdm/design/ctrlbase.cpp

@@ -1 @@
-#include <ctrlbase.h>
+#include "ctrlbase.h"
 
-LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){
+namespace bdm{
+
+void LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){
         dimx = A0.rows();
         dimy = C0.rows();
@@ -13 +15 @@
         B=B0;
         C=C0;
-        
-        // set temporary stuff;
-        pr = concat_vertical( 
-                                        concat_horizontal(B,A, zeros(dimx, dimu+dimy)),
-                                        concat_horizontal(zeros(dimu+dimy,dimu+dimx), eye(dimu+dimy)));
+        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){
+        it_assert_debug ( Qy0.cols() == dimy, "LQG: wrong dimensions of Qy " );
+        it_assert_debug ( Qu0.cols() == dimu, "LQG: wrong dimensions of Qu " );
+        it_assert_debug ( y_req0.length() == dimy, "LQG: wrong dimensions of y_req " );
+
+        Qy=Qy0;
+        Qu=Qu0;
+        y_req=y_req0;
+        horizon = horizon0;
+        prepare_qr();
+}
+
+void LQG::prepare_qr(){
+        // set parameter matrix
+        pr.set_submatrix(0,0,B);
+        pr.set_submatrix(0,dimu, A);
         
         //penalization
@@ -24 +41 @@
         qux.set_submatrix(0,dimx+dimu+dimy,Qu);
 
-        qyx=concat_horizontal(C,-eye(dimy),zeros(dimy,dimu));
+        qyx=zeros(dimy, dimx+dimy+dimu);
+        qyx.set_submatrix(0,0,C);
+        qyx.set_submatrix(0,dimx,-eye(dimy));
         
         //
-        s=1e-5*eye(dimx+dimu+dimy);
+        s=1e-5*eye(4);//dimx+dimu+dimy);
         // parts of QR
         hqy=Qy*qyx*pr;
         
         // pre_qr
-        pre_qr = concat_vertical(s*pr, hqy, qux);
-        
+        pre_qr = concat_vertical(s*pr, concat_vertical(hqy, qux));
+        post_qr = zeros(pre_qr.rows(), pre_qr.cols());
 }
+
+}