%% Loop-Shaping Controller Design
% One of the most powerful yet simple controller synthesis tools is
% <docid:robust_ref.f10-70773 loopsyn>. Given an LTI plant, you specify the shape
% of the open-loop systems frequency response plot that you want. Then
% |loopsyn| computes a stabilizing
% controller that best approximates your specified loop shape.
%%
% For example, consider the 2-by-2 NASA HiMAT aircraft model (Safonov,
% Laub, and Hartmann [1]), depicted in the following diagram. 
% 
% 
% <<../himat_diagram.gif>>
% 
%% 
% The control variables are elevon and canard actuators ($\delta_e$ and
% $\delta_c$). The output variables are angle of attack ($\alpha$) and
% attitude angle ($\theta$). The model has six states, given by:
%
% $$x = \left[ {\matrix{
%    {{x_1}}  \cr 
%   {{x_2}}  \cr 
%   {{x_3}}  \cr 
%   {{x_4}}  \cr 
%   {{x_5}}  \cr 
%   {{x_6}}  \cr 
% } } \right] = \left[ {\matrix{
%   {\dot \alpha }  \cr 
%   \alpha   \cr 
%   {\dot \theta }  \cr 
%   \theta   \cr 
%   {{x_e}}  \cr 
%   {{x_\delta }}  \cr 
% } } \right],$$
% 
% where $x_e$ and $x_{\delta}$ are the elevator and canard actuator states,
% respectively.
%% Create HiMAT Model
% The following commands create a state-space model |G| of the aircraft.
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);
%% Design Controller
% To design a controller to shape the frequency response (singular-value)
% plot so that the system has approximately a bandwidth of 10 rad/s,
% specify your target desired loop shape $G_{d}(s) = 10/s$.
% Then use |loopsyn|
% to find a loop-shaping controller for |G| that optimally
% matches the desired loop shape |Gd|.
s = zpk('s'); 
w0 = 10; 
Gd = w0/(s+.001);
[K,CL,GAM] = loopsyn(G,Gd); 
%%
% Examine the open-loop frequency response with the resulting controller, |K|.
sigma(G*K,'r',Gd,'k-.',Gd/GAM,'k:',Gd*GAM,'k:',{.1,30})
legend('Achieved Loop Shape','Target Loop Shape','Gd/GAM','Gd*GAM')
%%
% Examine the closed-loop response as well.
T = feedback(G*K,eye(2));
sigma(T,ss(GAM),'r*',{.1,30});
legend('Closed loop','GAM')
grid
%%
% The returned value |GAM| 
% is an indicator of the accuracy to which the optimal loop shape matches
% your desired loop shape.  |GAM| is an upper bound on the resonant peak
% magnitude of the closed-loop transfer function |T = feedback(G*K,eye(2))|.
% In this case, |GAM| = 1.6024 = 4 dB, as the singular
% value plots show. The plots also show that the achieved loop shape
% matches the desired target |Gd| to within about |GAM| dB.
%% References
% [1] Safonov, M.G., Laub, A.J., and Hartmann, G., "Feedback Properties of
% Multivariable Systems: The Role and Use of Return Difference Matrix,"
% _IEEE Trans. of Automat. Contr._, 1981, AC-26(1), pp. 47-65.



%% 
% Copyright 2012 The MathWorks, Inc.