%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This program implements the stable indirect controller
% for the aircraft wing rock example (model/problem used
% in the chapter, not the homework problem).
%
% Kevin Passino
% Version: 12/17/99
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize variables

clear

% Define plant parameters:

global a1 a2 a3 a4 a5 b tau beta0 beta1 thetam

a1=-0.0148927;
a2=0.0415424;
a3=0.01668756;
a4=-0.06578382;
a5=0.08578836;
b=1.5;
tau=1/15;
beta0=10;
beta1=40;
thetam=(beta1-beta0)/2;

% The number of parameters of the approximators

global pbeta R palpha

pbeta=1;  % Simply use a constant to estimate the beta term
R=9; % Number of rules
palpha=R*4; % 4 arises since there are 4 terms in the consequents (one is the affine term)

% Controller
	
n=3;  % Since in the plant we are computing y^(3) but really the state we 
                         % need is x=[x(1), x(2), x(3)]^T
% We will use a TS fuzzy system but with only x(1) and x(2) as inputs
% to the premise and all of x as an input to the consequent - with an affine term

global c

c=0*ones(n-1,1); % To force to be a column

%gr=-1:0.5:1;
gr=-2:2:2;

[Cx,Cy]=meshgrid(gr,gr);

l=0;

for i=1:length(gr)
      for j=1:length(gr)
			  l=l+1;
			  c(:,l)=[Cx(i,j); Cy(i,j)];
       end
end

global sigma

%sigma=0.5;
sigma=2;

% Next, parameters to compute the nu signal

global k1 k0 gamma

k1=20;
k0=100;

gamma=2;

% Sliding mode term

global Walpha Wbeta

Walpha=0.01;  % A guess
Wbeta=0;  % The most it could be off by ideally

% Define matrix for calculation of reference signals

global Am

Am=0*eye(4);

% Set adaptation gains

global etaalpha etabeta

etaalpha=2;
etabeta=2;

% Define simulation parameters:

Tfinal=2; % Units are seconds
Tspan=[0 Tfinal];

% Define initial conditions:

y0=[.4 0 0];   % For the plant
ym0=[0 0 0 0];  % For reference input
thetaalpha0=0.00*(-0.5*ones(1,palpha)+rand(1,palpha)); % Just pick small random values
thetabeta0=10;  % Simply use a constant for estimating beta

[t,z]=ode45('sindwingrockc',Tspan,[y0 ym0 thetaalpha0 thetabeta0]);
%[t,z]=ode15s('sindwingrock',Tspan,[y0 ym0 thetaalpha0 thetabeta0]);

y=z(:,1:3);   % The plant states, and the y^(3)
ym=z(:,4:7);  % The reference model signals
thetaalpha=z(:,length(y0)+length(ym0)+1:length(y0)+length(ym0)+palpha); % The parameters
thetabeta=z(:,length(y0)+length(ym0)+palpha+1);

% Plot the plant signals

figure(1)
clf
subplot(211)
plot(t,y(:,1),'k-',t,ym(:,1),'k--')
title('Roll angle y(t) (solid) and y_m(t) (dashed)')
subplot(212)
plot(t,y(:,3),'k-')
xlabel('Time, sec.')
ylabel('\delta_A')
title('Aileron input')
xlabel('Time (sec)')


figure(2)
clf
plot(t,y(:,2),'k-',t,ym(:,2),'k--')
title('Derivative of y and y_m')
xlabel('Time (sec)')


figure(3)
clf
plot(t,thetabeta)
title('\theta_\beta')
xlabel('Time (sec)')

figure(4)
clf
plot(t,thetaalpha)
title('\theta_\alpha components')
xlabel('Time (sec)')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%