16.333 Lecture # 9
Basic Longitudinal Control
? Basic aircraft control concepts
? Basic control approaches
? ? ? ?
? ?
Fall 2004 16.333 8–1
Basic Longitudinal Control
? Goal: analyze aircraft longitudinal dynamics to determine if the be-
havior is acceptable, and if not, then modify it using feedback control.
? Note that we could (and will) work with the full dynamics model,
but for now, let’s focus on the short period approximate model from
lecture 7–5.
x˙
sp
= A
sp
x
sp
+ B
sp
δ
e
where δ
e
is the elevator input, and
w Z
w
/m U
0
x
sp
=
q
, A
sp
=
I
?1
(M
w
+ M
w˙
Z
w
/m) I
?1
(M
q
+ M
w˙
U
0
)
yy yy
Z
δ
e
/m
B
sp
=
I
?1
(M
δ
e
+ M
w˙
Z
δ
e
/m)
yy
Add that θ
˙
= q, so sθ = q?
? Take the output as θ, input is δ
e
, then form the transfer function
θ(s) 1 q(s)
? ?
= = 0 1 (sI ? A
sp
)
?1
B
sp
δ
e
(s) sδ
e
(s)
? As shown in the code, for the 747 (40Kft, M = 0.8) this reduces to:
θ(s) 1.1569s + 0.3435
=
δ
e
(s)
?
s(s
2
+ 0.7410s + 0.9272)
≡ G
θδ
e
(s)
so that the dominant roots have a frequency of approximately 1
rad/sec and damping of about 0.4
Fall 2004 16.333 8–2
?1 ?0.9 ?0.8 ?0.7 ?0.6 ?0.5 ?0.4 ?0.3 ?0.2 ?0.1 0 0.1?1
?0.8
?0.6
?0.4
?0.2
0
0.2
0.4
0.6
0.8
1
0.09
0.95
0.090.20.30.420.540.68
0.84
1
0.95
0.20.30.420.540.68
0.84
1
0.2
0.4
0.6
0.8
0.8
0.2
0.4
0.6
Pole?Zero Map
Real Axis
Imaginary Axis
Figure 1: Pole-zero map for G
qδ
e
? Basic problem is that there are vast quantities of empirical data to
show that pilots do not like the ?ying qualities of an aircraft with this
combination of frequency and damping
– What do they prefer?
Acceptable
0.1
1
0
2
3
4
5
6
7
0.2 0.4 2 4
s
?
s
?
Unacceptable
Poor
Satisfactory
0.6 0.8 1
Undamped natur
al
frequency
r
ad/sec
Damping ratio
Figure 2: “Thumb Print” criterion
? This criterion has been around since the 1950’s, but it is still valid.
? Good target: frequency ≈ 3 rad/sec and damping of about ≈ 0.6
? Problem is that the short period dynamics are no where near these
numbers, so we must modify them.
Fall 2004 16.333 8–3
– Could do it by redesigning the aircraft, but it is a bit late for that...
?3.5 ?3 ?2.5 ?2 ?1.5 ?1 ?0.5 0?3
?2
?1
0
1
2
3
0.84
3.5
0.7 0.56 0.10.20.320.44
0.7 0.44 0.32
0.95
0.10.2
2.5
0.84
0.56
3
0.95
0.511.52
Pole?Zero Map
Real Axis
Imaginary Axis
Figure 3: Pole-zero map and target pole locations
? Of course there are plenty of other things that we will consider when
we design the controllers
– Small steady state error to commands
– δ
e
within limits
– No oscillations
– Speed control
Fall 2004 16.333 8–4
First Short Period Autopilot
? First attempt to control the vehicle response: measure θ and feed it
back to the elevator command δ
e
.
– Unfortunately the actuator is slow, so there is an apparent lag in
the response that we must model
δ
c
e
//
4
s + 4
δ
a
e
//
G
θδ
e
(s)
θ
//
oo
k
θ
?
OO
?θ
c
oo
? Dynamics: δ
a
is the actual elevator de?ection, δ
e
c
is the actuator
e
command created by our controller
4
θ = G
θδ
e
(s)δ
e
a
; δ
a
= H(s)δ
e
c
; H(s) =
e
s + 4
The control is just basic proportional feedback
δ
c
= ?k
θ
(θ ? θ
c
)
e
Which gives that
θ = ?G
θδ
e
(s)H(s)k
θ
(θ ? θ
c
)
or that
θ(s) G
θδ
e
(s)H(s)k
θ
=
θ
c
(s) 1 + G
θδ
e
(s)H(s)k
θ
? Looks good, but how do we analyze what is going on?
– Need to be able to predict where the poles are going as a function
of k
θ
? Root Locus
?
?
Fall 2004 16.333 8–5
Root Locus Basics
r e u
y
// OO //
G
c
(s)
//
G
p
(s)
//
?
? Assume that the plant transfer function is of the form
G
p
= K
p
N
p
= K
p
?
i
(s ? z
pi
)
D
p
i
(s ? p
pi
)
and the controller transfer function is
N
c
G
c
(s) = K
c
= K
c
?
i
(s ? z
ci
)
D
c
i
(s ? p
ci
)
? Signals are:
u control commands
y output/measurements
r reference input
e response error
? This is the unity feedback form. We could add the controller G
c
in
the feedback path without changing the pole locations.
Will add disturbances later.?
Fall 2004 16.333 8–6
? Basic questions:
– Analysis: Given N
c
and D
c
, where do the closed loop poles go
as a function of K
c
?
– Synthesis: Given K
p
, N
p
and D
p
, how should we chose K
c
, N
c
, D
c
to put the closed loop poles in the desired locations?
? Block diagram analysis: Since y = G
p
G
c
e and e = r ? y, then
easy to show that
y G
c
G
p
=
r 1 + G
c
G
p
≡ G
cl
(s)
where
K
c
K
p
N
c
N
p
G
cl
(s) =
D
c
D
p
+ K
c
K
p
N
c
N
p
is the closed loop transfer function
? The denominator is called the characteristic equation φ
c
(s) and
the roots of φ
c
(s) = 0 are called the closed-loop poles (CLP) .
? The CLP are clearly functions of K
c
for a given K
p
, N
p
, D
p
, N
c
, D
c
? a “locus of roots” [Evans, 1948]
Fall 2004 16.333 8–7
Root Locus Analysis
? General root locus is hard to determine by hand and requires Matlab
tools such as rlocus(num,den) to obtain full result, but we can get
some important insights by developing a short set of plotting rules.
– Full rules in FPE, page 260.
? Basic questions:
1. What points are on the root locus?
2. Where does the root locus start?
3. Where does the root locus end?
4. When/where is the locus on the real line?
5. Given that s
0
is found to be on the locus, what gain is need for
that to become the closed-loop pole location?
? Question #1: is point s
0
on the root locus? Assume that N
c
and D
c
are known, let
L
d
=
N
c
D
c
N
p
D
p
and K = K
c
K
p
then
φ
c
(s) = 1 + KL
d
(s) = 0
or equivalently for values of s for which L
d
(s) = ?1/K, with K real.
– For K positive, s
0
is on the root locus if
∠L
d
(s
0
) = 180
?
±l ·360
?
, l = 0, 1, . . .
– If K negative, s
0
is on the root locus if [0
?
locus]
∠L
d
(s
0
) = 0
?
±l ·360
?
, l = 0, 1, . . .
These are known as the phase conditions.
Fall 2004 16.333 8–8
? Question #2: Where does the root locus start?
N
c
N
p
φ
c
= 1 + K = 0
D
c
D
p
? D
c
D
p
+ KN
c
N
p
= 0
So if K → 0, then locus starts at solutions of D
c
D
p
= 0 which are
the poles of the plant and compensator.
? Question #3: Where does the root locus end?
Already shown that for s
0
to be on the locus, must have
1
L
d
(s
0
) = ?
K
So if K →∞, the poles must satisfy:
N
c
N
p
L
d
= = 0
D
c
D
p
? There are several possibilities:
1. Poles are located at values of s for which N
c
N
p
= 0, which are
the zeros of the plant and the compensator
2. If Loop L
d
(s) has more poles than zeros
– As s , L
d
(s)|→ 0, but we must ensure that the phase | | →∞ |
condition is still satis?ed.
? ?
Fall 2004 16.333 8–9
? More details as K →∞:
– Assume there are n zeros and p poles of L
d
(s)
– Then for large |s|,
1
L
d
(s) ≈
(s ?α)
p?n
– So the root locus degenerates to:
1
1 + = 0
n
(s ?α)
p?
– So n poles head to the zeros of L
d
(s)
– Remaining p ? n poles head to s = ∞ along asymptotes | |
de?ned by the radial lines
180
?
+ 360
?
·(l ?1)
φ
l
= l = 1, 2, . . .
p ?n
so that the number of asymptotes is governed by the number of
poles compared to the number of zeros (relative degree).
– If z
i
are the zeros if L
d
and p
j
are the poles, then the centroid
of the asymptotes is given by:
p n
z
i
p
j
?
α =
p ?n
? Example: L(s) = s
?4
?0.8 ?0.6 ?0.4 ?0.2 0 0.2 0.4 0.6 0.8?0.8
?0.6
?0.4
?0.2
0
0.2
0.4
0.6
0.8 Root Locus
Real Axis
Imaginary Axis
?
?
?
?
?
?
?
?
Fall 2004 16.333 8–10
? Question #4: When/where is the locus on the real line?
– Locus points on the real line are to the left of an odd number
of real axis poles and zeros [K positive].
– Explanation a bit too detailed and not that relevant
? Question #5: Given that s
0
is found to be on the locus, what gain
is needed for that to become the closed-loop pole location?
– Need
1
=K ≡
L
d
(s
0
)
D
p
(s
0
)D
c
(s
0
)
N
p
(s
0
)N
c
(s
0
)
| |
– Since K = K
p
K
c
, sign of K
c
depends on sign of K
p
3 e.g., assume that ∠L
d
(s
0
) = 180
?
, then need K
c
and K
p
to
be same sign so that K > 0
Fall 2004 16.333 8–11
Root Locus Examples
Figure 4: Basic Figure 5: Two poles
Figure 6: Add zero Figure 7: Three poles
Figure 8: Add zero
Examples similar to control design process: add compensator dynamics to mod-
ify root locus and then chose gain to place CLP at desired location on the locus.
Fall 2004 16.333 8–12
Figure 9: Complex case
?5 ?4 ?3 ?2 ?1 0 1 2 3 4?1
?0.8
?0.6
?0.4
?0.2
0
0.2
0.4
0.6
0.8
1 Root Locus
Real Axis
Imaginary Axis
?2.5 ?2 ?1.5 ?1 ?0.5 0 0.5 1 1.5 2 2.5?5
?4
?3
?2
?1
0
1
2
3
4
5 Root Locus
Real Axis
Imaginary Axis
Figure 10: Very complex case
?
Fall 2004 16.333 8–13
Dynamic Compensation
? For a given plant, can draw a root locus versus K. But if desired
pole locations are not on that locus, then need to modify it using
dynamic compensation.
– Basic root locus plots give us an indication of the e?ect of adding
compensator dynamics. But need to know what to add to place
the poles where we want them.
? New questions:
– What type of compensation is required?
– How do we determine where to put the additional dynamics?
? There are three classic types of controllers u = G
c
(s)e
1. Proportional feedback: G
c
≡ K
g
a gain, so that N
c
= D
c
= 1
– Same case we have been looking at.
2. Integral feedback
t
K
i
u(t) = K
i
e(τ )dτ ? G
c
(s) =
0
s
– Used to reduce/eliminate steady-state error
– If e(τ ) is approximately constant, then u(t) will grow to be very
large and thus hopefully correct the error.
? ?
Fall 2004 16.333 8–14
– Consider error response of
G
p
(s) = 1/(s + a)(s + b)
(a > 0, b > 0) to a step,
r(t) = 1(t) → r(s) = 1/s
where
e
r
=
1
1 + G
c
G
p
→ e(s) =
r(s)
(1 + G
c
G
p
)
– To analyze error, use FVT
lim e(t) = lim se(s)
s 0t→∞ →
so that with proportional control,
s 1 1
lim e
ss
= lim =
t→∞ s→0
s 1 + K
g
G
p
(s)
1 +
K
g
ab
so can make e
ss
small, but only with a very large K
g
– With integral control, lim
s 0
G
c
(s) = ∞, so e
ss
→ 0
→
– Integral control improves the steady state, but this is at
the expense of the transient response (typically gets worse
because the system is less well damped)
Fall 2004 16.333 8–15
1
Example #1: G(s) = , add integral feedback to
(s + a)(s + b)
improve the steady state response.
?6 ?5 ?4 ?3 ?2 ?1 0 1 2?4
?3
?2
?1
0
1
2
3
4 Root Locus
Real Axis
Imaginary Axis
Figure 11: RL after adding integral FB
– Increasing K
i
to increase speed of the response pushes the poles
towards the imaginary axis → more oscillatory response.
Combine proportional and integral (PI) feedback:
G
c
= K
1
+
K
2
=
K
1
s + K
2
s s
which introduces a pole at the origin and zero at s = ?K
2
/K
1
– PI solves many of the problems with just integral control (I).
?3 ?2.5 ?2 ?1.5 ?1 ?0.5 0?5
?4
?3
?2
?1
0
1
2
3
4
5 Root Locus
Real Axis
Imaginary Axis
Figure 12: RL with proportional and integral FB
Fall 2004 16.333 8–16
3. Derivative Feedback u = K
d
e˙ so that G
c
(s) = K
d
s
– Does not help with the steady state
– Provides feedback on the rate of change of e(t) so that the
control can anticipate future errors.
1
Example # 2 G(s) = , (a > 0, b > 0)
(s ? a)(s ? b)
with G
c
(s) = K
d
s
?2 ?1.5 ?1 ?0.5 0 0.5 1 1.5 2?1.5
?1
?0.5
0
0.5
1
1.5 Root Locus
Real Axis
Imaginary Axis
Figure 13: RL with derivative FB
– Derivative feedback is very useful for pulling the root locus into
the LHP - increases damping and more stable response.
Typically used in combination with proportional feedback to form
proportional-derivative feedback PD
G
c
(s) = K
1
+ K
2
s
which moves the zero from the origin.
Fall 2004 16.333 8–17
Controller Synthesis
? First determine where the poles should be located
? Will proportional feedback do the job?
? What types of dynamics need to be added? Typically use main build-
ing block
(s + z)
G
B
(s) = K
c
(s + p)
? Can be made to look like various controllers, depending on how we
pick K
c
, p, and z
– If we pick z > p, with p small, then
(s + z)
G
B
(s) ≈ K
c
s
which is essentially a PI compensator, called a lag.
– If we pick p ? z, then at low frequency, the impact of p/(s + p)
is small, so
G
B
(s) ≈ K
c
(s + z)
which is essentially PD compensator, called a lead.
? Various algorithms exist to design the components of the lead and
lag compensators (see 16.31 course notes online)
Fall 2004 16.333 8–18
Pole Placement
? One option for simple systems is called pole placement.
? Consider a simple system G
p
= s
?2
for which we want the closed
loop poles to be at ?1± 2i
? Proportional control clearly not su?cient, so use a compensator with
1 pole.
(s + z)
G
c
= K
(s + p)
So there are 3 CLP.
? Know that the desired characteristic equation is
φ
d
(s) = (s
2
+ 2s + 5)(s + α) = 0
? The actual closed loop poles solve:
φ
c
(s) = 1 + G
p
G
c
= 0
2
→ s (s + p) + K(s + z) = 0
3 2
→ s + s p + Ks + Kz = 0
? Clearly need to pull the poles at the origin into the LHP, so need a
lead compensator → Good rule of thumb is to take p = 10z as a
starting point.
? Compare the characteristic equations:
2
φ
c
(s) = s
2
+ 10zs + Ks + Kz = 0
φ
d
(s) = (s
2
+ 2s + 5)(s + α)
3 2
= s + s (α + 2) + s(2α + 5) + 5α = 0
gives
2
s α + 2=10z
s 2α + 5=K
0
s 5α=zK
Fall 2004 16.333 8–19
solve for α, z, K
25 5z
K =
5 ? 2z
; α =
5 ? 2z
→ z = 2.23, α = 20.25, K = 45.5
?25 ?20 ?15 ?10 ?5 0?20
?15
?10
?5
0
5
10
15
20
Root Locus
Real Axis
Imaginary Axis
Figure 14: CLP with pole placement
Fall 2004 16.333 8–20
Autopilot
? Back to the problem on 9–4: θ feedback to δ
e
to control short period
model
φ
c
(s) = 1 + G
θδ
e
(s)H(s)k
θ
= 0
with
1.1569s + 0.3435 4
G
θδ
e
(s) = ?
s
3
+ 0.7410s
2
+ 0.9272s
, H(s) =
s + 4
? Pole/zero map:
Figure 15: Pole/zero map of the short period autopilot with θ feedback
? Note: K
p
< 0, so if k
θ
> 0, then we would have to draw a 0
?
locus
– Pole at origin would move to right along real axis ? unstable.
?4 ?3 ?2 ?1 0 1 2 3 4
?4
?3
?2
?1
0
1
2
3
4
SP Autopilot with q FB: kq > 0
Real Axis
Imaginary Axis
Figure 16: With k
θ
> 0
Fall 2004 16.333 8–21
? So must use k
θ
< 0
?4 ?3 ?2 ?1 0 1 2 3 4
?4
?3
?2
?1
0
1
2
3
4
SP Autopilot with q FB: kq < 0
Real Axis
Imaginary Axis
Figure 17: With k
θ
< 0
? Clear from the plot that θ feedback alone is not going to work par-
ticularly well.
– Need to increase the gain to move the pole from the origin, but
in doing so the SP poles start to move very close to the imaginary
axis ? making the response worse
Fall 2004 16.333 8–22
? Could use a rate gyro as the sensor instead and feedback q
1.1569s + 0.3435 4
G
qδ
e
(s) = ?
s
2
+ 0.7410s + 0.9272
, H(s) =
s + 4
Figure 18: PZmap with q FB
? Again, pick the gain k
q
< 0. Note locus for the real pole.
?4 ?3 ?2 ?1 0 1
?4
?3
?2
?1
0
1
2
3
4
SP Autopilot with q FB: kq < 0
Real Axis
Imaginary Axis
Figure 19: With k
q
< 0. ? give the CLP with k
q
= ?1
Fall 2004 16.333 8–23
? Can get an even better result if we combine the q and θ feedback –
like PD on the measurement θ.
δ
c
e
//
4
s + 4
δ
a
e
//
G
qδ
e
q
//
oo
1
s
θ
//
oo
k
q
?
OO
k
θ
?
OO
?θ
c
oo
δ
c
e
= ?k
q
q ? k
θ
(θ ? θ
c
) δ
a
e
= H(s)δ
c
e
q = sθ
θ =
1
s
G
qδ
e
δ
a
e
= G
θδ
e
δ
a
e
= ?G
θδ
e
H [(k
θ
+ k
q
s)θ ? k
θ
θ
c
]
θ G
θδ
e
Hk
θ
=
θ
c
1 + G
θδ
e
H(k
θ
+ k
q
s)
? Now pick k
q
= ζk
θ
, and do RL versus k
θ
φ
c
(s) = 1 + G
θδ
e
H(1 + ζs)k
θ
= 0
– Places a zero at s = ?1/ζ
? Step response is reasonable, but now need to pick the k
θ
and k
q
to
get the desired ω
sp
and ζ
sp
Fall 2004 16.333 8–24
?4.5 ?4 ?3.5 ?3 ?2.5 ?2 ?1.5 ?1 ?0.5 0
?4
?3
?2
?1
0
1
2
3
4
SP PD Autopilot with z=1 FB: kq < 0
Real Axis
Imaginary Axis
Figure 20: PD feedback with ζ = 1. ? are the closed loop poles.
0 5 10 15 20 25 30 350
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
q step response to qc
Time (sec)
q
(t) ? rads
Figure 21: PD feedback with ζ = 1 - step
Fall 2004 16.333 8–25
?4.5 ?4 ?3.5 ?3 ?2.5 ?2 ?1.5 ?1 ?0.5 0
?4
?3
?2
?1
0
1
2
3
4
SP PD Autopilot with z=1.95 FB: kq < 0
Real Axis
Imaginary Axis
0 5 10 15 20 25 30 350
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
q step response to qc
Time (sec)
q
(t) ? rads
Figure 22: PD FB - step with ζ = 1.95 k
θ
= ?1. ? are the closed loop poles.
Fall 2004 16.333 8–26
Summary
? Presented only basics of control design and analysis, but this is enough
for us to get started
? Working with smaller models is good for design, but need to con?rm
that the full model still stable
? Could work with the full model, but that is much easier with state
space tools
Fall 2004 16.333 8–27
SP control codes (lect9a.m)
1 %
2 % Fall 2004
3 % 16.333
4 % Codes to investigate control for the SP
5 %
6 close all
7 figure(1);clf
8 set(gcf,’DefaultLineLineWidth’,2)
9 set(gcf,’DefaultlineMarkerSize’,10)
10 rlocus([1],conv([1 0],[1 3 2]))
11 print -depsc rl_pi1
12
13 load b747 Asp Bsp
14 figure(10);pzmap(ss(Asp,Bsp,[0 1],0));grid on;
15 hh=get(10,’children’);hhh=get(hh(1),’children’)
16 set(hhh(1),’MarkerSize’,24);set(hhh(1),’LineWidth’,2)
17 set(hhh(2),’MarkerSize’,24);set(hhh(2),’LineWidth’,2)
18 axis([-1 0.1 -1 1])
19 print -depsc pz_sp
20 jpdf(’pz_sp’)
21
22 figure(10);clf
23 test=[% thumbnail data from Nelson 167
24 .5 .4;
25 .8 .4;
26 1 .55;
27 .7 .6]
28 test(:,2)=test(:,2)*2*pi;
29 stest=[-test(:,1).*test(:,2) test(:,2).*sqrt(1-test(:,1).^2)]
30 fill(stest(:,1),stest(:,2),’blue’,stest(:,1),-stest(:,2),’blue’)
31 hold on
32 pzmap(ss(Asp,Bsp,[0 1],0));grid on;
33 hh=get(10,’children’);hhh=get(hh(1),’children’)
34 set(hhh(1),’MarkerSize’,24);set(hhh(1),’LineWidth’,2)
35 set(hhh(2),’MarkerSize’,24);set(hhh(2),’LineWidth’,2)
36 hold off
37 print -depsc pz_sp2
38 jpdf(’pz_sp2’)
39
40 figure(1);clf
41 set(gcf,’DefaultLineLineWidth’,2)
42 set(gcf,’DefaultlineMarkerSize’,10)
43 rlocus([1],conv([1 0],[1 3 2]))
44 print -depsc rl_pi1
45
46 figure(2);
47 set(gcf,’DefaultLineLineWidth’,2)
48 set(gcf,’DefaultlineMarkerSize’,12)
49 rlocus([1 3],conv([1 0],[1 3 2]))
50 print -depsc rl_pi2
51
52 figure(2);
53 set(gcf,’DefaultLineLineWidth’,2)
54 set(gcf,’DefaultlineMarkerSize’,12)
55 rlocus([1 0],conv([1 -2],[1 -1]))
56 print -depsc rl_d1
57
58 %Example: G(s)=1/2^2
59 %Design Gc(s) to put the clp poles at -1 + 2j
60 z=roots([-20 49 -10]);z=max(z),k=25/(5-2*z),alpha=5*z/(5-2*z),
61 num=1;den=[1 0 0];
62 knum=k*[1 z];kden=[1 10*z];
63 rlocus(conv(num,knum),conv(den,kden));
64 hold;plot(-alpha+eps*j,’d’);plot([-1+2*j,-1-2*j],’d’);hold off
65 r=rlocus(conv(num,knum),conv(den,kden),1)’
66 print -depsc rl_pp
Fall 2004 16.333 8–28
67
68 jpdf(’rl_pi1’)
69 jpdf(’rl_pi2’)
70 jpdf(’rl_d1’)
71 jpdf(’rl_pp’)
72
73 load b747 Asp Bsp
74 G=tf(ss(Asp,Bsp,[0 1],0))*tf(1,[1 0]);
75 H=tf(4,[1 4]);
76 rlocus(G*H)
77 axis([-3 3 -3 3]*1.5)
78 title(’SP Autopilot with \theta FB: k_\theta
79 print -depsc sp_ap1
80 jpdf(’sp_ap1’)
81
82 rlocus(-G*H)
83 axis([-3 3 -3 3]*1.5)
84 title(’SP Autopilot with \theta FB: k_\theta
85 print -depsc sp_ap2
86 jpdf(’sp_ap2’)
87
88 load b747 Asp Bsp
89 G=tf(ss(Asp,Bsp,[0 1],0));
90 H=tf(4,[1 4]);
91 rlocus(-G*H)
92 rr=rlocus(-G*H,1)
93 hold on
> 0’,’FontSize’,16)
< 0’,’FontSize’,16)
94 plot(rr+sqrt(-1)*eps,’d’,’MarkerFaceColor’,’b’)
95 hold off
96 axis([-3 1 -3 3]*1.5)
97 title(’SP Autopilot with q FB: k_q < 0’,’FontSize’,16)
98 print -depsc sp_ap3
99 jpdf(’sp_ap3’)
100
101 load b747 Asp Bsp
102 zeta=1;
103 k_th=1;
104 PD=tf([zeta 1],1);
105 G=tf(ss(Asp,Bsp,[0 1],0))*tf(1,[1 0]);
106 H=tf(4,[1 4]);
107 rlocus(-G*H*PD)
108 rr=rlocus(-G*H*PD,k_th)
109 hold on
110 plot(rr+sqrt(-1)*eps,’d’,’MarkerFaceColor’,’b’)
111 plot(-1.8+2.4*sqrt(-1),’s’,’MarkerFaceColor’,’r’)
112 plot(-1.8-2.4*sqrt(-1),’s’,’MarkerFaceColor’,’r’)
113 hold off
114 axis([-3 0 -3 3]*1.5)
115 title(’SP PD Autopilot with \zeta=1 FB: k_\theta
116 print -depsc sp_pd1
117 jpdf(’sp_pd1’)
118
119 figure(2);
120 set(gcf,’DefaultLineLineWidth’,2)
121 set(gcf,’DefaultlineMarkerSize’,12)
122 G_cl=G*H*(-k_th)/(1+G*H*PD*(-k_th));
123 step(G_cl,35)
< 0’,’FontSize’,16)
124 h=get(gcf,’children’);hh=get(h(1),’children’);set(hh(1),’LineWidth’,2)
125 title(’\theta step response to \theta_c’)
126 ylabel(’\theta(t) - rads’)
127 print -depsc tstep
128 jpdf(’tstep’)
Fall 2004 16.333 8–29
SP control codes: Part 2 (lect9b.m)
1 % 16.333 Fall 2004
2 % SP design meeting the performance goals a
3 %
4 load b747 Asp Bsp
5 G=tf(ss(Asp,Bsp,[0 1],0))*tf(1,[1 0]);
6 H=tf(4,[1 4]);
7
8 zeta=1.95;
9 k_th=1;
10 PD=tf([zeta 1],1);
11
12 rlocus(-G*H*PD)
13 rr=rlocus(-G*H*PD,k_th)
14 hold on
bit better
15 plot(rr+sqrt(-1)*eps,’d’,’MarkerFaceColor’,’b’)
16 plot(-1.8+2.4*sqrt(-1),’s’,’MarkerFaceColor’,’r’)
17 plot(-1.8-2.4*sqrt(-1),’s’,’MarkerFaceColor’,’r’)
18 hold off
19 axis([-3 0 -3 3]*1.5)
20 title([’SP PD Autopilot with \zeta=’,num2str(zeta),’
21 print -depsc sp_pd2
22 jpdf(’sp_pd2’)
23
24 figure(2);
25 set(gcf,’DefaultLineLineWidth’,2)
26 set(gcf,’DefaultlineMarkerSize’,12)
27 G_cl=G*H*(-k_th)/(1+G*H*PD*(-k_th));
28 step(G_cl,35)
FB: k_\theta < 0’],’FontSize’,16)
29 h=get(gcf,’children’);hh=get(h(1),’children’);set(hh(1),’LineWidth’,2)
30 title(’\theta step response to \theta_c’)
31 ylabel(’\theta(t) - rads’)
32 print -depsc tstep2
33 jpdf(’tstep2’)
34
35 load b747 A B
36 Gf=tf(ss(A,B(:,1),[0 0 0 1],0));
37 H=tf(4,[1 4]);
38
39 zeta=1.95;
40 k_th=1;
41 PD=tf([zeta 1],1);
42
43 rlocus(-Gf*H*PD)
44 rrf=rlocus(-Gf*H*PD,k_th)
45 hold on
46 plot(rrf+sqrt(-1)*eps,’s’,’MarkerFaceColor’,’b’)
47 plot(rr+sqrt(-1)*eps,’rd’,’MarkerFaceColor’,’r’)
48 hold off
49 axis([-3 0 -3 3]*1.5)
50 title([’SP PD Autopilot with \zeta=’,num2str(zeta),’ FB: k_\theta < 0’],’FontSize’,16)
51 print -depsc sp_f1
52 jpdf(’sp_f1’)
Fall 2004 16.333 8–30
SP control: Basic Dynamics (lect9.m)
1 clear all
2 prt=1;
3 %
4 % B747 longitudinal dynamics
5 % 16.333 Fall 2004
6 %
7 % B747 at Mach 0.8, 40,000ft, level-flight
8 % From Etkin and Reid
9 %
10 % metric
11 Xu=-1.982e3;Xw=4.025e3;
12 Zu=-2.595e4;Zw=-9.030e4;Zq=-4.524e5;Zwd=1.909e3;
13 Mu=1.593e4;Mw=-1.563e5;Mq=-1.521e7;Mwd=-1.702e4;
14
15 g=9.81;theta0=0;S=511;cbar=8.324;
16 U0=235.9;Iyy=.449e8;m=2.83176e6/g;cbar=8.324;rho=0.3045;
17 Xdp=.3*m*g;Zdp=0;Mdp=0;
18 Xde=-3.818e-6*(1/2*rho*U0^2*S);Zde=-0.3648*(1/2*rho*U0^2*S);
19 Mde=-1.444*(1/2*rho*U0^2*S*cbar);;
20
21 A=[Xu/m Xw/m 0 -g*cos(theta0);[Zu Zw Zq+m*U0 -m*g*sin(theta0)]/(m-Zwd);
22 [Mu+Zu*Mwd/(m-Zwd) Mw+Zw*Mwd/(m-Zwd) Mq+(Zq+m*U0)*Mwd/(m-Zwd) ...
23 -m*g*sin(theta0)*Mwd/(m-Zwd)]/Iyy;
24 [ 0 0 1 0]];
25 B=[Xde/m Xdp/m;Zde/(m-Zwd) Zdp/(m-Zwd);(Mde+Zde*Mwd/(m-Zwd))/Iyy ...
26 (Mdp+Zdp*Mwd/(m-Zwd))/Iyy;0 0];
27
28 % Short-period Approx
29 Asp=[Zw/m U0;
30 [Mw+Zw*Mwd/m Mq+U0*Mwd]/Iyy];
31 Bsp=[Zde/m;(Mde+Zde/m*Mwd)/Iyy];
32 [nsp,dsp]=ss2tf(Asp,Bsp,eye(2),zeros(2,1));
33 [Vsp,evsp]=eig(Asp);evsp=diag(evsp);
34 %rifd(evsp)
35
36 %
37 % CONTROL
38 %
39 % actuator dynamics are a lag at 4
40 actn=4;actd=[1 4]; % H(s) in notes
41 %
42 % use the short period model since that is all we are trying to control
43 %
44 [nsp,dsp]=ss2tf(Asp,Bsp,eye(2),zeros(2,1));
45 [nfull,dfull]=ss2tf(A,B(:,1),eye(4),zeros(4,1));
46 %
47 % form num and den for the "plant" = act_dyn*G == N/D
48 %
49 % short period model
50 N=conv(actn,nsp(2,:));% q is second state
51 D=conv(actd,dsp);
52 % full model
53 Nqfull=conv(actn,nfull(3,:));% q is third state
54 Ntfull=conv(actn,nfull(4,:));% theta is fourth state
55 Dfull=conv(actd,dfull);
56 %
57 % add an extra pole to get the integrator for the \dot theta = q
58 %
59 Ntheta=conv(N,1);Dtheta=conv(D,[1 0]);
60
61 figure(1);clf;
62 K_th0=-1;
63 rlocus(sign(K_th0)*Ntheta,Dtheta);
64 r_th0=rlocus(Ntheta,Dtheta,K_th0)’;hold on;plot(r_th0+eps*sqrt(-1),’v’);
65 hold off
66 sgrid(.6,3); % gives target damping and freqs
Fall 2004 16.333 8–31
67 axis([-5 .5 -4 4]);grid
68 title(’Closed-loop poles using only theta FB’)
69 if prt
70 print -depsc ac_th0
71 end
72
73 figure(2);clf;
74 K_q=-1;
75 rlocus(sign(K_q)*N,D);
76 r_q=rlocus(N,D,K_q)’;hold on;plot(r_q+eps*sqrt(-1),’v’);
77 hold off
78 sgrid(.6,3);
79 axis([-5 .5 -4 4]);grid
80 title(’Closed-loop poles using q FB’)
81 if prt
82 print -depsc ac_q
83 end
84 %
85 %closed-loop dynamics with q FB in place (inner loop)
86 % GH/(1+k_q GH) = (N/D) / (1+k_q N/D) = N/(D+k_q N)
87 %
88 Nq=N;Dq=K_q*[0 N]+D;
89 %
90 % add integrator for q ==> theta
91 %
92 K_th=-1;
93 %
94 figure(3);clf;
95 rlocus(sign(K_q)*Nq,conv(Dq,[1 0]));grid
96 r_th=rlocus(Nq,conv(Dq,[1 0]),K_th)’;hold on;
97 plot(r_q+eps*sqrt(-1),’v’);plot(r_th+eps*sqrt(-1),’^’);
98 hold off
99 sgrid(.6,3);
100 axis([-5 .5 -4 4]);grid
101 title(’Closed-loop poles also using theta FB’)
102 if prt
103 print -depsc ac_th
104 end
105 %
106 % as a final check, form the closed-loop dynamics
107 % using the original short period model
108 Ncl=Ntheta*K_th;
109 Dcl=Dtheta+conv([0 N],[K_q K_th]);
110 roots(Dcl)
111 %
112 SPcl=tf(Ncl,Dcl);
113 [y,t]=step(SPcl);
114 figure(4)
115 plot(t,y);xlabel(’time’);ylabel(’\theta rad’,’FontSize’,12)
116 if prt
117 print -depsc ac_th_step
118 end
119
120 %
121 % as a final check, form the closed-loop dynamics
122 % using the full dynamics from del_e to theta
123 Nfcl=Ntfull*K_th;
124 Dfcl=Dfull+conv(Ntfull,[K_q K_th]);
125 roots(Dfcl)
126 figure(3)
127 hold on;plot(roots(Dfcl)+eps*sqrt(-1),’r*’);hold off
128 axis([-3 .5 -2 2]);grid
129 title(’Closed-loop poles with q and th FB on FULL model’)
130 if prt
131 print -depsc ac_full
132 end
133
134 %return
135
136 %
137 % add theta state to the short period approx model
138 %
1
2
3
4
5
6
7
8
9
Fall 2004 16.333 8–32
139 Ksp=place(Asp,Bsp,[roots([1 2*.6*3 3^2])’])
140 Asp2=[Asp [0 0]’;0 1 0];Bsp2=[Bsp;0];
141 Ksp2=place(Asp2,Bsp2,[roots([1 2*.6*3 3^2])’,-.25])
142 %
143 % add actuator model to SP with theta
144 %
145 Plist=[roots([1 2*.6*3 3^2])’,-.25,-3];
146 At2=[Asp2 Bsp2(:,1);zeros(1,3) -4];Bt2=[zeros(3,1);4];
147 Kt2=place(At2,Bt2,[Plist])
148 step(ss(At2-Bt2*Kt2,Bt2,[0 0 1 0],0),35)
149 hh=get(gcf,’children’);hhh=get(hh(1),’children’)
150 set(hhh(1),’MarkerSize’,24);set(hhh(1),’LineWidth’,2)
151 ylabel(’\theta rads’,’FontSize’,12)
152 print -depsc fsfb_step.eps
153 jpdf(’fsfb_step’)
154 %
155 ev=eig(A);
156 % dampe short period, but leave the phugoid where it is
157 Plist=[roots([1 2*.6*3 3^2])’ ev([3 4],1)’];
158 K1=place(A,B(:,1),Plist)
159 %
160 % add actuator model
16 %
16 At=[A B(:,1);zeros(1,4) -4];Bt=[zeros(4,1);4];
16 Kt=place(At,Bt,[Plist -3])
16
16 save b747 A B Asp Bsp
16
16 %!eps2pdf /f="ac7_fig1.eps"
16 %!mv c:\ac7_fig1.pdf ./ac7_fig1.pdf
16
170
