Changeset 508

Timestamp:

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

Author:

smidl

Message:

LQG basic implementation

Location:

library

Files:

• bdm/CMakeLists.txt (modified) (1 diff)
• bdm/base/bdmbase.h (modified) (1 diff)
• bdm/design/ctrlbase.cpp (modified) (3 diffs)
• bdm/design/ctrlbase.h (modified) (6 diffs)
• bdm/itpp_ext.cpp (modified) (1 diff)
• bdm/itpp_ext.h (modified) (1 diff)
• tests/CMakeLists.txt (modified) (3 diffs)

library/bdm/CMakeLists.txt

@@ -2 +2 @@
 
 SET(bdm_base base/bdmbase.cpp base/bdmbase.h base/datasources.cpp base/datasources.h base/loggers.cpp base/loggers.h)
-SET(bdm_math math/square_mat.cpp math/square_mat.h math/chmat.cpp math/chmat.h math/functions.cpp math/functions.h)
+SET(bdm_math math/square_mat.cpp math/square_mat.h 
+        math/chmat.cpp math/chmat.h 
+        math/functions.cpp math/functions.h)
 SET(bdm_stat stat/exp_family.cpp stat/exp_family.h stat/emix.cpp stat/emix.h stat/merger.h stat/merger.cpp)
 SET(bdm_estim estim/kalman.cpp estim/kalman.h estim/particles.cpp estim/particles.h estim/arx.cpp estim/arx.h estim/mixtures.cpp estim/mixtures.h)
-SET(bdm_user_info base/libconfig/libconfigcpp.cc base/libconfig/grammar.c base/libconfig/libconfig.c base/libconfig/scanner.c base/user_info.cpp base/user_info.h )
+SET(bdm_ctrl design/ctrlbase.cpp design/ctrlbase.h)
+SET(bdm_user_info base/libconfig/libconfigcpp.cc base/libconfig/grammar.c base/libconfig/libconfig.c base/libconfig/scanner.c 
+        base/user_info.cpp base/user_info.h )
 SET(bdm_mex mex/mex_datasource.h mex/mex_parser.h mex/mex_logger.h  )
 

library/bdm/base/bdmbase.h

@@ -1 +1 @@
 /*!
   \file
-  \brief Bayesian Models (bm) that use Bayes rule to learn from observations
+  \brief Basic structures of probability calculus: random variables, probability densities, Bayes rule
   \author Vaclav Smidl.
 

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());
 }
+
+}

library/bdm/design/ctrlbase.h

@@ -11 +11 @@
 */
 
-#include <bdmbase.h>
+#include "../base/bdmbase.h"
+
+namespace bdm{
 
 //! Base class of designers of control strategy
@@ -17 +19 @@
         public:
                 //! Redesign control strategy 
-                virtual redesign(){it_error("Not implemented"); };
+                virtual void redesign(){it_error("Not implemented"); };
                 //! apply control strategy to obtain control input
                 virtual vec apply(const vec &cond){it_error("Not implemented"); return vec(0);}
-}
+};
 
 //! Linear Quadratic Gaussian designer for constant penalizations and constant target
@@ -31 +33 @@
                 int dimu;
                 //! dimension of output
-                /int dimy;
+                int dimy;
 
                 //! matrix A of the linear system
@@ -39 +41 @@
                 //! matrix C of the linear system
                 mat C;
-                //! expected value of x at time t
-                vec xt;
                 //! required value of the output y at time t (assumed constant)
                 vec y_req;
@@ -77 +77 @@
                 //! set system parameters from given matrices
                 void set_system_parameters(const mat &A, const mat &B, const mat &C);
+                //! set penalization matrices and control horizon
+                void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec y_req0, int horizon0);
                 //! set system parameters from Kalman filter
