hinf_ctr.m

similer program for matlab

## Copyright (C) 1996, 2000, 2004, 2005, 2007
##               Auburn University.  All rights reserved.
##
## This file is part of Octave.
##
## Octave is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 3 of the License, or (at
## your option) any later version.
##
## Octave is distributed in the hope that it will be useful, but
## WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
## General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with Octave; see the file COPYING.  If not, see
## <http://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn {Function File} {@var{K} =} hinf_ctr (@var{dgs}, @var{f}, @var{h}, @var{z}, @var{g})
## Called by @code{hinfsyn} to compute the 
## @iftex
## @tex
## $ { \cal H }_\infty $
## @end tex
## @end iftex
## @ifinfo
## H-infinity
## @end ifinfo
## optimal controller.
##
## @strong{Inputs}
## @table @var
## @item dgs
## data structure returned by @code{is_dgkf}
## @item f
## @itemx h
## feedback and filter gain (not partitioned)
## @item g
## final gamma value
## @end table
## @strong{Outputs}
## @table @var
## @item K
## controller (system data structure)
## @end table
##
## Do not attempt to use this at home; no argument checking performed.
## @end deftypefn

## Author: A. S. Hodel <[e-mail]>
## Created: August 1995
## Revised by Kai P. Mueller April 1998 to solve the general H_infinity
## problem using unitary transformations Q (on w and z)
## and non-singular transformations R (on u and y).

function K = hinf_ctr (dgs, F, H, Z, g)

  if (nargin != 5)
    print_usage ();
  endif

  nw = dgs.nw;
  nu = dgs.nu;
  nz = dgs.nz;
  ny = dgs.ny;
  d22nz = dgs.Dyu_nz;

  B1  = dgs.Bw;
  B2  = dgs.Bu;
  C1  = dgs.Cz;
  C2  = dgs.Cy;
  C = [C1; C2];
  D11 = dgs.Dzw;
  D12 = dgs.Dzu;
  D21 = dgs.Dyw;
  D22 = dgs.Dyu;
  A = dgs.A;
  Ru = dgs.Ru;
  Ry = dgs.Ry;

  nout = nz + ny;
  nin = nw + nu;
  nstates = size (A, 1);

  F11 = F(1:(nw-ny),:);
  F12 = F((nw-ny+1):nw,:);
  F2  = F((nw+1):nin,:);
  H11 = H(:,1:(nz-nu));
  H12 = H(:,(nz-nu+1):nz);
  H2  = H(:,(nz+1):nout);

  ## D11 partitions
  D1111 = D11(1:(nz-nu),1:(nw-ny));
  D1112 = D11(1:(nz-nu),(nw-ny+1):nw);
  D1121 = D11((nz-nu+1):nz,1:(nw-ny));
  D1122 = D11((nz-nu+1):nz,(nw-ny+1):nw);

  ## D11ik may be the empty matrix, don't calculate with empty matrices
  [nd1111, md1111] = size (D1111);
  md1112 = length (D1112);
  md1121 = length (D1121);

  if (nd1111 == 0 || md1112 == 0)
    d11hat = -D1122;
  else
    xx = inv (g*g*eye(nz-nu) - D1111*D1111');
    d11hat = -D1121*D1111'*xx*D1112 - D1122;
  endif
  if (md1112 == 0)
    d21hat = eye (ny);
  elseif (nd1111 == 0)
    d21hat = chol (eye(ny) - D1112'*D1112/g/g);
  else
    xx = inv (g*g*eye(nz-nu) - D1111*D1111');
    xx = eye (ny) - D1112'*xx*D1112;
    d21hat = chol (xx);
  endif
  if (md1121 == 0)
    d12hat = eye (nu);
  elseif (md1111 == 0)
    d12hat = chol (eye(nu) - D1121*D1121'/g/g)';
  else
    xx = inv (g*g*eye(nw-ny) - D1111'*D1111);
    xx = eye (nu)-D1121*xx*D1121';
    d12hat = chol (xx)';
  endif

  b2hat = (B2+H12)*d12hat;
  c2hat = -d21hat*(C2+F12)*Z;
  b1hat = -H2 + (b2hat/d12hat)*d11hat;
  c1hat =  F2*Z + (d11hat/d21hat)*c2hat;
  ahat  =  A + H*C + (b2hat/d12hat)*c1hat;

  ## rescale controller by Ru and Ry
  b1hat = b1hat/Ry;
  c1hat = Ru\c1hat;
  bhat  = [b1hat, b2hat];
  chat  = [c1hat; c2hat];
  dhat  = [Ru\d11hat/Ry, Ru\d12hat; d21hat/Ry, 0*d11hat'];

  ## non-zero D22 is a special case
  if (d22nz)
    if (rank (eye(nu) + d11hat*D22) < nu)
      error (" *** cannot compute controller for D22 non-zero.");
    endif

    d22new = [D22, zeros(ny,ny); zeros(nu,nu), 0*D22'];
    xx = inv (eye(nu+ny) + d22new*dhat);
    mhat = inv (eye(nu+ny) + dhat*d22new);
    ahat = ahat - bhat*((eye(nu+ny)-xx)/dhat)*chat;
    bhat = bhat*xx;
    chat = mhat*chat;
    dhat = dhat*xx;

  endif

  K = ss (ahat, bhat(:,1:ny), chat(1:nu,:), dhat(1:nu,1:ny));

endfunction