%% Loopshaping Example
%
% This file goes through a loopshaping design example and is
% accompanied by AEM 4321/EE 4231 notes.


%% Plant: G(s) = 10/s;
G = tf([10],[1 0]);

%% Performance Specifications: 
% - Desired Crossover Frequency
%   * wc = 1 rad/sec
% - Tracking Performance
%   * |S(j0.01)| <= 0.0004 --------> |L(j0.01)| >= 2501
%   * |S(j0.1)| <= 0.04 -----------> |L(j0.1)| >= 26
%   * |S(j0.2)| <= 0.2 ------------> |L(j0.2)| >= 6
% - Noise Rejection
%   * |T(j100)| <= 0.0004 ---------> |L(j100)| <= .0004
%   * |T(j10)| <= 0.04 ------------> |L(j10)| <= .0385
%   * |T(j3)| <= 0.3 --------------> |L(j3)| <= .23

% Desired crossover frequency, rad/sec
wc = 1;     

% Define Tracking Bounds: Upper bounds on S in dB
wS = [0.01 0.1 0.2];
ubS = [0.0004 0.04 0.2];
ubS_dB = 20*log10(ubS);
lbL = 1./ubS + 1;
lbL_dB = 20*log10(lbL);

% Define Noise Rejection Bounds: Upper bounds on T in dB
wT = [3 10 100];
ubT = [0.3 0.04 0.0004];
ubT_dB = 20*log10(ubT);
ubL = ubT ./ (1+ubT);
ubL_dB = 20*log10(ubL);

%% Stage 0: Plot Plant and Required Bounds on L

% Bode Magnitude Plot
figure(1);
bodemag(G,'b',{1e-2,1e2});
ylim([-70 70])
hold on;
plot(wS,lbL_dB,'ko',wT,ubL_dB,'kx');
legend('G','L lb','L ub','Location','Northeast');
grid
hold off;

if exist('garyfyFigure','file') 
    garyfyFigure;
end



%% Stage 1: Proportional Control to set the crossover frequency

% Proportional Gain
Gmag = abs( freqresp(G, wc) );
K1 = 1/Gmag;

% Loopshape with plant and K1
L1 = G*K1;

% Bode Magnitude Plot
figure(2);
bodemag(G,'b--',L1,'r',{1e-2,1e2});
ylim([-70 70])
hold on;
plot(wS,lbL_dB,'ko',wT,ubL_dB,'kx');
legend('G','L1','L lb','L ub','Location','Northeast');
grid
hold off;

if exist('garyfyFigure','file') 
    garyfyFigure;
end

%% Stage 2: Low frequency boost to increase the low frequency gain

% Low Frequency Boost
K2 = tf([1 0.3],[1 .3/50]); 

% Loopshape with plant, K1, and K2
L2 = G*K1*K2;

% Bode Magnitude Plot
figure(3);
bodemag(L1,'r--',L2,'g',{1e-2,1e2});
ylim([-70 70])
hold on;
plot(wS,lbL_dB,'ko',wT,ubL_dB,'kx');
legend('L1','L2','L lb','L ub','Location','Northeast');
grid
hold off;

if exist('garyfyFigure','file') 
    garyfyFigure;
end

%% Stage 3: High frequency rolloff to decrease the high frequency gain

% High Frequency Rolloff
K3 = tf([2.75],[1 2.75]);

% Loopshape with plant, K1, K2, and K3
L3 = G*K1*K2*K3;

% Bode Magnitude Plot
figure(4);
bodemag(L2,'g--',L3,'c',{1e-2,1e2});
ylim([-70 70])
hold on;
plot(wS,lbL_dB,'ko',wT,ubL_dB,'kx');
legend('L2','L3','L lb','L ub','Location','Northeast');
grid
hold off;

if exist('garyfyFigure','file') 
    garyfyFigure;
end

%% Final Results
% Compare plant G and final loopshape L3.
% Also plot S and T to verify that the requirements are satisfied.

% Final Controller
% This is simply the product of all stages
Kfinal = K1*K2*K3;

% Bode Magnitude Plot: G and L3
figure(5);
bodemag(G,'b',L3,'c',{1e-2,1e2});
ylim([-70 70])
hold on;
plot(wS,lbL_dB,'ko',wT,ubL_dB,'kx');
legend('G','L3','L lb','L ub','Location','Northeast');
grid
hold off;

if exist('garyfyFigure','file') 
    garyfyFigure;
end

% Bode Magnitude Plot: S and T
S = feedback(1,G*Kfinal);
T = feedback(G*Kfinal,1);

figure(6);
bodemag(S,'b',T,'r',{1e-2,1e2});
ylim([-70 10])
hold on;
plot(wS,ubS_dB,'ko',wT,ubT_dB,'kx');
legend('S','T','S ub','T ub','Location','Northeast');
grid
hold off;

if exist('garyfyFigure','file') 
    garyfyFigure;
end


