%% Loop Shaping of HIMAT Pitch Axis Controller
% This example shows how to use Robust Control Toolbox™ to design a multi-input, multi-output controller by shaping the gain of an open-loop response across frequency. This technique is applied to controlling the pitch axis of a HIMAT aircraft.
% We show how to choose a suitable target loop shape and to use the loopsyn function to compute a multivariable controller that optimally matches the target loop shape.
%% Loop Shaping Specifications: Performance, Bandwidth, and Robustness
% Typically, loop shaping is a tradeoff between two potentially conflicting objectives. We want to maximize the open-loop gain to get the best possible performance, but for robustness, we need to drop the gain below 0dB where the model accuracy is poor and high gain might cause instabilities. This requires a good model where performance is needed (typically at low frequencies), and sufficient roll-off at higher frequencies where the model is often poor. The frequency Wc, where the gain crosses the 0dB line, is called the crossover frequency and marks the transition between performance and robustness requirements.
%
% Figure 1: Loop shaping specifications.
%% Target Loop Shape
% There are several guidelines for choosing a target loop shape Gd:
% Stability Robustness: Gd should have a gain of less than 0dB at high frequencies, where the plant model is so poor that the phase error may approach 180 degrees.
% Performance: Gd should have high gain where you want good control accuracy and good disturbance rejection.
% Crossover and Rolloff: Gd should cross the 0dB line between these two frequency regions, and roll off with a slope of -20 to -40 dB/decade past the crossover frequency Wc.
% A simple target loop shape is
% G_d = W_c/s
% where the crossover Wc is the reciprocal of the rise-time of the desired step response.
%% Model of NASA's HiMAT Aircraft
% For illustration purposes, let's use a six-state model of the longitudinal dynamics of the HiMAT aircraft trimmed at 25000 ft and 0.9 Mach. The aircraft dynamics are unstable, with two right-half plane phugoid modes.
%
% Figure 2: NASA HiMAT aircraft
% This model has two control inputs:
% Elevon deflection
% Canard deflection
% It also has two measured outputs:
% Angle of attack alpha
% Pitch angle theta
% The model is unreliable beyond 100 rad/sec. Fuselage bending modes and other uncertain factors induces deviations between the model and the true aircraft of as much as 20 decibels (or 1000%) beyond frequency 100 rad/sec.

ag = [
-2.2567e-02  -3.6617e+01  -1.8897e+01  -3.2090e+01   3.2509e+00  -7.6257e-01;
9.2572e-05  -1.8997e+00   9.8312e-01  -7.2562e-04  -1.7080e-01  -4.9652e-03;
1.2338e-02   1.1720e+01  -2.6316e+00   8.7582e-04  -3.1604e+01   2.2396e+01;
0            0   1.0000e+00            0            0            0;
0            0            0            0  -3.0000e+01            0;
0            0            0            0            0  -3.0000e+01];
bg = [0     0;
      0     0;
      0     0;
      0     0;
     30     0;
      0    30];
cg = [0     1     0     0     0     0;
      0     0     0     1     0     0];
dg = [0     0;
      0     0];
G = ss(ag,bg,cg,dg);
G.InputName = {'elevon','canard'};
G.OutputName = {'alpha','theta'};

clf
step(G), title('Open-loop step response of HIMAT aircraft')

% Our task is to control alpha and theta by issuing appropriate elevon and canard commands. We also want minimum spillover between channels - that is, a command in alpha should have minimum effect on theta and vice versa.
%
%% Loop Shaping Design Steps
% Designing a controller with loopsyn involves the following steps:
% Step 1: Look at the plant dynamics and responses
% Step 2: Specify the desired loop shape Gd
% Step 3: Use loopsyn to compute the optimal loop shaping controller
% Step 4: Analyze the shaped-loop L, closed-loop T, and sensitivity S
% Step 5: Verify that the closed-loop responses meet your specs.
%% Step 1: Analyzing the Plant Dynamics
% In this example, the aircraft model G has two unstable right-half plane phugoid-mode poles. It has one zero, which is in the left-half plane:

plant_poles = pole(G)
plant_zeros = tzero(G)

%
% We'll use sigma to plot the min and max I/O gain as a function of frequency:

clf, sigma(G,'g',{.1,100});
title('Singular value plot for aircraft model G(s)');