-                void set_system_parameters(const Kalman &K);
-                //! set current state
-                void set_state(const vec &xt0){xt=xt0;};
-                //! refresh temporary storage 
-                //! function for future use which is called at each time td;
-                virtual update_state(){};
+//              void set_system_parameters(const Kalman &K);
+                //! refresh temporary storage - inefficient can be improved
+                void prepare_qr();
+                //! function for future use which is called at each time td; Should call prepare_qr()!
+                virtual void update_state(){};
                 //! redesign one step of the 
                 void ricatti_step(){
                         pre_qr.set_submatrix(0,0,s*pr);
-                        pre_qr.set_submatrix(dimx, dimu+dimx, -Qy*y_req);
-                        post_qr=qr(pre_qr);
+                        pre_qr.set_submatrix(dimx+dimu+dimy, dimu+dimx, -Qy*y_req);
+                        if (!qr(pre_qr,post_qr)) it_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, dimu, 2*dimu+dimx+dimy);
+                        s=post_qr.get(dimu, 2*dimu+dimx+dimy-1, dimu, 2*dimu+dimx+dimy-1);
                 };
                 void redesign(){
@@ -101 +102 @@
                         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, dimu, 2*dimu+dimx+dimy);
+                        L = -inv(post_qr.get(0,dimu-1, 0,dimu-1)) * post_qr.get(0,dimu-1, dimu, 2*dimu+dimx+dimy-1);
                 }
-                vec apply(const vec &state, const vec &ukm){vec pom=concat_vertical(state, ones(dimy,1), ukm); return L*pom;}
-}
+                vec apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
+        } ;
+
+} // namespace

library/bdm/itpp_ext.cpp

@@ -371 +371 @@
 }
 
-}
+void triu(mat &A){
+        for(int i=1;i<A.rows();i++) { // row cycle
+                for (int j=0; j<i; j++) {A(i,j)=0;}
+        }
+}
+}

library/bdm/itpp_ext.h

@@ -100 +100 @@
 //! implementation of digamma (psi) function
 double psi ( double );
+
+//! implementation of matlab triu function
+void triu(mat &A);
 }
 

library/tests/CMakeLists.txt

@@ -7 +7 @@
 link_directories (./unittest-cpp)
 
-SET(testutil_src egiw_harness.cpp egiw_harness.h epdf_harness.cpp epdf_harness.h mat_checks.cpp mat_checks.h mpdf_harness.cpp mpdf_harness.h square_mat_point.cpp square_mat_point.h test_util.cpp test_util.h)
+SET(testutil_src egiw_harness.cpp egiw_harness.h epdf_harness.cpp epdf_harness.h mat_checks.cpp mat_checks.h 
+        mpdf_harness.cpp mpdf_harness.h square_mat_point.cpp square_mat_point.h test_util.cpp test_util.h)
 
 add_library(testutil ${testutil_src})
@@ -16 +17 @@
 LINK_EXEC(square_mat_stress)
 
-add_executable(square_mat_prep additive_generator.cpp additive_generator.h generator.cpp generator.h size_generator.cpp size_generator.h square_mat_prep.cpp)
+add_executable(square_mat_prep additive_generator.cpp additive_generator.h generator.cpp generator.h 
+        size_generator.cpp size_generator.h square_mat_prep.cpp)
 target_link_libraries(square_mat_prep testutil)
 LINK_EXEC(square_mat_prep)
@@ -39 +41 @@
 # using UnitTest++
 add_executable(testsuite datalink_test.cpp egiw_test.cpp emix_test.cpp epdf_test.cpp loggers_test.cpp merger_test.cpp mpdf_test.cpp rv_test.cpp shared_ptr_test.cpp square_mat_test.cpp testsuite.cpp user_info_test.cpp)
+        mpdf_test.cpp rv_test.cpp shared_ptr_test.cpp square_mat_test.cpp testsuite.cpp user_info_test.cpp)
 target_link_libraries(testsuite testutil unittest)
 LINK_EXEC(testsuite)