doflq(par1,par2,par3,par4,par5)

function [out1,out2,out3] = doflq(par1,par2,par3,par4,par5)
%DOFLQ  Linear quadratic output feedback design for SISO discrete-time systems.
%
%       [Cfb,clP] = DOFLQ(P,H,lambda,Fy,Fu)  calculates the optimal output
%       feedback transfer function such that the control law
%               u[k] = -Cfb(q) y[k]
%       minimises the (frequency weighted) cost function
%               J = E {(Fy*y)^2 + lambda*(Fu*u)^2}
%       subject to the constraint equation:
%               y[k] = P(q)u[k] + H(q)e[k],
%       where e[k] is a zero mean white noise.
%       
%       Also returned is clP(q), the closed loop characteristic equation.
%       The frequency weights, Fy and Fu, are optional.
%
%       Constraints: * P(q), H(q), Fy(q) and Fu(q) must be transfer functions.
%                    * Lambda must be a non-negative scalar.
%
%
%       [S,R,clP] = DOFLQ(A,B,C,lambda)  calculates the optimal output feedback
%       transfer function such that the control law  u[k] = -S(q)/R(q) y[k]
%       minimises the cost function
%               J = E {y^2 + lambda*u^2}
%       subject to the constraint equation:
%               A(q)y[k] = B(q)u[k] + C(q)e[k],
%       where e[k] is a zero mean white noise.
%       
%       Also returned is clP(q), the closed loop characteristic equation.
%
%       Constraints: * A(q), B(q) and C(q) must be polynomials described by row
%                      vectors with the same length. Obs: Use leading zeros.
%                    * The first element of B(q) must be zero.
%                    * The first element of A(q) cannot be zero.
%                    * The polynomials cannot be zero.
%                    * Lambda must be a non-negative scalar.
%
%
%       See also: DLQR.

%       L.C. Kammer 13 Feb 96
%       $Date: 1997/12/01 00:25:25 $ - $Revision: 2.1 $
%       Copyright (c) 1996-97 by Leonardo C. Kammer.
%       Contact address: [e-mail]


% Verification of calling method: transfer functions or polynomials.
if isa(par1, 'tf')
   % Tests for conditions of error.
   error(nargchk(3,5,nargin));
   if nargin < 5
      Fu = 1;
   elseif isa(par5, 'tf')
      Fu = par5;
   else
      error('Parameter Fu must be a transfer function.');
   end
   if nargin < 4
      Fy = 1;
   elseif isa(par4, 'tf')
      Fy = par4;
   else
      error('Parameter Fy must be a transfer function.');
   end
   lb = par3;
   if ~isa(par2, 'tf')
      error('Parameter H must be a transfer function.');
   end
   P = par1;
   H = par2;
   
   Pbar = minreal(Fy * inv(Fu) * P);
   Hbar = minreal(Fy * H);
   
   PHbar = minreal([ss(Pbar) ss(Hbar)]);
   [B, A] = tfdata(tf(PHbar(1,1)), 'v');
   [C, D] = tfdata(tf(PHbar(1,2)), 'v');
   
   rC = dsort(roots(C));
   while max(abs(rC)) > 1
      rC(1) = 1/rC(1);
      rC = dsort(rC);
   end
   C = poly(rC);
   
   if A ~= D
      error('Not an appropriate ARMAX model.');
   end
   
   [Slq,Rlq, PC] = doflq(A, B, C, lb);
   out1 = minreal(tf(Slq, Rlq, -1) * Fy * inv(Fu));
   out2 = PC;
   
else
   % Tests for conditions of error.
   error(nargchk(4,4,nargin));
   A = par1;
   B = par2;
   C = par3;
   lb = par4;
   [mA,nA] = size(A);
   [mB,nB] = size(B);
   [mC,nC] = size(C);
   if (mA ~= 1) | (mB ~= 1) | (mC ~= 1)
      error('A, B and C must be polynomials described by row vectors')
   end
   if ~nA
      error('A, B and C polynomials cannot be empty.')
   end
   if (nB ~= nA) | (nC ~= nA) | (B(1) ~= 0)
      error('Degrees of polynomials must be equal and B[1]=0.')
   end
   if length(lb) > 1 | lb < 0
      error('Lambda must be a non-negative scalar.')
   end
   if A(1) == 0
      error('A[1] cannot be zero.')
   end
   if (max(abs(C)) == 0) | (max(abs(B)) == 0)
      error('B and C polynomials cannot be zero.')
   end
   
   % Adjusting data for computations. Polynomials A and C must be monic.
   if A(1) ~= 1
      C = C / A(1);
      B = B / A(1);
      A = A / A(1);
   end
   while C(1) == 0
      C = [C(2:nC) 0];
   end
   if C(1) ~= 1
      C = C / C(1);
   end
   
   
   rPP = lb*conv(A,fliplr(A))+conv(B,fliplr(B));
   rts = flipud(dsort(roots(rPP)));
   P = poly(rts(1:nA-1));
   PC=conv(P,C);
   
   PP = conv(P,fliplr(P));
   r = sum(abs(rPP))/sum(abs(PP));
   
   As = fliplr(A);
   rP = r*P;
   
   M = zeros(2*nA-2);
   for i=1:nA-1
      M(i:i+nA-1,i) = As';
      M(i:i+nA-1,i+nA-1) = rP';
   end
   BCs = conv(B(2:nA),fliplr(C));
   XSs = M \ BCs';
   
   X(1:nA) = [0 XSs(1:nA-1)'];
   Ss = [0 XSs(nA:2*nA-2)'];
   S = real(fliplr(Ss));
   
   
   B1 = B;
   while B1(1) == 0
      B1 = B1(2:length(B1));
   end
   [Rs,res1] = deconv(conv(fliplr(P),X)+lb*conv(A,Ss), B1);
   R = real(fliplr(Rs(length(Rs)-nA+1:length(Rs))));
   
   res2 = sum(abs(conv(A,R)+conv(B,S)-PC));
   if (sum(abs(res1)) > 1e-5) | (res2 > 1e-5)
     disp('Warning: Residuals of the spectral factorisation are too big. (?!)')
   end
   
   out1 = S;
   out2 = R;
   out3 = PC;
   
end