phlead(varargin)

function C = phlead(varargin)
%PHLEAD Phase-lead compensator.
%   C = PHLEAD(W, LEADDEG) returns a state-space representation of a phase
%   lead compensator giving the specified phase lead angle LEADDEG and
%   unity gain at W rad/sec.

% Check input arguments and calculate constants.
[w, leadDeg] = parseInputArguments(varargin{:});
alpha = calcLeadConstant(leadDeg);

% Form C as a transfer function to start with. 
num = [alpha w];
den = [1 w*alpha];
C = tf(num, den);

% Convert to state-space.
C = ss(C);
C = prescale(C);

% Check we've calculated reasonably well - give warnings if not
checkResults(C, w, leadDeg);       

end



function alpha = calcLeadConstant(leadDeg)
%calcLeadConstant Calculate alpha to give a specified phase lead.
%   alpha = calcLeadConstant(leadDeg) returns the 'alpha' constant required
%   to give the specified phase lead.
%
%   A phase lead compensator has the following structure:
%
%             a*s + w
%      G(s) = -------
%             s + w*a
%
%   When s = j*w, this becomes
%
%               j*a*w + w   1+j*a      1+j*a
%      G(jw) =  --------- = ----- =  ---------
%               j*w + w*a    a+j     j*(1-j*a)
%
%   This has unit magnitude.  The phase is given by
%
%      arg G(jw) = arg(1+j*a) - arg(1-j*a) - arg(j)
%                = 2*arg(1+j*a) - arg(j)
%                = 2*atan(a) - 90 deg
%
%   This is the peak phase lead.  (At high or low frequencies, the phase
%   lead is zero.) 
leadRad = leadDeg * pi/180;
alpha = tan(leadRad/2 + pi/4);
end



function [w, leadDeg] = parseInputArguments(varargin)
error(nargchk(2, 2, nargin, 'struct')); %#ok<NCHKN>
w = varargin{1};
leadDeg = varargin{2};
assert(isValidW(w), 'phlead:infeasibleWValue', ...
    'Infeasible w value: w must be a real scalar in {x: 0 < x < inf}.');
assert(isValidLeadDeg(leadDeg), 'phlead:infeasibleLeadDegValue', ...
    ['Infeasible leadDeg value: must be a real scalar in ' ...
   '{x: 0 <= x < inf}.']);
end



function success = isValidW(x)
if ~isa(x, 'double')
    success = false;
else
    success = isscalar(x) & isreal(x) & all(x(:) > 0) & all(x(:) < inf);
end
end



function success = isValidLeadDeg(x)
if ~isa(x, 'double')
    success = false;
else
    success = isscalar(x) & isreal(x) & all(x(:) >= 0) & all(x(:) < 90);
end
end



function checkResults(C, w, leadDeg)
M = freqresp(C, w);
achievedLead = angle(M)*180/pi;
phaseErr = achievedLead - leadDeg;
phaseTol = 1e-2;
if abs(phaseErr) > phaseTol
    warning('phlead:phaseError', ...
        'Achieved phase lead is %0.2f but demand was %0.2f.', ...
        achievedLead, leadDeg);
end
achievedGain = 20*log10(abs(M));
gainErr = achievedGain;
gainTol = 1e-2;
if abs(gainErr) > gainTol
   warning('phlead:gainError', ...
        'Achieved gain at peak phase lead is %0.2f dB, not 0 dB.', ...
        achievedGain);
end 
end