%
% MAYNE  Pole placement gain selection using the Mayne-Murdoch formula.
%        K = MAYNE(A,B,P)  calculates the feedback gain matrix K such that
%        the single input system
%                .
%                x = Ax + Bu 
% 
%        with a feedback law of  u = -Kx  has closed loop poles at the 
%        values specified in vector P, i.e.,  P = eig(A-B*K).
% 
%        See also ACKER, PLACE.
% 
%        Written by: Kjetil Havre, 12/11-1996.
%
%        References: 
%           Kailath, T., Linear Systems, Prentice-Hall, 1980, p. 202.
%
%           Mayne, D. Q. and  P. Murdoch,  ``Modal control of Linear Time 
%           Invariant Systems'' Int. J. Control, Vol. 11, pp. 223-227.
%
%           Porter, B. and P. Murdoch, 
%           IEEE Trans. Autom. Control, AC-20, p. 582, Aug. 1975.
%

function K = mayne(A,B,P)
   n1 = size(P,1);
   n2 = size(P,2);
   if n2 > n1
      P = P.';  hp = n1;  n1 = n2;  n2 = n1;
   end
   Po = eig(A);
   nx = size(B,1);
   nu = size(B,2);
   
   if nx ~= n1
      disp('Error: Incorrect number of poles is spesified.');
      return
   end
   if nx ~= size(A,1)
      disp('Error: dimemsions on A, B  does not agree.');
      return
   end
   if nu ~= 1
      disp('Error: Single input only.');
      return
   end
   [XR,DR] = eig(A);
   if rank(XR) < nx 
      disp('Error: System is not diagonalizable'); 
   end
   Bd = inv(XR)*B;     % Transform the B matrix to new states.
   onnx = ones(nx,1);
   onnx1 = ones(nx-1,1);
   for i = 1:nx 
      Pom = Po([1:i-1,i+1:nx]);
      K(i) = prod(onnx*Po(i)-P)/prod(onnx1*Po(i)-Pom)/Bd(i);
   end
   K = K*inv(XR);      % Transform back to the old states. 
end











