Aircraft Controls Final Exam

Drexel University
 Part A: Controller Design

In Order to design a state-feedback controller, we first had to concatenate the aircraft plant and actuator
dynamics into a single plant. We wrote the actuator dynamics, given to us in Laplace space, in state-space
representations such that the output matrix C was the identity. This ensured that the states relating the
actuators represented the actual actuator positions. This is the concatenated system:

Af = [E^-1*A E^-1*B;zeros(2,4) [-10 0;0 -.2]];

Bf = [zeros(4,2);[10 0;0 .2]];
Cf = [rtod*[-1 0 0 1 0 0];0 0 1 0 0 0]; %monitored outputs = [climb angle;vt]
Mf = [0 0 0 rtod 0 0;0 0 1 0 0 0]; %measured outputs = [theta, vt]
Df = zeros(size(Cf,1),2);

The six states in order are angle of attack, pitch rate, velocity, pitch, elevator deflection, and throttle
setting. All angular states are stored in radians. The Cf and Mf matrices contain the transformations into
degrees, so the system outputs are in degrees rather than radians.

Our group designed two separate controllers, one for climb and one for cruise flight.

For the first, we set

Q = diag([10 1]); r = diag([1000 1]); rho = 30;

The first element in Q weights climb angle and the second weights velocity. The first element in r weights
elevator control and the second weights throttle. Rho is relatively high because the plane will vibrate on
its short-period frequencies for low rho values. The high rho slows the response of the system and raises
the damping ratio. We calculated feedback gain as follows:

