pzplace(A,b,pw,zw)

function [k,c,d] = pzplace(A,b,pw,zw)
%PZPLACE state space pole and zero placement
%
%Usage:  [k,c,d] = pzplace(A,b,Newpoles,Newzeros)
%
%        This function works exactly the same way that the
%        MATLAB 'ACKER' command does.
%        It calculates the state feedback gain, k,
%        required to move the poles of a linear state space
%        system to the values specified in Newpoles.
%        When called without left hand arguments
%        pzplace returns the state feedback vector, k.
%
%        Also, calculates the c and d vectors required to
%        generate the zeros specified in Newzeros.
%        Uses the Bass-Gura method.
%
%        Tested under version 5.2.0
%
%        See also ACKER, PLACE, PZMAP
%                 SS, TF, SS2TF, TF2SS, ZPK, INVFREQS
%
%Reference: Ogata   pages 407-408, 396
%           Chen    pages 329, 334-338, 67
%           Kailath pages 197-199, 215, 651-661

%Paul Godfrey
[e-mail]

if nargin<1
   clc
   help pzplace
   return
end

n=max(size(A));

Poles=eig(A).';
denh=poly(Poles);

if not(exist('zw','var'))
   zw=[];
end

numw=poly(zw);
denw=poly(pw);

dd=0;
if length(zw)==n
   dd=1;
end

deltap=denw-denh;
deltap=deltap(2:n+1);

Ua=triu(toeplitz(denh(1:n)));
%form controllablity matrix
Ca(:,1)=b;
for m=2:n
   Ca(:,m)=A*Ca(:,m-1);
end

if rank(Ca)~=n
   disp('Sorry, this system is not controllable!')
end

k=deltap*pinv(Ca*Ua);
AA=A-b*k;
FinalPoles=eig(AA).';
denf=poly(FinalPoles);

while length(numw)<n+1
   numw=[0 numw];
end

deltaz=numw-dd*denf;
deltaz=deltaz(2:n+1);

Uaa=triu(toeplitz(denf(1:n)));
%form controllablity matrix
Caa(:,1)=b;
for m=2:n
   Caa(:,m)=AA*Caa(:,m-1);
end

if rank(Caa)~=n
   %we should never reach this statement since
   %we never change the controllability
   %of the original system
   disp('Sorry, new system is not controllable! (Due to roundoff error)')
end

cc=deltaz*pinv(Caa*Uaa);

A=AA;
b=b;
c=cc;
d=dd;
k=k;

return