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

Timestamp:

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

Author:

smidl

Message:

LQG basic implementation

Files:

• library/bdm/design/ctrlbase.h (modified) (6 diffs)

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