root/applications/dual/experiment/lq_P.m @ 805

function [ulq]  = c2008lqcM(A, B, C, Xt, yp, N, P)
% function computes LQ controller for unit Qy and zero Qu with given
% uncertainty matrix P, which is a N-dimensional cell of P matrices on the considered
% horizon.

% vector of the quadratic form z is [u,y_{t-1},1]


dx=size(A,1); du=size(B,2); dy=size(C,1);

Qy = eye(dy);
qx=C'*Qy*C; 

qux=zeros(du,dx+du+dy); 
if ~iscell(P)
    qux(:,1:du)=P;
end
qyx=[C,-eye(dy)];


s=1e-5*eye(dx+dy); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pr=[B,A, zeros(dx, dy); 
    zeros(dy,dx+du), eye(dy)]; 

hqy=Qy*qyx*pr
hqy(:,du+dx+1:du+dx+dy)=-Qy*diag(yp)
  
nstep=N;
for i=1:nstep,
    if iscell(P)
        qux(:,1:du)=P{nstep-i+1};
    end
        
    h= s*pr;
    pomqr=[h;hqy;qux];
    hn=triu(qr(pomqr));   
    s=hn(du+1:end,du+1:end);

    ws=hn(1:du,du+1:end);
    wsd=hn(1:du,1:du);
  
    Bellman_core=s'*s
    L=-inv(wsd)*ws
    sprintf('u_t = %f y_{t-1} + %f ',L(1), L(2))

end
 
ulq=L*[Xt;ones(dy,1)];                % ��zen� u na z�lad� LQ synt�zy
keyboard