% Figure 4: Singular value plot for aircraft model G(s).
%
%% Step 2: Specifying the Target Desired Loop Shape Gd
% For this design we use the target loop shape Gd(s)=8/s corresponding to a rise time of about 1/8=0.125 sec.

s = zpk('s'); % Laplace variable s
Gd = 8/s;     
sigma(Gd,{.1 100})
grid
title('Target loop shape Gd(s).')

% create textarrow pointing to crossover frequency Wc
hold on;
plot([8,35],[0,21],'k.-'); 
plot(8,0,'kd');
plot([.1,100],[0 0],'k');
text(3,23,'Crossover Frequency \omega_c = 8');
hold off;

% Figure 5: Target loop shape Gd(s).
%
%% Step 3: Using LOOPSYN to Compute the Optimal Loop-Shaping Controller
% Now, we're ready to design an H-infinity controller K, such that the gains of the open-loop response G(s)*K(s) match the target-loop shape Gd as well as possible (while stabilizing the aircraft dynamics).

[K,CL,GAM] = loopsyn(G,Gd);
GAM

%
% The value GAM of about 1.6 indicates that the target-loop shape was met within +/-4.3dB (using 20*log10(GAM) = 4.3). Compare the singular values of the open-loop L = G*K with the target-loop shape Gd:

L = G*K;              % form the compensated loop L
sigma(Gd,'b',L,'r--',{.1,100});
grid
legend('Gd (target loop shape)','L (actual loop shape)');

% Figure 6: Singular values of L compared to Gd
%
%% Step 4: Analyzing the Shaped-Loop L, Closed-Loop T, and Sensitivity S
% Next we'll compare the gains of the open-loop transfer L, closed-loop transfer T, and sensitivity function S.

T = feedback(L,eye(2));
T.InputName = {'alpha command','theta command'};
S = eye(2)-T;

% SIGMA frequency response plots
sigma(inv(S),'m',T,'g',L,'r--',Gd,'b',Gd/GAM,'b:',...
	Gd*GAM,'b:',{.1,100})
legend('1/\sigma(S) performance',...
	'\sigma(T) robustness',...
	'\sigma(L) open loop',...
	'\sigma(Gd) target loop shape',...
	'\sigma(Gd) \pm GAM(dB)');
% Make lines wider and fonts larger

% set(findobj(gca,'Type','line','-not','Color','b'),'LineWidth',2);
h = findobj(gca,'Type','line','-not','Color','b');
set(h,'LineWidth',2);

% Figure 7: Sigma frequency response plots.
%
%% Step 5: Verifying that the Closed-Loop Responses Meet Your Specifications
% Finally, in this step we plot the step responses of the closed-loop system T.

step(T,8)
title('Responses to step commands for alpha and theta');

% Figure 8: Responses to step commands for alpha and theta.
% Our design looks good. The alpha and theta controls are fairly decoupled, overshoot is less the 15 percent, and peak time is only 0.5 sec.
%
%% Controller Simplification
% We just designed a 2-input, 2-output controller K with satisfactory performance. However this controller has fairly high order:

size(K)

%
% We can now use model reduction algorithms to try to simplify this controller while retaining its performance characteristics. First we compute the Hankel singular values of K to understand how many controller states effectively contribute to the control law:

hsv = hankelsv(K);
semilogy(hsv,'*--')
grid
title('Hankel singular values of K')
xlabel('Order')

% Figure 9: Hankel singular values of K.
%
% The Hankel singular values (HSV) measure the relative energy of each state in a balanced realization of K. The HSVs begin to drop off more steeply after the 10th state, so try computing a 10th-order approximation of K.

Kr = reduce(K,10);
order(Kr)

%
% The simplified controller Kr has order 10 compared to 17 for the initial controller K. Compare the approximation error K-Kr across frequency with the gains of K.

sigma(K,'b',K-Kr,'r-.',{1e-4,1e6})
legend('K','error K-Kr')

%
% This plot suggests that the approximation error is small in relative terms, so compare the closed-loop responses with the original and simplified controllers K and Kr.

Tr = feedback(G*Kr,eye(2));
step(T,'b',Tr,'r-.',8)
title('Responses to step commands for alpha and theta');
legend('K','Kr')

% Figure 11: Responses to step commands for alpha and theta.
% The two responses are indistinguishable so the simplified 10th-order controller Kr is a good substitute for K for implementation purposes.
% Copyright 1986-2012 The MathWorks, Inc.