%% Robust control for a HIMAT aircraft using mixed-sensitivity H-infinity minimization 


%% Entering the (nominal) state-space model of the system
% The longitudinal dynamics of the HIMAT aircraft trimmed at 25000 ft and 0.9 Machmodel 
% can be found in Robust Control Toolbox materials at <https://www.mathworks.com/help/robust/examples/loop-shaping-of-himat-pitch-axis-controller.html>.

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];
G0=ss(ag,bg,cg,dg);
set(G0,'InputName',{'\delta_e','\delta_c'},'OutputName',{'\alfa','\theta'})

%% 
% Some insight into the dynamics of the (linearized model of the) system can be earned 
% by looking at poles and zeros of the system. 

pole(G0)
tzero(G0)

%% Weighting filter for the mixed-sensitivity design
% We first create the weighting filter that captures the performance requirements

MS=2; AS=.003; WS=5;
W1=tf([1/MS,WS],[1,AS*WS]);

%%
% We then continute by specifying the weighting filter that would bring in
% some penalty for the magnitude of the generated control signal.

W2=[];                  % you can also experiment with setting this weight to zero but 
                        % you will see that it just does not work well. 
                            
W2 = 0.01*eye(2,2);     % it seems that setting this filter as a proportional gain is just enough.

%%
% Finally, we define a filter that models an uncertainty within the
% input-multiplicative uncertainty structure

tau = 1/50; r0 = 0.3; rinf = 10;
W3 = tf([tau,r0],[tau/rinf,1]);

%% Mixed sensitivity control design
% As easy as calling a mixsyn function

[K0,CL0,GAM0]=mixsyn(G0,W1,W2,W3);

%% Building the closed-loop transfer functions

L0=G0*K0;
I=eye(size(L0));
S0=feedback(I,L0); 
T0=I-S0;
set(T0,'InputName',{'\alpha_{ref}','\theta_{ref}'},'OutputName',{'\alpha','\theta'})

%% Plotting various nominal closed-loop responses

figure(1)
step(T0,1.5);
title('Response of \alpha and \theta unit steps in both references');

figure(2);
sigma(S0,'r',T0,'b',1/W1,'r--',1/W3,'b--');grid on
legend('\sigma(S)',...
'\sigma(T)',...
'1/|W1| upper bound on performance',...
'1/|W3| upper bound on robustness')
title('Sensitivity and complementary sensitivity'), xlabel('Frequency [rad/s]'), ylabel('Magnitude')

figure(3)
sigma(K0*S0)
title('Input sensitivity'), xlabel('Frequency [rad/s]'), ylabel('Magnitude')

figure(4)
step(K0*S0)
title('Control signal in response to a unit step in references \alpha a \theta') 

%% Plotting responses of the closed-loop responses for the uncertain system
% Just out of curiosity because essentiall all the relevant information is already contained in the 
% plots for the nominal system together with the weighting filters.

%%
% We first build all the relevant closed-loop responses with the uncertain system

Delta = ultidyn('Delta');
G = (1+Delta*W3)*G0;
L=G*K0;
S=feedback(I,L); 
T=I-S;
set(T,'InputName',{'\alpha_{ref}','\theta_{ref}'},'OutputName',{'\alpha','\theta'})

%%
% It is now possibel to visualize not only the responses of the
% uncontrolled system

figure(5)
step(G,1.5)
title('Response of \alpha and \theta unit steps in both references');

figure(6)
sigma(G)
title(''), xlabel('Frequency [rad/s]'), ylabel('Magnitude')

%%
% but we can also plot various response of the closed-loop(s) 

figure(7);
sigma(S,'r',T,'b',1/W1,'r--',1/W3,'b--');grid on
legend('\sigma(S)',...
'\sigma(T)',...
'1/|W1| upper bound on performance',...
'1/|W3| upper bound on robustness')
title('Sensitivity and complementary sensitivity'), xlabel('Frequency [rad/s]'), ylabel('Magnitude')

figure(7)
sigma(K0*S)
title('Input sensitivity'), xlabel('Frequency [rad/s]'), ylabel('Magnitude')