% INVNDEMO Inverse Nyquist Array Demonstration for MFD Toolbox.

echo off
% P. Phaal, December 1987. Revised by J.M.Maciejowski, 11 Dec 87.
% Copyright (c) 1987,1993 by GEC Engineering Research Centre and 
% Cambridge Control Ltd
% History:
%       Made Matlab 4 compatible, 14.7.93, JMM.
%       zeros(kd) changed to zeros(size(kd)) in 2 places, 12.7.93, JMM.
%       MRN0023, MRN0026, MRN0027

clc
echo on

% This demo shows the use of the Inverse Nyquist Array (INA) method.

% It is based on the example in chapter 13 of [O'Reilly87]
% (see references in MFD Tutorial).

% The transfer function matrix:
%
%                      -                        -
%                     |      s+4          1      |
%                     |   ----------    ------   |
%                     |   (s+1)(s+5)     5s+1    |
%            G(s) =   |                          |
%                     |      s+1          2      |
%                     |   ----------    ------   |
%                     |    2                     |
%                     |   s +10s+100     2s+1    |
%                      -                        -
%
% can be expressed in the MFD toolbox as a numerator matrix and
% a common denominator.

pause % Strike any key to continue
clc

% The numerator and common denominator matrices for G(s) are:

num = [0 10 147 1499 4994 2940 400 0  2  33  346 1465 1650  500
       0 10  77  160  134   46   5 0 10 162 1682 6830 6300  1000];

den = [10 167 1763 7671 9715 4150 500];

% Now generate a vector w, containing the frequencies, in radians,
% at which the MVFR matrix is to be evaluated.

w = logspace(-2,2,20);   % Only 20 points to speed up demo

% Then generate the MVFR matrix

fg = mv2fr(num,den,w);

pause % Strike any key to continue
clc

% The inverse nyquist array consists of a matrix of plots, 
%                          -1
% one for each element of G  (jw).

% The function FINV allows the inverse of an MVFR matrix to be 
% computed.

ifg = finv(w,fg);

% Gershgorin circles computed using FRGERSH or FCGERSH allow the
% degree of diagonal dominance to be assessed. Diagonal precompensators
% preserve row dominance of inverse systems. Since we are going to try
% to use a diagonal precompensator, we can only change the column
% dominance, and hence we shall use FCGERSH.

pause % Strike any key to continue
clc

% Plotting the inverse nyquist array (INA).
%                                    -1
% Examining the INA plot shows that G   is not diagonally
% dominant, since the Gershgorin bands overlap the origin.

hold off
subplot(2,2,1); plotnyq(fget(w,ifg,[1 1])); % plot element (1,1)
hold on
circles = fcgersh(w,ifg,1);
plotnyq(circles,'--'); % plot gershgorin circles
hold off
subplot(2,2,2); plotnyq(fget(w,ifg,[1 2])); % plot element (1,2)
subplot(2,2,3); plotnyq(fget(w,ifg,[2 1])); % plot element (2,1)
subplot(2,2,4); plotnyq(fget(w,ifg,[2 2])); % plot element (2,2)
hold on
circles = fcgersh(w,ifg,2);
plotnyq(circles,'--') % plot gershgorin circles
hold off

pause  % Strike any key to continue
clc


% Perron-Frobenius eigenvalues can be used to design a diagonal
%                              -1
% precompensator K so that (GK)   will be diagonally dominant.

% First calculate the normalised comparison matrix of ifg
% (see Reference Manual entry for FPERRON):

m=abs(ifg);
omega = fdiag(w,fdiag(w,m));
nc = fmulf(w,finv(w,omega),m);  % Normalised Comparison Matrix

% Perron-Frobenius eigenvalues and eigenvectors are calculated 
% using FPERRON:

[v,l,r] = fperron(w,nc);

pause % Strike any key to continue
clc

% Plotting the Perron-Frobenius eigenvalues against w indicates the
% frequency ranges over which the system can be made diagonally 
% dominant by a diagonal precompensator. This is possible at those
% frequencies at which the magnitude of the P-F eigenvalue is 
% less than 6 decibels (ie 2).

pause   % Press any key to continue

% Plotting P-F eigenvalues against w
subplot(1,1,1);
plotdb(w,v), title('Perron-Frobenius eigenvalue')

% The display shows that the P-F eigenvalue is smaller than 6dB        
% at all freuencies.

pause  % Strike any key to continue
clc

% In order to find the diagonalising controller K, the left P-F
% eigenvector is examined.  This eigenvector is normalised
% so that the first element is 1.  The second element is then plotted
% against w.

for i=1:length(w), l(i,:)=l(i,:)/l(i,1); end  % Normalise eigenvector

plotdb(w,l(:,2))
title('Perron-Frobenius eigenvector 2')

pause  % Strike any key to continue
clc

% PHLAG can be used to find a transfer function whose gain change
% approximates that of the eigenvector:

[kn,kd] = phlag(-23,8,-7);  % Gain change of -23 dB, flattening out...
			    % above 8 rad/sec at level of -7 dB.

kn, kd

pause   % Press any key to continue
clc

% Compare its gain variation with that of the eigenvector:

k2 = mv2fr(kn,kd,w);  % Frequency response of kn(s)/kd(s)

% Now get its Bode magnitude plot.

hold on
plotdb(w,k2,'--')
hold off

pause
clc

% That looks pretty good. Now we are going to use the precompensator
% K = diag( 1, kd/kn ), but since we are working with inverse responses
% we shall premultiply inv(G) by diag( 1, kn/kd ):

ifk = mv2fr([kd zeros(size(kd));zeros(size(kd)) kn],kd,w); 
ifkg = fmulf(w,ifk,ifg);

pause   % Press any key to continue
clc

%                                                -1 -1
% Plotting the inverse Nyquist array for GK (ie K  G  ):
%                        -1
% The INA shows that (GK)   is diagonally dominant.

subplot(2,2,1);plotnyq(fget(w,ifkg,[1 1])); % plot element (1,1)
circles = fcgersh(w,ifkg,1);
hold on
plotnyq(circles,'--r');   % plot gershgorin circles
hold off
subplot(2,2,2);plotnyq(fget(w,ifkg,[1 2])); % plot element (1,2)
subplot(2,2,3);plotnyq(fget(w,ifkg,[2 1])); % plot element (2,1)
subplot(2,2,4);plotnyq(fget(w,ifkg,[2 2])); % plot element (2,2)
circles = fcgersh(w,ifkg,2);
hold on
plotnyq(circles,'--r')    % plot gershgorin circles
hold off
set(gcf,'Name','Gershgorin circles')

pause  % Strike any key to continue
clc

% The closed loop stability of a system can be determined by
% examining its INA. Since our system is open-loop stable, it
% will be stable with negative feedback provided
% by a diagonal gain matrix, D = diag(di) say, if the Gershgorin
% band swept out by the circles on the i'th diagonal element of 
% the INA does not touch the segment of the negative real axis 
% between the origin and the point -di.

% Ostrowski circles provide narrower bands, which can be used for 
% predicting closed-loop performance, in the same way as the inverse
% Nyquist (Whiteley) locus is used with SISO systems.
% The functions FROST and FCOST compute Ostrowski circles given 
% a diagonal feedback matrix.

% The Ostrowski bands for the (1,1) element of the INA of GK will
% be plotted for different feedback gains.

pause % Strike any key to continue
clc

% Plotting Ostrowski bands for the (1,1) element of the INA of GK
% for different values of d.  The feedback matrix, D = diag([d d]).
% The gains in each loop may be different in general.
% The Ostrowski bands become narrower with increasing feedback gain d.

d = [1 5 10 50];   % These are the cases we shall display.

pause   % Press any key to continue
clc 

for i = 1:4,
	subplot(2,2,i);
	plotnyq(fget(w,ifkg,[1 1]));title(sprintf('d = %2g',d(i)));
	axis([-20,20,-20,20]);
	circles = frost(w,ifkg,1,[d(i) d(i)]);
	hold on
	subplot(2,2,i); plotnyq(circles,'--');
	hold off
end
set(gcf,'Name','Ostrowski circles')

pause  % Strike any key to continue
clc

% Let's see the closed-loop step response if feedback gains of 10
% are applied in each loop.
% First we need state space models of the plant and compensator:

[ga,gb,gc,gd] = mvtf2ss(num,den);  % That's the plant

[ka,kb,kc,kd] = tf2ss(kd,kn); % The scalar kd(s)/kn(s), (1 state)
kb=[0 kb]; kc = [0 ; kc]; % Build the 2x2 compensator with transfer
kd = [1 0 ; 0 kd];        % function diag(1, kd(s)/kn(s))

kb = 10*kb; kd = 10*kd;   % Insert gain 10 in each loop (precompensator)

% Now make the series connection of the compensator with the plant, and
% put unity negative feedback around them.

[gka,gkb,gkc,gkd] = mvser(ka,kb,kc,kd,ga,gb,gc,gd); % Series connection

[ca,cb,cc,cd] = mvfb(gka,gkb,gkc,gkd,0,[0 0],[0;0],eye(2)); % Feedback

pause   % Strike any key to continue
clc

% Now plot the unit step responses over 1.5 seconds:

t = [0:3:150]/100;
y1 = step(ca,cb,cc,cd,1,t);
y2 = step(ca,cb,cc,cd,2,t);
subplot(2,1,1); plot(t,y1), title('Response to demand on output 1')
subplot(2,1,2); plot(t,y2), title('Response to demand on output 2')
set(gcf,'Name','factory')   % Reset figure name to default

pause % Strike any key to return to the demo menu.
echo off
clc