F1 = -lqr(Af,Bf,Cf'*Q*Cf,rho^2*r);

Observer poles were simply set to twice the regulator poles.

H1 = -place(Af',Mf',eRegulator*2)';

We then built the closed-loop system as such:

Ac1 = [Af Bf*F1;-H1*Mf Af+H1*Mf+Bf*F1];

Cc = [rtod*[-1 0 0 1 0 0 0 0 0 0 0 0];
[0 0 1 0 0 0 0 0 0 0 0 0]];

Where the first output is climb angle, in degrees, and the second is velocity. This system drives the plane
to equilibrium flight, however, and we needed to turn it into a setpoint controller by adding a setpoint
filter placed between the setpoint and input to the system. To solve for this filter, we set the time
derivative of the system to zero and solved for the input filter, as shown below. x’ represents the
estimated states, r represents the input signal, and P represents the filter. The system is solved by saying r
and y are equal at steady-state.
So, we set the setpoint filter to
Pi = -(Cc(1,:)*Ac1^-1*[Bf([1:end 1:end],1)])^-1;

And Bc1 to

Bc1 = [Bf([1:end 1:end],1)]*Pi;

For the climb phase, only the first column of Bf and the first row of Cc are used, because the exam
specifies that the setpoint controller for the climb phase is only controlling climb angle and is only
commanding the elevator. For the level cruise phase, both rows of Cc and both columns of Bf are used.

The block diagram for the closed-loop systems is below. The climb phase system has a slightly different
form for its block diagram because the input r is a scalar; however, the general appearance of the block
diagram still holds.
The eigenvalues of the closed-loop system for the climb phase are

-20.0000
-10.0000
-0.8758 + 2.2356i
-0.8758 - 2.2356i
-0.4379 + 1.1178i
-0.4379 - 1.1178i
-0.3484 + 0.2862i
-0.3484 - 0.2862i
-0.1742 + 0.1431i
-0.1742 - 0.1431i
-0.0986
-0.1971

At first, our group mistakenly took this to mean that the LQR controller should not control throttle either.
We set up that system and got some interesting results. The aircraft reached a steady state climb angle, but
also maintained a nonzero pitch rate such that the plane was continuously pitching up and slowing down
to maintain the climb angle. When the controller switched at 2000 feet, the plane rolled forward and
picked up speed, but actually loses 1500 feet of altitude before it settles to equilibrium. The graphs are
included at the end of this report simply because we find them interesting.

The controller for level, cruise flight is very similar to the one above, but with different values for r and
Q.

Q = diag([10 1]);r = diag([1000 10]);rho = 30;

Elevator deflection is weighted more heavily here, as is climb angle.

Since the setpoint controller must control both climb angle and velocity while commanding both elevator
and throttle, the setpoint filter must be a 2x2 matrix:

Pi = -(Cc*Ac2^-1*[Bf;Bf])^-1;

And we may use both inputs, so

Bc2 = [Bf;Bf]*Pi;
Again, observer poles are simply placed at twice the regulator poles. The poles of the closed loop system
with the second controller are

-20.0000
-10.0000
-0.8758 + 2.2355i
-0.8758 - 2.2355i
-0.4379 + 1.1178i
-0.4379 - 1.1178i
-0.3743
-0.1590 + 0.1916i
-0.1590 - 0.1916i
-0.0795 + 0.0958i
-0.0795 - 0.0958i
-0.1872

To model these systems separately, one may simply use lsim(), but we could not do this for two reasons:
First, we must model height and Northern (we assume the plane is going North) velocities, but we cannot
accurately linearize those differential equations; second, the controllers must switch when the plane
reaches 2000ft, and a linear system cannot contain background logic to do this.

Instead, we wrote a differential function for use with ode45() or a similar solver. State-space systems are
passed to this function via global variables, and the state vector is appended with altitude and North
displacement. This way, we model both the linearized state-space system and the non-linear flight path
system.

To switch between the two controllers, a simple persistent Boolean variable is set when the plane hits
2000ft above the altitude from which it started. An if() statement separates the two closed-loop systems
with the persistent Boolean as its argument.
Part B: Evaluation (nominal)

a. Response of all the primary airplane variables: alpha, pitch rate, velocity and pitch are
plotted and shown in Figure -1. First plots are angle of attack and pitch rate while the
third one is velocity. Plane has velocity of 500 ft/s at start and slows down to 499.4 ft/s
during the angle change. After it reaches the desired altitude the velocity decreases to
498.5 ft/s for leveling off and comes back to the velocity of 500 ft/s. Pitch rate has spikes
at start as the angle of attack goes up to 1o and another spike at 238 s as the angle is
changing from 1o back to zero. Pitch response shows the same behavior as expected.

Alpha Response
5.7
Alpha (deg)
5.65

5.6

5.55

5.5

5.45

5.4

5.35

5.3

5.25

5.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Pitch Rate Response

0.3
q (deg/s)
0.2

0.1

-0.1

-0.2

-0.3

-0.4
0 50 100 150 200 250 300 350 400 450 500
Time (s)
 Velocity Response
500.5
Vt (ft/s)
500

499.5

499

498.5

498

497.5

497

496.5

496
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Pitch Response
1.2
theta (deg)
1.1

0.9

0.8

0.7

0.6

0.5
0 50 100 150 200 250
Time (s)

Figure -1 Plots of primary airplane variables

b. Plots of climb angle response, elevator repose, throttle response and flight path are shown
in figure 2. These demonstrations are further analyzed to see if the system has reached the
settling time specifications. Climb angle is the command given by the team with the
initial condition of 0 degree angle. The climb angle is zero at start and reaches desired 1o
in 84 seconds and the angle is maintained until the plane reaches the altitude of 32070 ft.
Overall, the auto-pilot takes 238 seconds to come from 30000 ft to 32000 ft.
Second graph shows that there is a spike in the elevator deflection at 238 seconds which
is caused when leveling off from 1o climb angle. This takes approximately 38 seconds
while the overall leveling process takes 50 seconds. It should be also noted that the there
is an inverse relation between the climb angle and the elevator deflection therefore when
the elevator deflection is negative, climb angle is positive.

Last graph can be analyzed to observe the settling time in terms of displacement. 126700
feet of north displacement is needed to reach to the desired altitude and 25100 ft more to
go back to the level-off position.
 Climb Angle Response
1.2
gamma (deg)
1

0.8

0.6

0.4

0.2

-0.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Elevator Response
-4.092

-4.094

-4.096
Elevator Deflection (deg)

-4.098

-4.1

-4.102

-4.104

-4.106

-4.108
0 50 100 150 200 250 300 350 400 450 500
Time (s)
 Throttle Response
0.29

0.28

0.27

0.26
Throttle

0.25

0.24

0.23

0.22

0.21

0.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

4
x 10 Flight Path
3.25

3.2

3.15
Altitude (ft)

3.1

3.05

3
0 0.5 1 1.5 2 2.5
North Displacement (ft) 5
x 10

Figure – 2 Airplane response I

c. Response plots for at least two different values of ρ indicating the improvement in
performance at the expense of larger “actuator-activity”
1. There are two rho values for each controller as one controller is for one degree flight
and the other is for level cruise flight. The values of rho are 30 for each controller.
First, both values of rho indicated were decreased by 30% and the following graphs
were obtained. Plane is expected to vibrate on its short-period frequencies for lower
rho values. Alpha and pitch rate responses oscillate more when leveling off from 1o
climb angle. In addition, the plane does not slow down as much during this change.
Overall, the response of the system is increases but this drives system to be less
stable. Graph – 4 shows that spike up from -4.095 to -4.096 in elevator deflection
whereas there are no significant changes in other variables.

Alpha Response
5.9
Alpha (deg)
5.8

5.7

5.6

5.5

5.4

5.3

5.2

5.1
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Pitch Rate Response

0.3
q (deg/s)
0.2

0.1

-0.1

-0.2

-0.3

-0.4
0 50 100 150 200 250 300 350 400 450 500
Time (s)
 Velocity Response
500.5
Vt (ft/s)
500

499.5

499

498.5

498

497.5

497
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Pitch Response
1.2
theta (deg)
1

0.8

0.6

0.4

0.2

-0.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Figure -4 Responses with 30% decrease in rho

 Climb Angle Response
1.2
gamma (deg)
1

0.8

0.6

0.4

0.2

-0.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Elevator Response
-4.09

-4.092

-4.094
Elevator Deflection (deg)

-4.096

-4.098

-4.1

-4.102

-4.104

-4.106

-4.108
0 50 100 150 200 250 300 350 400 450 500
Time (s)
 Throttle Response
0.29

0.28

0.27

0.26
Throttle

0.25

0.24

0.23

0.22

0.21

0.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

4
x 10 Flight Path
3.25

3.2

3.15
Altitude (ft)

3.1

3.05

3
0 0.5 1 1.5 2 2.5
North Displacement (ft) 5
x 10

Figure – 5 Response with 30% decrease in rho

2. The values of rho are 30 for each controller. Secondly, both values of rho indicated
were increased by 50% and the following graphs were obtained. The system
response is expected to slow down while the damping ratio is expected to rise. The
same expected results are illustrated in figures 6 & 7. Alpha and pitch rate response
have small spikes while having more oscillations as a result of increase in damping
ratio. The plane needs less velocity to overcome the same angle change but takes
slightly longer time to settle down because of the oscillations.

Alpha Response
5.75
Alpha (deg)
5.7

5.65

5.6

5.55

5.5

5.45

5.4

5.35

5.3

5.25
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Pitch Rate Response

0.3
q (deg/s)
0.2

0.1

-0.1

-0.2

-0.3

-0.4
0 50 100 150 200 250 300 350 400 450 500
Time (s)
 Velocity Response
501
Vt (ft/s)
500

499

498

497

496

495

494
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Pitch Response
1.2
theta (deg)
1

0.8

0.6

0.4

0.2

-0.2

-0.4
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Figure – 6 Response with 50% increase in rho

 Climb Angle Response
1
gamma (deg)
0.8

0.6

0.4

0.2

-0.2

-0.4
0 50 100 150 200 250 300 350 400 450 500
Time (s)

Elevator Response
-4.096

-4.098
Elevator Deflection (deg)

-4.1

-4.102

-4.104

-4.106

-4.108
0 50 100 150 200 250 300 350 400 450 500
Time (s)
 Throttle Response
0.29

0.28

0.27

0.26
Throttle

0.25

0.24

0.23

0.22

0.21

0.2
0 50 100 150 200 250 300 350 400 450 500
Time (s)

4
x 10 Flight Path
3.25

3.2

3.15
Altitude (ft)

3.1

3.05

3
0 0.5 1 1.5 2 2.5
North Displacement (ft) 5
x 10

Figure – 7 Response with 50% increase in rho

d. Demonstration of meeting the objectives:

All required plots are above.

Part C: Evaluation (robustness)

Part a.
By modifying the MyTrim.m script slightly (changing altitude by 2000ft) and naming the script
FinalExamRobustness, we were able to determine what the trim conditions should during the level flight
phase.

Original System Evaluation Model

Elevator (deg) -4.1013 -4.8991
Throttle .2038 .2055
Alpha (deg) 5.4333 6.0293

Part b.
From FinalExamRobustness, we also get the linearized system for the evaluation. Using this model, we
replaced our design model and with the evaluation model. The eigenvalues of both closed loop systems is
shown below.

Orignal System Evaluation Model

-20.0000 -20.0000
-10.0000 -10.0000
-0.8758 + 2.2356i -0.8755 + 2.2370i
-0.8758 - 2.2356i -0.8755 - 2.2370i
-0.4379 + 1.1178i -0.4047 + 1.0765i
-0.4379 - 1.1178i -0.4047 - 1.0765i
-0.3484 + 0.2862i -0.3501 + 0.2834i
-0.3484 - 0.2862i -0.3501 - 0.2834i
-0.1742 + 0.1431i -0.1776 + 0.1414i
-0.1742 - 0.1431i -0.1776 - 0.1414i
-0.0986 -0.0847
-0.1971 -0.2047

We can see that the poles have shifted slightly, but overall, the evaluation model is stable and
approximately equal to the design model.
Appendix 1: Code

Main script code

%% MEM 530 Midterm Exam 2
% clear all

%% Problem A
% Plane Properties
rtod = 57.29578; %was used to convert radians to degrees
s = 2170;
cbar = 17.5;
mass = 5.0e3;
Iyy = 4.1e6;
tstat = 6e4;
dtdv = -38.0;
ze = 2;
cdcls = .042;
cla = .085*rtod;
cma = -.022*rtod;
cmde =-.016*rtod;
cmq = -16.0;
cmadot = -6;
cladot = 0;
cl0 = .2;
cd0 = .016;
cm0 = .05;
dcdg = 0;
dcmg = 0;
clq = 0;
clv = 0;
cmv = 0;
cdv = 0;
clde = 0;
cdde = 0;

gd = 32.17;
xcg = .25;

%%
%team specific properties
h = 30000;
vt = 500;
gamma = 0*pi/180;
[m, qbar] = adc(vt,h);
qs = qbar*s;

%%
%Equilibrium flight conditions
[deltaee, deltate, alphae] = MyTrim(vt, h, gamma);
cle = cl0 + cla*alphae;
cme = dcmg + cm0 + cma*alphae + cmde*deltaee + cle*(xcg-.25);
cde = dcdg + cd0 + cdcls*cle*cle;
weight = mass * gd;
lift = qs*cle;
drag = qs*cde;
thrust = (drag+weight*sin(gamma))/cos(alphae);

global vte
vte = vt;
%%
% Residual of steady-state flight
res = thrust*sin(alphae)+lift-weight*cos(gamma); %% for debug
%%
%constants for state space construction
Zalphadot = qs*cbar/(2*mass*vt)*cladot;
Malphadot = qs*cbar^2/(Iyy*2*vt)*cmadot;
Zalpha = -qs/mass*(cde + cla);
Zq = qs*cbar/(2*mass*vt)*clq;
Zv = -qs/(mass*vt)*(2*cle+clv);
Zde = -qs/mass*clde;
Xtv = 1/mass*dtdv*deltate;
Xde = -qs/mass*cdde;
Malpha = qs*cbar/Iyy*cma;
Mtalpha = 0;
Mq = qs*cbar^2/(Iyy*2*vt)*cmq;
Mv = qs*cbar/(Iyy*vt)*(2*cme+cmv);
Mtv = 1/Iyy*dtdv*deltate;
Mde = qs*cbar/Iyy*cmde;

Mdt = 1/Iyy*ze*(tstat+dtdv*vt);
Xdt = 1/mass*(tstat+dtdv*vt);

cdalpha = 2*cdcls*cle*cla;
Xalpha = qs/mass*(cle-cdalpha);
Xv = -qs/(mass*vt)*(2*cde+cdv);

%%
%state space representation
E = [vt - Zalphadot, 0, 0, 0; -Malphadot 1 0 0; 0 0 1 0; 0 0 0 1];
A = [Zalpha, vt + Zq, Zv - Xtv*sin(alphae), -gd*sin(gamma); Malpha + ...
Mtalpha, Mq, Mv + Mtv, 0; Xalpha, 0, Xv + Xtv*cos(alphae), ...
-gd*cos(gamma); 0 1 0 0];
B = [Zde -Xdt*sin(alphae);Mde Mdt;Xde Xdt*cos(alphae);0 0];
C = eye(4)*rtod;
C(3,3)=1;

g = ss(E^-1*A,E^-1*B,C,zeros(4,2));

%%
% Eignevectors, values
[v e] = eig(E^-1*A)

%%
% Periods, damping ratios
% [omega d] = damp(diag(e));
% fprintf('\n\nFrequency\tPeriod\t\tdamping ratio\n');
% fprintf('%1.4f\t\t%1.4f\t\t%1.4f\n',[omega 2*pi./omega d]');

%% Part A

%% I. Full State Feedback Design

%%
% Augment system with actuator dynamics

% x=[alpha;q;vt;theta;de;dt]
Af = [E^-1*A E^-1*B;zeros(2,4) [-10 0;0 -.2]];
Bf = [zeros(4,2);[10 0;0 .2]];
Cf = [rtod*[-1 0 0 1 0 0];0 0 1 0 0 0]; %monitored outputs = [climb angle;vt]
Mf = [0 0 0 rtod 0 0;0 0 1 0 0 0]; %measured outputs = [theta, vt]
Df = zeros(size(Cf,1),2);

global Ac1 Ac2 Cc Bc1 Bc2 U

%% Build separate controllers

% Out = [gamma;vt]
Cc = [rtod*[-1 0 0 1 0 0 0 0 0 0 0 0];
[0 0 1 0 0 0 0 0 0 0 0 0]];

%% For 1 Degree of Climb

% Find LQR feedback gain
Q = diag([10 1]);
r = diag([1000 1]);
rho = 30;
F1 = -lqr(Af,Bf,Cf'*Q*Cf,rho^2*r);

eRegulator = eig(Af+Bf*F1)

%%
% Place observer poles
H1 = -place(Af',Mf',eRegulator*2)';

eObserver = eig(Af+H1*Mf)

%%
% Build closed-loop system

Ac1 = [Af Bf*F1;-H1*Mf Af+H1*Mf+Bf*F1];

Pi = -(Cc(1,:)*Ac1^-1*[Bf([1:end 1:end],1)])^-1;
Bc1 = [Bf([1:end 1:end],1)]*Pi;

eClosedLoop1 = eig(Ac1)

%% For Level Cruise Flight

% Find LQR feedback gain
Q = diag([10 1]);
r = diag([1000 10]);
rho = 30;

F2 = -lqr(Af,Bf,Cf'*Q*Cf,rho^2*r);

eRegulator = eig(Af+Bf*F2)

%%
% Place observer poles
H2 = -place(Af',Mf',eRegulator*2)';

eObserver = eig(Af+H2*Mf)

%%
% Build closed-loop system
Ac2 = [Af Bf*F2;-H2*Mf Af+H2*Mf+Bf*F2];

Pi = -(Cc*Ac2^-1*[Bf;Bf])^-1;
Bc2 = [Bf;Bf]*Pi;

eClosedLoop2 = eig(Ac2)

%% Simulation

t = 0:.01:500;

% zeroU = [zeros(1,length(t))+2;zeros(1,length(t))+1]; %we want 1 degree of

% climb

% system is linearized around steady-state deltas, so input is 0

% [outC] = lsim(gc,zeroU,t,.0*[alphae;0;vt;gamma;0;0;0;0;0;0;0;0]);

clear modelDiff

U = [1;2];
% [t X] = ode45(@modelDiff,[t(1) t(end)],[.1*alphae;0;.1*vt;0;0;0;0;0;0;0;0;0;h;0]);
[t X] = ode45(@modelDiff,[t(1) t(end)],[zeros(12,1);h;0]);
outC = (Cc*X(:,1:12)')';

% X = [alpha,q,vt,theta,de,dt,
% alphaHat,qHat,vtHat,thetaHat,deHat,dtHat,
% altitude,north displacement]

%%
% Primary airplane variable responses
figure(1);clf
subplot(211)
plot(t,X(:,[1])*rtod+alphae*rtod);
legend 'Alpha (deg)'
xlabel 'Time (s)'
title 'Alpha Response'

subplot(212);
plot(t,X(:,[2])*rtod);
legend 'q (deg/s)'
xlabel 'Time (s)'
title 'Pitch Rate Response'

figure(2)
subplot(211);
plot(t,X(:,[3]) + vte);
legend 'Vt (ft/s)'
xlabel 'Time (s)'
title 'Velocity Response'

subplot(212);
plot(t,X(:,[4])*rtod+gamma*rtod);
legend 'theta (deg)'
xlabel 'Time (s)'
title 'Pitch Response'

%%
% Meeting-the-Objectives plots
figure(3);clf
subplot(211);
plot(t,outC(:,[1]));
legend 'gamma (deg)'
xlabel 'Time (s)'
title 'Climb Angle Response'

subplot(212);
plot(t,X(:,5)+deltaee*rtod);
ylabel 'Elevator Deflection (deg)'
xlabel 'Time (s)'
title 'Elevator Response'

figure(4)
subplot(211);
plot(t,X(:,6)+deltate);
ylabel 'Throttle'
xlabel 'Time (s)'
title 'Throttle Response'

subplot(212);
plot(X(:,14),X(:,13));
ylabel 'Altitude (ft)'
xlabel 'North Displacement (ft)'
title 'Flight Path'

%%
% Plot states to show observer convergence
figure(5)
plot(t,X(:,1:12))
legend 'alpha (rad)' 'q (rad/s)' 'Vt (ft/s)' 'theta (rad)' 'Elevator (rad)' 'throttle'
axis([0 60 -.15 .1])

figure(5)
plot(t,X(:,1:12))
legend 'alpha (rad)' 'q (rad/s)' 'Vt (ft/s)' 'theta (rad)' 'Elevator (rad)' 'throttle'
axis([230 350 -.2 .1])

%%
[eRobust deltaeEval deltatEval alphaEval] = FinalExamRobustness(Af,Bf,Mf,H2,F2);
fprintf('\n\n\n\nTrim Values:\n');
fprintf('\t\t\tOrignal System\t\tEvaluation Model\n')
fprintf('Elevator (deg):\t%1.4f\t\t\t%1.4f\n',deltaee*rtod,rtod*deltaeEval);
fprintf('Throttle: \t%1.4f\t\t\t%1.4f\n',deltate,deltatEval);
fprintf('Alpha (deg): \t%1.4f\t\t\t%1.4f\n',alphae*rtod,rtod*alphaEval);

fprintf('\n\nClosed-loop poles:\n')
fprintf('Orignal System\t\tEvaluation Model')
[eClosedLoop2 eRobust]
Function acd
function [amach,qbar] = adc(vt,alt)

r0 = 2.377e-3;

tfac = 1-0.703e-5*alt;
t = 519*tfac;

if(alt >= 35000)

t=390;
end

rho=r0*tfac^4.14;
amach=vt/sqrt(1.4*1716.3*t);

qbar=.5*rho*vt^2;

end

Function cost
function f = cost(s)
global x u gamma h u2
u(1) = s(1);
u(2) = s(2);
x(2) = s(3);
x(3) = x(2) + gamma;
time = 0;
[xd] = transp(time, x, u, h, u2);
f = xd(1)^2 + 100*xd(2)^2 + 10*xd(4)^2;
end

Function FinalExamRobustness
%% MEM 530 Final Exam, part C
function [eRobust deltaeEval deltatEval alphaEval] = FinalExamRobustness(Af,Bf,Mf,H,F)

%% Problem A
% Plane Properties
rtod = 57.29578; %was used to convert radians to degrees
s = 2170;
cbar = 17.5;
mass = 5.0e3;
Iyy = 4.1e6;
tstat = 6e4;
dtdv = -38.0;
ze = 2;
cdcls = .042;
cla = .085*rtod;
cma = -.022*rtod;
cmde =-.016*rtod;
cmq = -16.0;
cmadot = -6;
cladot = 0;
cl0 = .2;
cd0 = .016;
cm0 = .05;
dcdg = 0;
dcmg = 0;
clq = 0;
clv = 0;
cmv = 0;
cdv = 0;
clde = 0;
cdde = 0;

gd = 32.17;
xcg = .25;

%%
%team specific properties
h = 32000;
vt = 500;
gamma = 1*pi/180;
[m, qbar] = adc(vt,h);
qs = qbar*s;

%%
%Equilibrium flight conditions
[deltaee, deltate, alphae] = MyTrim(vt, h, gamma);
cle = cl0 + cla*alphae;
cme = dcmg + cm0 + cma*alphae + cmde*deltaee + cle*(xcg-.25);
cde = dcdg + cd0 + cdcls*cle*cle;
weight = mass * gd;
lift = qs*cle;
drag = qs*cde;
thrust = (drag+weight*sin(gamma))/cos(alphae);

global vte
vte = vt;
%%
% Residual of steady-state flight
res = thrust*sin(alphae)+lift-weight*cos(gamma); %% for debug
%%
%constants for state space construction
Zalphadot = qs*cbar/(2*mass*vt)*cladot;
Malphadot = qs*cbar^2/(Iyy*2*vt)*cmadot;
Zalpha = -qs/mass*(cde + cla);
Zq = qs*cbar/(2*mass*vt)*clq;
Zv = -qs/(mass*vt)*(2*cle+clv);
Zde = -qs/mass*clde;
Xtv = 1/mass*dtdv*deltate;
Xde = -qs/mass*cdde;
Malpha = qs*cbar/Iyy*cma;
Mtalpha = 0;
Mq = qs*cbar^2/(Iyy*2*vt)*cmq;
Mv = qs*cbar/(Iyy*vt)*(2*cme+cmv);
Mtv = 1/Iyy*dtdv*deltate;
Mde = qs*cbar/Iyy*cmde;

Mdt = 1/Iyy*ze*(tstat+dtdv*vt);
Xdt = 1/mass*(tstat+dtdv*vt);

cdalpha = 2*cdcls*cle*cla;
Xalpha = qs/mass*(cle-cdalpha);
Xv = -qs/(mass*vt)*(2*cde+cdv);

%%
%state space representation
E = [vt - Zalphadot, 0, 0, 0; -Malphadot 1 0 0; 0 0 1 0; 0 0 0 1];
A = [Zalpha, vt + Zq, Zv - Xtv*sin(alphae), -gd*sin(gamma); Malpha + ...
Mtalpha, Mq, Mv + Mtv, 0; Xalpha, 0, Xv + Xtv*cos(alphae), ...
-gd*cos(gamma); 0 1 0 0];
B = [Zde -Xdt*sin(alphae);Mde Mdt;Xde Xdt*cos(alphae);0 0];
C = eye(4)*rtod;
C(3,3)=1;

g = ss(E^-1*A,E^-1*B,C,zeros(4,2));

%%
% Eignevectors, values
[v e] = eig(E^-1*A);

%%
% Periods, damping ratios
[omega d] = damp(diag(e));
fprintf('\n\nFrequency\tPeriod\t\tdamping ratio\n');
fprintf('%1.4f\t\t%1.4f\t\t%1.4f\n',[omega 2*pi./omega d]');

%% Part A

%% I. Full State Feedback Design

%%
% Augment system with actuator dynamics

% x=[alpha;q;vt;theta;de;dt]
Ar = [E^-1*A E^-1*B;zeros(2,4) [-10 0;0 -.2]];
Br = [zeros(4,2);[10 0;0 .2]];
Cr = [rtod*[-1 0 0 1 0 0];0 0 1 0 0 0]; %monitored outputs = [climb angle;vt]
Mr = [0 0 0 rtod 0 0;0 0 1 0 0 0]; %measured outputs = [theta, vt]
Dr = zeros(size(Cr,1),2);

%% controller variables from linearized system in FinalExam2.m

%%
% Build close-loop system

Acr = [Ar Br*F;-H*Mr Af+H*Mf+Bf*F];

%% Evaluatiopn of robustness

eRobust = eig(Acr);
deltaeEval = deltaee;
deltatEval = deltate;
alphaEval = alphae;

end

Function ModelDiff
function [dx] = modelDiff(t,x)

global Ac1 Ac2 Bc1 Bc2 U vte

persistent h0 hHit

if(isempty(h0))
h0 = x(13,1);
hHit = false;
end

% hHit = false;
hHit = hHit || x(13,1) - h0 > 2000;

% x = [alpha;q;vt;theta;de;dt;
% alphaHat;qHat;vtHat;thetaHat;deHat;dtHat;
% altitude;north displacement]

if(~hHit)
setpoint = 1;

dx = zeros(size(x));
dx(1:12,:) = Ac1*x(1:12,:) + Bc1 * ones(1,size(x,2))*setpoint;

dx(13,:) = (vte + x(3,:)) * sin(x(4,:)-x(1,:));

dx(14,:) = (vte + x(3,:)) * cos(x(4,:)-x(1,:));
else
setpoint = [0;0];

dx = zeros(size(x));
dx(1:12,:) = Ac2*x(1:12,:) + Bc2 * repmat(setpoint,1,size(x,2));

dx(13,:) = (vte + x(3,:)) * sin(x(4,:)-x(1,:));

dx(14,:) = (vte + x(3,:)) * cos(x(4,:)-x(1,:));
end
end

Function MyTrim
function [deltaee, deltate, alphae] = MyTrim(vt,h,gamma)
%%Inputs
% vt is the equilibrium velocity in ft/sec
% h is the elavation in ft
% gamma is the climb angle in degrees

%%Purpose
% Reference Section 3.6 for the reasoning behind the trim function.
global x u gamma h u2
% x is the state vector, x = [vt, alpha, theta, q]'
% u is the input vector, u = [deltae, deltat]'
% u2 is the secondary input vector, u = [cg, land]'
% gamma is climb angle
% h is elevation

%Initial velocity
x(1) = vt;

%Initial elevation
h = h;

%Initial climb angle, in radians

% gamma = gamma*pi/180;

%Represents center of gravity position

cg = 0.25;

% 0 for clean flight, 1 for gears plus flaps

land = 0;

%Setup the initial input vector

u = [.1 -10*pi/180];

%Setup secondary input vector

u2 = [cg, land];

%Initial guess on alpha, in radians

x(2) = .1;
x(3) = x(2) + gamma;
x(4) = 0;

%Initial input to the cost function

s0 = [u(1) u(2) x(2)];
disp(['Initial cost = ', num2str(cost(s0))]);
[s, fval] = fminsearch(@cost, s0);

u(1) = s(1);
u(2) = s(2);
x(2) = s(3);
x(3) = s(3) + gamma;

disp(['minimum cost = ', num2str(fval)]);

disp(['minimizing vector = ', num2str(s)]);
disp(['minimizing throttle = ', num2str(s(1))]);
disp(['minimizing elevator = ', num2str(s(2)*180/pi)]);
disp(['minimizing alpha = ', num2str(s(3)*180/pi)]);

deltate = s(1);
deltaee = s(2);
alphae = s(3);

end
Function transp
function xd = transp(time, x, u, h, u2)
%Medium-sized transport aircraft, longitundinal dynamics
RTOD = 57.29578; %was used to convert radians to degrees
S = 2170;
CBAR = 17.5;
MASS = 5.0E3;
Iyy = 4.1e6;
TSTAT = 6E4;
DTDV = -38.0;
ZE = 2;
CDCLS = .042;
CLA = .085*RTOD;
CMA = -.022*RTOD;
CMDE =-.016*RTOD;
CMQ = -16.0;
CMADOT = -6;
CLADOT = 0;
GD = 32.17;

THTL = u(1);
ELEV = u(2);
XCG = u2(1);
LAND = u2(2);
VT = x(1);
ALPHA = x(2);
THETA = x(3);
Q = x(4);
H = h;

[MACH, QBAR] = adc(VT,H);

QS = QBAR*S;
GAM = THETA - x(2);

if LAND == 0
CL0 = .2;
CD0 = .016;
CM0 = .05;
DCDG = 0;
DCMG = 0;
elseif LAND == 1
CL0 = 1;
CD0 = .8;
CM0 = -.2;
DCDG = .02;
DCMG = -.05;
end

THR = (TSTAT + VT*DTDV)*max(THTL,0);

CL = CL0 + CLA*ALPHA;
CM = DCMG + CM0 + CMA*ALPHA + CMDE*ELEV + CL*(XCG-.25);
CD = DCDG + CD0 + CDCLS*CL*CL;

xd(1) = (THR*cos(x(2))-QS*CD)/MASS - GD*sin(GAM);

xd(2) = (-THR*sin(x(2))-QS*CL+MASS*(VT*Q+GD*cos(GAM)))/(MASS*VT+QS*CLADOT);
xd(3) = Q;
D = .5*CBAR*(CMQ*Q+CMADOT*xd(2))/VT;
xd(4) = (QS*CBAR*(CM + D) + THR*ZE)/Iyy;
end