freplot.m

hard disk drive servo control tools, it is very helpful

function [Gm,Pm,Wcg,Wcp] = freplot(A,B,C,D,Av,Bv,Cv,Dv,w,option)

% [Gm,Pm,Wcg,Wcp] = freplot(A,B,C,D,Acmp,Bcmp,Ccmp,Dcmp,w[,option])
%
% This function is to generate Bode plot, Nyquist plot, sensitivity and
% complementary functions for the following system:
%           .
%           x = A x + B u,     y = C x + D u
% 
% with the measurement feedback
%           .
%           xc = Acmp xc + Bcmp y,   u = Ccmp xc + Dcmp y
%
% and with a pre-specific frequency range in w (in logspace). It also 
% returns relevant gain margin (in normal scale) and phase margins 
% (in degree), and the gain and phase crossover frequencies (in rad/sec).
%
% If option = 'Hz' or 'hz', all plots are done in Hz. Otherwise, they 
% will be in rad/sec as default. 

%
% Ben M. Chen at National University of Singapore, October 7, 2005
%

if nargin == 9
    option='xx';
    W=w;
    XLAB='Frequency (rad/sec)';
end
if sum(single(option)) == 194
    W=w*2*pi;
    XLAB='Frequency (Hz)';
elseif sum(single(option)) == 226
    W=w*2*pi;
    XLAB='Frequency (Hz)';
else
    W=w;
    XLAB='Frequency (rad/sec)'; 
end

[n,m] = size(B);
nv = max(size(Av));

Ao = [A zeros(n,nv); Bv*C Av];
Bo = [B; Bv*D];
Co = - [Dv*C Cv];
Do = - Dv*D;

x = inv(1-Dv*D);

Ac = [A+B*x*Dv*C B*x*Cv; Bv*C+Bv*D*x*Dv*C Av+Bv*D*x*Cv];
Bc = [B*x*Dv; Bv+Bv*D*x*Dv];
Cc = [C+D*x*Dv*C D*x*Cv];
Dc = 1+D*x*Dv;

Open = ss(Ao,Bo,Co,Do);

[Mo,Po]=bode(Open,W);

figure(1)
subplot(211)
semilogx(w,20*log10(Mo(1,:)))
grid
title('Bode plot')
xlabel(XLAB)
ylabel('Magnitude (dB)')
subplot(212)
semilogx(w,Po(1,:))
grid
xlabel(XLAB)
ylabel('Phase (degrees)')

[Gm,Pm,Wcg,Wcp]=margin(Open);

figure(2)
nyquist(Open,W);

Closed = ss(Ac,Bc,Cc,Dc);

[S,X]=bode(Closed,W);

TF4T = ss(Ac,Bc,-Cc,1-Dc);
[T,Y]=bode(TF4T,W);

figure(3)
semilogx(w,20*log10(S(1,:)),'-',w,20*log10(T(1,:)),'--')
grid
title('Sensitivity and Complementary Sensitivity Functions')
xlabel(XLAB)
ylabel('Magnitude (dB)')

% End ...