Ticket #11: c2008lqcM.m

function [ulq]  = c2008lqcM(A, B, C, Xk, w, N, No, Nu, Qy, Qu, ukm1)

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
% funkce vrac��n� generovan�Q ��algoritmem.       /* tvar m - file */   %
% function [ulq]  = c2004lqM(AD, BD, CD, Xk, w, N, No, Nu, Qy, Qu, ukm1);             %
% Out:      ugpc     ��(mx1).                                                   %
% In:       discr. matice: /AD(stav�n]), BD(vstup�m]), CD(v� [rxn])/,    %
%           Xk - stav v k (nx1), w - �adan�� k+1:k+N-No (((N-No)*r)x1),        %
%           horizont: N = nstep (1) /No - necitlivosti (1), Nu - ��1)/,         %
%           penaliza� matice: Qy - v� (((N-No)*r)x((N-No)*r)) - (Qy !> 0)      %
%                               Qu - vstup�u*mxNu*m)                              %
%           ukm1 - predchozi hodnota ulq tj. ulq(k-1)                                 %     
%=====================================================================================%
% � Feb 2008, Jednoduch�Q ��a 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�n�apln� 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�enalizace % ??? 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
    hn=triu(qr([h;hqy;qux]));   % QR rozklad bez uchov� ortogon��atice 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;                % ���n z�an� synt�
% ulq=Lklq*[Xk;eye(r,1);eye(m,1)];             % �� z LQ synt�
  

%pom=ws*[Xk;ones(r,1);zeros(m,1)];
pom=ws*[Xk;ones(r,1);ukm1];
ulq=-inv(wsd)*pom;                % �� na z�d�Q synt�
%keyboard

% % %     uNu=zeros(Nu*m,1);   % nezn� z maticov�ovnice 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�rvn� m ak�ch z�h�. pro � k*Ts, krok k; m - po� vstup�k�ch z�h�% %     ugpc=uNu(1:m,1);   
% % %