% FlatEarthAnalysis 
% Script to analyse the aircraft system assuming flat earth.
% Version 7, 10/25/02. Perturbed inputs are now written to the workspace
% by the Simulink simulation and used as small pert. inputs
% in the linear simulation.
%
% This programn assumes that all constants are defined and
% that the Simulink simulation FlatEarth has been run.

disp(' ');disp('Start here');disp(' ');
disp(' We will assume that xIC and uIC contain the initial conditions')
disp(' for states and controls. We will use there initial conditions')
disp(' as the reference condtioons around which to linearize ')
disp(' the nonlinear equations of motion.')
%
% Establish reference conditions for linearization 
xR=xIC;   xR'
uR=uIC;   uR'
% Get yR
flag=3; t=0;
[yR,x0,str,ts] = aircraft7(t,xR,uR,flag,constant,xR);

% Linearize the Simulink model about the reference conditions.
disp('Linearize the Simulink model about the reference conditions.')
[a,b,c,d]=linmod('FlatEarth7b',xR,uR) %Find linear model of nonlinear pendulum
disp('Natural frequencies and damping ratios of the linearized system')
[Wn,Z]=damp(a) % Find properties of the poles of linearized system
disp('Poles (eigenvalues) of the linearized system')
poles=eig(a)
PSYS=ss(a,b,c,d); %Create a linear state space system
set(PSYS,'statename',{'u(f/s)' 'v(f/s)' 'w(f/s)' 'p(r/s)' 'q(r/s)' 'r(r/s)' ...
        'phi(r)' 'thet(r)' 'psi(r)' 'x(f)' 'y(f)' 'h(f)'})
set(PSYS,'inputname',{'deltaE(r)' 'deltaA(r)' 'deltaR(r)' 'Bhp(hp)' 'vwx(f/s)' 'vwy(f/s)' 'vwz(f/s)'})
set(PSYS,'outputname',{'u(f/s)' 'v(f/s)' 'w(f/s)' 'p(r/s)' 'q(r/s)' 'r(r/s)' ...
        'phi(r)' 'thet(r)' 'psi(r)' 'x(f)' 'y(f)' 'h(f)' ...
        'Vt(f/s)' 'alf(r)' 'beta(r)' 'gam(r)' 'xdot(f/s)' 'ydot(f/s)' 'zdot(f/s)'})
