Index: /library/bdm/design/ctrlbase.cpp
===================================================================
--- /library/bdm/design/ctrlbase.cpp (revision 491)
+++ /library/bdm/design/ctrlbase.cpp (revision 491)
@@ -0,0 +1,36 @@
+#include <ctrlbase.h>
+
+LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){
+	dimx = A0.rows();
+	dimy = C0.rows();
+	dimu = B0.cols();
+
+	it_assert_debug ( A0.cols() == dimx, "LQG: A is not square" );
+	it_assert_debug ( B0.rows() == dimx, "LQG: B is not compatible" );
+	it_assert_debug ( C0.cols() == dimx, "LQG: C is not square" );
+
+	A=A0;
+	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)));
+	
+	//penalization
+	qux=zeros(dimu,dimx+2*dimu+dimy);     
+	qux.set_submatrix(0,0,Qu);
+	qux.set_submatrix(0,dimx+dimu+dimy,Qu);
+
+	qyx=concat_horizontal(C,-eye(dimy),zeros(dimy,dimu));
+	
+	//
+	s=1e-5*eye(dimx+dimu+dimy);
+	// parts of QR
+	hqy=Qy*qyx*pr;
+	
+	// pre_qr
+	pre_qr = concat_vertical(s*pr, hqy, qux);
+	
+}
Index: /library/bdm/design/c2008lqcM.m
===================================================================
--- /library/bdm/design/c2008lqcM.m (revision 491)
+++ /library/bdm/design/c2008lqcM.m (revision 491)
@@ -0,0 +1,103 @@
+function [ulq]  = c2008lqcM(A, B, C, Xk, w, N, No, Nu, Qy, Qu, ukm1)
+
+% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
+% funkce vrac� ��zen� u generovan� LQ ��dic�m algoritmem.       /* tvar m - file */   %
+% function [ulq]  = c2004lqM(AD, BD, CD, Xk, w, N, No, Nu, Qy, Qu, ukm1);             %
+% Out:      ugpc     ��zen� u(mx1).                                                   %
+% In:       discr. matice: /AD(stav� [nxn]), BD(vstup� [nxm]), CD(v�stup� [rxn])/,    %
+%           Xk - stav v k (nx1), w - �adan� v�stupy k+1:k+N-No (((N-No)*r)x1),        %
+%           horizont: N = nstep (1) /No - necitlivosti (1), Nu - ��zen� (1)/,         %
+%           penaliza�n� matice: Qy - v�stup� (((N-No)*r)x((N-No)*r)) - (Qy !> 0)      %
+%                               Qu - vstup�  (Nu*mxNu*m)                              %
+%           ukm1 - predchozi hodnota ulq tj. ulq(k-1)                                 %     
+%=====================================================================================%
+% � Feb 2008, Jednoduch� LQ ��zen� na horizontu N = nstep                             %
+%=====================================================================================%
+%
+% [rr,ss,kon,clw,clu0,cld,s,ds,du]=lqex2(b,a,k,nstep,qur,d)
+%
+%  LQ optimal controller with external disturbance
+%
+% b(z^-1)/(a(z^-1) system discrete transfer function
+% k/(a(z^-1)  system offset
+% nstep number of iterations of the Riccati equation (horizon)
+% qur input penalization
+% d(z^-1)/(a(z^-1) disturbance discrete transfer function
+%
+% ss(z^-1)/rr(z^-1) controller feedback part transfer function
+% kon/rr(z^-1) offset term of the controller
+% clw/rr(z^-1) transfer function of the output reference  filter
+% clu0/rr(z^-1)transfer function  of the input refrence  filter
+% cld(z^-1)/rr(z^-1)transfer function  of the disturbance  filter
+% s,ds Riccati matrix factors s'* ds * s
+% du weight of the control law
+%
+
+echo on
+  
+n=size(A,1); m=size(B,2); r=size(C,1);
+  
+qy=eye(r)*Qy(1); Qu=eye(m)*Qu(1); qx=C'*qy*C; % do�asn� napln�n� matic penalizac�
+  
+if 0,
+    if ~isequal(size(qx),[n n]), error('bad dimension of state penalization');  end 
+    if ~isequal(size(qy),[r r]), error('bad dimension of output penalization'); end
+    if ~isequal(size(qu),[m m]), error('bad dimension of input penalization'); end
+    if No<0, error('bad initial insensitivity horizon');  end
+    if Nu<0, error('bad control horizon');  end  
+end
+  
+qux=zeros(m,n+2*m+r);      % qyx=zeros(r,n+m+r);     % pomocn� penalizace % ??? qux=zeros(m,n+2*m+r); qyx=qux;  
+qux(:,1:m)=eye(m);
+qux(:,n+m+r+1:end)=-eye(m);
+qux=qux*Qu(1);
+qyx=[C,-eye(r),zeros(r,m)];
+
+%if size(s)~=size(eye(n+m+r)), s=s*eye(n+m+r);  disp('1'), end
+s=1e-5*eye(n+m+r); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+pr=[B,A, zeros(n, m+r); zeros(m+r,m+n), eye(m+r)]; % paramentry 
+
+% size(pr),
+% size(qyx)  
+%h=s*pr;
+%hqx=qx*pr;
+  
+% hqy=qyx*pr
+hqy=qy*qyx*pr;
+% keyboard
+  
+nstep=N;
+for i=1:nstep,
+    h= s*pr; % s1=s;
+    % diag(w((N-i)*r+1:(N-i)*r+r))
+    hqy(:,m+n+1:m+n+r)=-qy*diag(w((N-i)*r+1:(N-i)*r+r));
+%     hqy
+%     keyboard
+pomqr=[h;hqy;qux];
+    hn=triu(qr(pomqr));   % QR rozklad bez uchov�n� ortogon�ln� matice Q
+    s=hn(m+1:2*m+n+r,m+1:2*m+n+r);
+end
+ 
+ws=hn(1:m,m+1:2*m+n+r);
+wsd=hn(1:m,1:m);
+  
+
+% Lklq=-inv(wsd)*ws;                % ��dic� z�kon z�skan� z LQ synt�zy
+% ulq=Lklq*[Xk;eye(r,1);eye(m,1)];             % ��zen� u z LQ synt�zy
+  
+
+%pom=ws*[Xk;ones(r,1);zeros(m,1)];
+pom=ws*[Xk;ones(r,1);ukm1];
+ulq=-inv(wsd)*pom;                % ��zen� u na z�lad� LQ synt�zy
+keyboard
+
+% % %     uNu=zeros(Nu*m,1);   % nezn�m� z maticov� rovnice R*uNu=c
+% % %     uNu(Nu*m,1)=c(Nu*m,1)/(R(Nu*m,Nu*m));
+% % %     for i=(Nu*m-1):-1:1,
+% % %         uNu(i,1)=(1/(R(i,i)))*(c(i,1)-R(i,i+1:Nu*m)*uNu(i+1:Nu*m,1));
+% % %     end
+% % %     
+% % %     % V�b�r prvn�ch m ak�n�ch z�sah� (tj. pro �as k*Ts, krok k; m - po�et vstup� - ak�n�ch z�sah�.
+% % %     ugpc=uNu(1:m,1);   
+% % %    
Index: /library/bdm/design/ctrlbase.h
===================================================================
--- /library/bdm/design/ctrlbase.h (revision 491)
+++ /library/bdm/design/ctrlbase.h (revision 491)
@@ -0,0 +1,106 @@
+/*!
+  \file
+  \brief Base classes for designers of control strategy
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#include <bdmbase.h>
+
+//! Base class of designers of control strategy
+class Designer : public root {
+	public:
+		//! Redesign control strategy 
+		virtual 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
+//! Its internals are very close to Kalman estimator
+class LQG : public Designer {
+	protected:
+		//! dimension of state
+		int dimx;
+		//! dimension of inputs
+		int dimu;
+		//! dimension of output
+		/int dimy;
+
+		//! matrix A of the linear system
+		mat A;
+		//! matrix B of the linear system
+		mat B;
+		//! 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;
+		
+		//! Control horizon, set to maxint for infinite horizons
+		int horizon;
+		//! penalization matrix Qy
+		mat Qy;
+		//! penalization matrix Qu
+		mat Qu;
+		//! time of the design step - from horizon->0
+		int td;
+		//! controller parameters
+		mat L;
+		
+		//!@{ \name temporary storage for ricatti
+		//!  parameters
+		mat pr;
+		//! penalization 
+		mat qux;
+		//! penalization
+		mat qyx;
+		//! internal quadratic form 
+		mat s;
+		//! penalization
+		mat qy;
+		//! pre_qr part
+		mat hqy;
+		//! pre qr matrix
+		mat pre_qr;
+		//! post qr matrix
+		mat post_qr;
+		//!@}
+		
+	public:
+		//! set system parameters from given matrices
+		void set_system_parameters(const mat &A, const mat &B, const mat &C);
+		//! 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(){};
+		//! 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);
+			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);
+		};
+		void redesign(){
+			for(td=horizon; td>0; td--){
+				update_state();
+				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, dimu, 2*dimu+dimx+dimy);
+		}
+		vec apply(const vec &state, const vec &ukm){vec pom=concat_vertical(state, ones(dimy,1), ukm); return L*pom;}
+}