PSYS
disp(' Execution is paused. Hit any key to continue')
pause
%
% Set up for the time history generation (overplots of nonlinear
% and linear simulations.
disp(' ')
disp('Small Perturbation Initial condition for linear simulation.')
disp('Note that this is xIC-xR, where xIC=X(0) is the same I.C. as')
disp('used in the nonlinear simulation.')
dx0=xIC-xR; %Use this small perturbation initial condition for linear simulation
input='Time response comparison. Flat Earth Model';
timevector=taircraft;
tstart=timevector(1)
tstop=timevector(length(timevector))
% Below is the vector of uniformly spaced time points for the linear simulation
t=tstart:.02:tstop; 
u=zeros(length(t),4);
% Form the control time histories as used in the nonlinear simulation but
% interpolated to a set of uniform times.
%YI = INTERP1(X,Y,XI)
u(:,1)=interp1(timevector,deltaE,t');
u(:,2)=interp1(timevector,deltaA,t');
u(:,3)=interp1(timevector,deltaR,t');
u(:,4)=interp1(timevector,deltabhp,t');
u(:,5)=zeros(size(u(:,4)));
u(:,6)=u(:,5);
u(:,7)=u(:,5);

% Form the small perturbation control time histories
du=zeros(size(u));
du(:,1)=u(:,1)-uIC(1);
du(:,2)=u(:,2)-uIC(2);
du(:,3)=u(:,3)-uIC(3);
du(:,4)=u(:,4)-uIC(4);
du(:,5)=u(:,5)-uIC(5);

% Use lsim to generate the linear simulation using
% the small perturbation initial condition and the 
% small perturbation control vector.
[y,t]=lsim(PSYS,du,t,dx0); %Do linear simulation
%Plot the linear results and the nonlinear results
%  which have been stored in tnlsim and ynlsim.
figure(1) 
disp('Take care to add the reference state xR or yR to the small perturbation')
disp('solution in order to get the total approximate solution.')
disp('Only the total approximate solution should be compared to the ')
disp('results from the nonlinear simulation.')

figure(1)
subplot(311)
plot(t,y(:,5)+yR(5),'-',timevector,yaircraft(:,5),':')
xlabel('time (sec)')
ylabel('Q (rad/sec)')
legend('linear sim','nonlinear sim')
title(input)

subplot(312)
plot(t,y(:,8)+yR(8),'-',timevector,yaircraft(:,8),':')
xlabel('time (sec)')
ylabel('theta (rad)')
legend('linear sim','nonlinear sim')

subplot(313)
plot(t,y(:,14)+yR(14),'-',timevector,yaircraft(:,14),':')
xlabel('time (sec)')
ylabel('alpha (rad)')
legend('linear sim','nonlinear sim')

figure(2)
subplot(311)
plot(t,y(:,6)+yR(6),'-',timevector,yaircraft(:,6),':')
xlabel('time (sec)')
ylabel('R (rad/sec)')
legend('linear sim','nonlinear sim')
title(input)

subplot(312)
plot(t,y(:,9)+yR(9),'-',timevector,yaircraft(:,9),':')
xlabel('time (sec)')
ylabel('psi (rad)')
legend('linear sim','nonlinear sim')

subplot(313)
plot(t,y(:,15)+yR(15),'-',timevector,yaircraft(:,15),':')
xlabel('time (sec)')
ylabel('beta (rad)')
legend('linear sim','nonlinear sim')

figure(3)
subplot(311)
plot(t,y(:,4)+yR(4),'-',timevector,yaircraft(:,4),':')
xlabel('time (sec)')
ylabel('P (rad/sec)')
legend('linear sim','nonlinear sim')
title(input)

subplot(312)
plot(t,y(:,7)+yR(7),'-',timevector,yaircraft(:,7),':')
xlabel('time (sec)')
ylabel('phi (rad)')
legend('linear sim','nonlinear sim')

subplot(313)
plot(t,y(:,11)+yR(11),'-',timevector,yaircraft(:,11),':')
xlabel('time (sec)')
ylabel('Y (ft)')
legend('linear sim','nonlinear sim')


disp(' Execution is paused. Hit any key to continue')
pause
%%%%%%
% The state vector of the nonlinear and linear systems are defined on the
% Simulink model, FlatEarth. This model contains both lateral-directional 
% variables and longitudinal variables.
% States
%   x=[U,V,W,P,Q,R,Phi,Theta,Psi,Xprime,Yprime,Hprime]
%   Units are [ft/sec, ft/sec, ft/sec, rad/sec, rad/sec, rad/sec, rad, rad, rad, ft, ft, ft]
% Inputs
%   u=[deltaE,deltaA,deltaR,bhp, Vwx, Vwy, Vwz]]
%   Units are [rad,rad,rad,bhp, ft/sec, ft/sec, ft/sec]
%   Vwx, Vwy, Vwz are components of the wind vector in NED frame
% Outputs (19)
%   y=[U,V,W,P,Q,R,Phi,Theta,Psi,Xprime,Yprime,Hprime,Vt,alpha,beta,gamma,Xdot,Ydot,Hdot]
%   Xdot,Ydot,Hdot are components of the inertial velocity vector in the NED frame.
%   Units are [ft/sec, ft/sec, ft/sec, rad/sec, rad/sec, rad/sec, 
%   rad, rad, rad, ft, ft, ft, ft/sec, rad, rad, rad, ft/sec, ft/sec, ft/sec]
% The code below will separate the lateral-directional states and 
% controls from the complete stae space model given by a, b, c, and d.
% Select lateral-directional subsystem.
disp(' '); disp('Lateral-Directional Subsystem'); disp(' ')
xselect=[2,4,6,7,9]; uselect=[2,3]; yselect=[2,4,6,7,9,15];
[aLD,bLD,cLD,dLD]=selector(a,b,c,d,xselect,uselect,yselect)
save modelLat aLD bLD cLD dLD Vt Hp aircraft % Save subsystem linear model
LDsys=ss(aLD,bLD,cLD,dLD);
set(LDsys,'statename',{'v(f/s)' 'p(r/s)' 'r(r/s)' 'phi(r)' 'psi(r)'})
set(LDsys,'inputname',{'deltaA(r)' 'deltaR(r)'})
set(LDsys,'outputname',{'v(f/s)' 'p(r/s)' 'r(r/s)' 'phi(r)' 'psi(r)' 'beta(r)'})
LDsys
[Wn,Z,Poles]=damp(LDsys)
LDtfsys=tf(LDsys)
LDzpksys=zpk(LDsys)

disp(' Execution is paused. Hit any key to continue')
pause
%%%%%%
% The state vector of the nonlinear and linear systems are defined on the
% Simulink model, FlatEarth. This model contains both lateral-directional 
% variables and longitudinal variables.
% The code below will separate the longitudinal states and 
% controls from the complete stae space model given by a, b, c, and d.
% Select longitudinal subsystem.
disp(' '); disp('Longitudinal Subsystem'); disp(' ')
xselect=[1,3,5,8,12]; uselect=[1,4]; yselect=[1,3,5,8,12,13,14,16];
[aL,bL,cL,dL]=selector(a,b,c,d,xselect,uselect,yselect)
%    SAVE fname X Y Z  -ASCII  uses 8-digit ASCII form instead of binary.
save modelLong aL bL cL dL Vt Hp aircraft % Save subsystem linear model
Lsys=ss(aL,bL,cL,dL);
set(Lsys,'statename',{'u(f/s)' 'w(f/s)' 'q(r/s)' 'thet(r)' 'h(f)'})
set(Lsys,'inputname',{'deltaE(r)' 'Bhp(hp)'})
set(Lsys,'outputname',{'u(f/s)' 'w(f/s)' 'q(r/s)' 'thet(r)' 'h(f)' 'Vt(f/s)' 'alf(r)' 'gam(r)'})
Lsys
[Wn,Z,Poles]=damp(Lsys)
Ltfsys=tf(Lsys)
Lzpksys=zpk(Lsys)




