Lec12 PDF
Lec12 PDF
http://ocw.mit.edu
For information about citing these materials or our Terms of Use, visit: http://ocw.mit.edu/terms.
16.323 Lecture 12
• Bryson Chapter 14
• Stengel Chapter 5
• System dynamics:
ẋ(t) = A(t)x(t) + Bu(t)u(t) + Bw (t)w(t)
– Assume that w(t) is a white Gaussian noise 20 ⇒ N (0, Rww )
– The initial conditions are random variables too, with
E[x(t0)] = 0, and E[x(t0)xT (t0)] = X0
– Assume that a perfect measure of x(t) is available for feedback.
• Given the noise in the system, need to modify our cost functions from
before ⇒ consider the average response of the closed-loop system
1 tf T
� � �
1 T
Js = E x (tf )Ptf x(tf ) + (x (t)Rxx(t)x(t) + uT (t)Ruu(t)u(t))dt
2 2 t0
– Average over all possible realizations of the disturbances.
20 16.322 Notes
w2 y
� G(s) �
• Account for the spectral content using a shaping filter H(s), so that
the picture now is of a system y = G(s)H(s)w1, with a white noise
input
w1 w̃2 y
� H(s) � G(s) �
– Then must design filter H(s) so that the output is a noise w̃2 that
has the frequency content that we need
y = Cy 0
xH
where the noise input w1 is a white Gaussian noise.
• Clearly this augmented system has the same form as the original system
that we analyzed - there are just more states to capture the spectral
content of the original shaped noise.
21 K+S pg 262
• Recall that the specific initial conditions do not effect the LQR con
troller, but they do impact the cost-to-go from t0
– Consider the stochastic LQR problem, but with w(t) ≡ 0 so that
the only uncertainty is in the initial conditions
– Have already shown that LQR cost can be written in terms of the
solution of the Riccati equation (4–7):
1 T
JLQR = x (t0)P (t0)x(t0)
2 � �
1 T
⇒ Js = E x (t0)P (t0)x(t0)
2
1 � T
�
= E trace[P (t0)x(t0)x (t0)]
2
1
= trace[P (t0)X0]
2
which gives expected cost-to-go with uncertain IC.
• Not too useful in this form, but if P (t) is the solution of the LQR
Riccati equation, then can show that the cost can be written as:
� � tf �
1
Js = trace P (t0)X(t0) + (P (t)Bw Rww BwT )dt
2 t0
1 � �
Js = trace Ptf X(tf ) + P (t0)X(t0) − Ptf X(tf )
2 �� tf �
1
+ trace {Rxx(t)X(t) + Ruu(t)K(t)X(t)K(t)T }dt
2
�� t0tf �
1
+ trace {Ṗ (t)X(t) + P (t)Ẋ(t)}dt
2 t0
−1 T
and (first reduces to standard CARE if K(t) = Ruu Bu P (t))
• For LTI system with stationary process noise (constant Rww ) and well-
posed time-invariant control problem (steady gain u(t) = −Kssx(t))
mean square value of state settles down to a constant
lim X(t) = Xss
tf →∞
• Consider a missile roll attitude control system with ω the roll angular
velocity, δ the aileron deflection, Q the aileron effectiveness, and φ
the roll angle, then
1 Q
ω̇ = − ω + δ + n(t)
δ̇ = u φ̇ = ω
τ τ
where n(t) is a noise input.
⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎡ ⎤
δ̇ 0 0 0 δ 1 0
⎣ ω̇ ⎦ = ⎣ −1/τ Q/τ 0 ⎦ ⎣ ω ⎦ + ⎣ 0 ⎦ u + ⎣ 1 ⎦ n
φ̇ 0 1 0 φ 0 0
23 Process noise input to a derivative of ω, so the units of n(t) must be deg/sec2 , but since E[n(t)n(τ )] = R
ww δ(t − τ ) and
δ(t)dt = 1, then the units of δ(t) are 1/sec and thus the units of Rww are (rad/sec2 )2 · sec=rad2 /sec3
�
• Cost:
� � tf �
1 T 1
J =E x (tf )Ptf x(tf ) + (zT (t)Rzzz(t) + uT (t)Ruuu(t))dt
2 2 t0
• Estimator from:
x̂˙(t) = Ax̂ + Buu + L(t)(y(t) − Cy x̂(t))
where x̂(t0) = x̄0 and Q(t0) = Q0
−1
Q̇(t) = AQ(t) + Q(t)AT + Bw Rww BwT − Q(t)CyT Rvv Cy Q(t)
−1
L(t) = Q(t)CyT Rvv
Ac = A − BuK(t) − L(t)Cy
Bc = L(t)
Cc = K(t)
• Valid for SISO and MIMO systems. Plant dynamics can also be time-
varying, but suppressed for simplicity.
– Obviously compensator is constant if we use the steady state regu
lator and estimator gains for an LTI system.
– Again, this changes the cost, but not the optimality conditions
• Note that
J¯ = Tr[PssLssRvv LTss + QssCzT RzzCz ]
= Tr[PssBw Rww BwT + QssKssT
RuuKss]
both of which contain terms that are functions of the control and
estimation problems.
• To see how both terms contribute, let the regulator get very fast
⇒ Ruu → 0. A full analysis requires that we then determine what
¯ But what is clear is that:
happens to Pss and thus J.
lim J¯ ≥ Tr[QssCzT RzzCz ]
Ruu →0
• Rule of Thumb: for given Rzz and Rww , select Ruu and Rvv so that
the performance contributions due to the estimation and regulation
error are comparable.
• It is not obvious that this system will even be stable: λi(Acl) < 0?
– To analyze, introduce n = x − xc, and the similarity transform
� � � � � �
I 0 x x
T = = T −1 ⇒ =T
I −I n xc
so that Acl ⇒ T AclT −1 ≡ Acl and when you work through the
math, you get
� �
A − BuK BuK
Acl =
0 A − LCy
– This shows that we can design any estimator and regulator sepa
rately with confidence that the combination will stabilize the system.
� Also means that the LQR/LQE problems decouple in terms of
being able to predict the stability of the overall closed-loop system.
r e u y
� � Gc(s) � G(s) �
• Important points:
– Closed-loop system will be stable, but the compensator dynamics
need not be.
– Often very simple and useful to provide classical interpretations of
the compensator dynamics Gc(s).
• Approach:
– Rewrite cost and system in terms of the estimator states and dy
namics ⇒ recall we have access to these
– Design a stochastic LQR for this revised system ⇒ full state feed
back on x̂(t)
• Start with the cost (use a similar process for the terminal cost)
E[zT Rzzz] = E[xT Rxxx] {±x̂}
= E[(x − x̂ + x̂)T Rxx(x − x̂ + x̂)] {x̃ = x − x̂}
= E[x̃T Rxxx̃] + 2E[x̃T Rxxx̂] + E[x̂T Rxxx̂]
• Note that x̂(t) is the minimum mean square estimate of x(t) given
y(τ ), u(τ ), t0 ≤ τ ≤ t.
– Key property of that estimate is that x̂ and x̃ are uncorrelated24
E[x̃T Rxxx̂] = trace[E{x̃x̂T }Rxx] = 0
• Also,
E[x̃T Rxxx̃] = E[trace(Rxxx̃x̃T )] = trace(RxxQ)
where Q is the solution of the LQE Riccati equation (11–11)
25 Gelb, pg 317
� �
0 1
� � � �
0 0
ẋ = x+ u+ w
0 0 1 1
� �
1 0
z = x
0 1
� �
y = 1 0 x+v
where in the LQG problem we have
� �
1 0
Rzz = Ruu = 1 Rvv = 1 Rww = 1
0 1
• Solve the SS LQG problem to find that
Tr[PssLssRvv LTss] = 8.0 Tr[QssCzT RzzCz ] = 2.8
Bu=[0 1]’;%
Bw=[0 1]’; %
Cy=[1 0];%
Rww=1;%
Rvv=1;%
Rzz=diag([1 1]);%
Ruu=1;%
[K,P]=lqr(A,Bu,Cz*Rzz*Cz’,Ruu);%
[L,Q]=lqr(A’,Cy’,Bw*Rww*Bw’,Rvv);L=L’;%
N1=trace(P*(L*Rvv*L’))%
N2=trace(Q*(Cz’*Rzz*Cz))%
N3=trace(P*(Bw*Rww*Bw’))%
N4=trace(Q*(K’*Ruu*K))%
Stochastic Simulation
• Consider the linearized longitudinal dynamics of a hypothetical heli
copter. The model of the helicopter requires four state variables:
– θ(t):fuselage pitch angle (radians)
– q(t):pitch rate (radians/second)
– u(t):horizontal velocity of CG (meters/second)
– x(t):horizontal distance of CG from desired hover (meters)
ẋ(t) = u(t)
• Augment the noise model, and using the same control gains, form the
closed-loop system which includes the wind disturbance w(t) as part
of the state vector.
• Solve necessary Lyapunov equations to determine the (steady-state)
variance of the position hover error, x(t) and rotor angle δ(t).
– Without feedforward:
� �
2
E[x ] = 0.048 E[δ 2] = 0.017
• Then design a LQR for the augmented system and repeat the process.
– With feedforward:
� �
2
E[x ] = 0.0019 E[δ 2] = 0.0168
3 % Jon How
4 %
10 Cz = [0 0 0 1];
11 Rxx = Cz’*Cz;
12 rho = 5;
13 Rww=1;
14
15 % lqr control
16 [K,S,E]=lqr(A,Bu,Rxx,rho);
17 [K2,S,E]=lqr(A,Bu,Rxx,10*rho);
18 [K3,S,E]=lqr(A,Bu,Rxx,rho/10);
19
20 % initial response with given x0
21 x0 = [0 0 0 1]’;
23 tf=8;t=0:Ts:tf;
24 [y,x] = initial(A-Bu*K,zeros(4,1),Cz,0,x0,t);
25 [y2,x2] = initial(A-Bu*K2,zeros(4,1),Cz,0,x0,t);
26 [y3,x3] = initial(A-Bu*K3,zeros(4,1),Cz,0,x0,t);
30 axes(h)
32 xlabel(’Time’), ylabel(’\delta’)
34
35 % shaping filter
36 Ah=-0.2;Bh=6;Ch=1;
39 Bua = [Bu;0];
47
48 zeta = sqrt(Rww/Ts)*randn(length(t),1); % discrete equivalent noise
50 %
52 %
56
57 % disturbance FF
59 Acl=Aa-Bua*KK;
60 PP=lyap(Acl,Bwa*Rww*Bwa’);
61 vxa = Cza*PP*Cza’;
62 vda = KK*PP*KK’;
67
68 figure(2);
69 subplot(211)
70 plot(t,y,’LineWidth’,2)
71 hold on;
72 plot(t,dy,’r-.’,’LineWidth’,1.5)
74 hold off
75 xlabel(’Time’);ylabel(’y(t)’);legend(’cts’,’disc’)
77 subplot(212)
78 plot(t,u,’LineWidth’,2)
79 xlabel(’Time’);ylabel(’u(t)’);legend(’No FF’)
80 hold on;
82 hold off
83
84 figure(3);
85 subplot(211)
86 plot(t,ya,’LineWidth’,2)
87 hold on;
88 plot(t,dya,’r-.’,’LineWidth’,1.5)
90 hold off
91 xlabel(’Time’);ylabel(’y(t)’);legend(’cts’,’disc’)
93 subplot(212)
94 plot(t,ua,’LineWidth’,2)
95 xlabel(’Time’);ylabel(’u(t)’);legend(’with FF’)
96 hold on;
98 hold off
99
Helicopter LQG
3 % Jon How
4 %
6 set(0,’DefaultAxesFontName’,’arial’)
7 set(0,’DefaultAxesFontSize’,12)
8 set(0,’DefaultTextFontName’,’arial’)
13 Cz = [0 0 0 1];
15 rho = 5;
16 % lqr control
17 [K,S,E]=lqr(A,Bu,Rxx,rho);
18
19 % initial response with given x0
20 x0 = [0 0 0 1]’;
22 tf=20;t=0:Ts:tf;nt=length(t);
24 Ah=-0.2;Bh=6;Ch=1;
27 Bua = [Bu;0];
30 x0a=[x0;0];
33
34 %%%% Now consider disturbance FF
36 Acl=Aa-Bua*KK;
37 PP=lyap(Acl,Bwa*Rww*Bwa’);
40 %
45
46 %%%% Now consider Output Feedback Case
49 FULL=1;
50 if FULL
54 Cya=[Cy [0;0]];
55 end
56 Ncy=size(Cya,1);Rvv=(1e-2)^2*eye(Ncy);
61 Ccl_lqg=[Cza zeros(1,5)];Dcl_lqg=zeros(1,1+Ncy);
62 x0_lqg=[x0a;zeros(5,1)];
63 zeta_lqg=zeta;
64 % now just treat this as a system with more sensor noise acting as more
65 % process noise
66 for ii=1:Ncy
68 end
72 ua_lqg = [zeros(1,5) KK]*xa_lqg’; % find control commands given the state estimate
73
76 vx_lqg=Ccl_lqg*X_lqg*Ccl_lqg’;
78
79 figure(3);clf
80 subplot(211)
81 plot(t,ya,’LineWidth’,3)
82 hold on;
83 plot(t,dya,’r-.’,’LineWidth’,2)
85 hold off
86 xlabel(’Time’);ylabel(’y(t)’);legend(’cts’,’disc’)
88 subplot(212)
89 plot(t,ua,’LineWidth’,2)
90 xlabel(’Time’);ylabel(’u(t)’);legend(’with FF’)
91 hold on;
94 hold off
96
97 figure(4);clf
98 subplot(211)
99 plot(t,ya_lqg,’LineWidth’,3)
2. Can also design an LQG controller for this system by assuming that
Bw = Bu and Cz = Cy , and then tuning Ruu and Rvv to get a
reasonably balanced performance.
– Took Rww = 0.1 and tuned Rvv
Pole−Zero Map
4
lead
LQG
1
Imaginary Axis
−1
−2
−3
−4
−4 −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0
Real Axis
4.5
3.5
3
Imaginary Axis
2.5
1.5
0.5
4.5
3.5
3
Imaginary Axis
2.5
1.5
0.5
Figure 12.7: B747: root locus (Lead on left, LQG on right shown as a function of
the overall compensator gain)
3. Compare the Bode plots of the lead compensator and LQG designs
2
10
G
Gclead
Gclqg
0
10
−2
10 −3 −2 −1 0 1
10 10 10 10 10
Freq (rad/sec)
100
G
Gclead
0 Gclqg
−100
−200 −3 −2 −1 0 1
10 10 10 10 10
Freq (rad/sec)
2
10
G
Looplead
Looplqg
0
10
−2
10 −3 −2 −1 0 1
10 10 10 10 10
Freq (rad/sec)
100
G
Looplead
0
Looplqg
−100
−200
−300 −3 −2 −1 0 1
10 10 10 10 10
Freq (rad/sec)
1
10
0
10
−1
10
−2
10
−3
10
−4
10 −3 −2 −1 0 1
10 10 10 10 10
Freq (rad/sec)
0.5
−0.5
0 1 2 3 4 5 6 7 8 9 10
Time
4
Gclead
Gclqg
2
u(t)
−2
0 1 2 3 4 5 6 7 8 9 10
Time
B747 LQG
3 %
4 clear all
5 set(0,’DefaultAxesFontName’,’arial’)
6 set(0,’DefaultAxesFontSize’,12)
7 set(0,’DefaultTextFontName’,’arial’)
8
9 gn=1.16*conv([1 .0113],[1 .295]);
13
14 f=logspace(-3,1,300);
15 g=freqresp(gn,gd,2*pi*f*sqrt(-1));
16
17 [nc,dc]=cloop(conv(gn,kn),conv(gd,kd)); % CLP with lead
19 %roots(dc)
20 %loglog(f,abs([g gc]))
21
22 %get state space model
23 [a,b,c,d]=tf2ss(gn,gd);
25 % take y=z
26 Rzz=1;Ruu=0.01;Rww=0.1;Rvv=0.01;
27 [k,P,e1] = lqr(a,b,c’*Rzz*c,Ruu);
28 [l,Q,e2] = lqe(a,b,c,Rww,Rvv);
29 [ac,bc,cc,tdc] = reg(a,b,c,d,k,l);
30 [knl,kdl]=ss2tf(ac,bc,cc,tdc);
31 N1=trace(P*(l*Rvv*l’))%
32 N2=trace(Q*(c’*Rzz*c))%
33 N3=trace(P*(b*Rww*b’))%
34 N4=trace(Q*(k’*Ruu*k))%
36
37 [ncl,dcl]=cloop(conv(gn,knl),conv(gd,kdl)); % CLP with lqg
39 [[roots(dc);0;0;0] roots(dcl)]
40 figure(2);clf;
42 setlines(2)
43 legend(’G’,’Gcl_{lead}’,’Gcl_{lqg}’)
44 xlabel(’Freq (rad/sec)’)
45
46 Gclead=freqresp(kn,kd,2*pi*f*sqrt(-1));
47 Gclqg=freqresp(knl,kdl,2*pi*f*sqrt(-1));
48
49 figure(3);clf;
50 subplot(211)
52 setlines(2)
53 legend(’G’,’Gc_{lead}’,’Gc_{lqg}’)
54 xlabel(’Freq (rad/sec)’)
56 subplot(212)
57 semilogx(f,180/pi*unwrap(phase([g])));hold on
58 semilogx(f,180/pi*unwrap(phase([Gclead])),’g’)
59 semilogx(f,180/pi*unwrap(phase([Gclqg])),’r’)
60 xlabel(’Freq (rad/sec)’)
61 hold off
62 setlines(2)
63 legend(’G’,’Gc_{lead}’,’Gc_{lqg}’)
64
65 figure(6);clf;
66 subplot(211)
68 setlines(2)
69 legend(’G’,’Loop_{lead}’,’Loop_{lqg}’)
70 xlabel(’Freq (rad/sec)’)
72 subplot(212)
73 semilogx(f,180/pi*unwrap(phase([g])));hold on
74 semilogx(f,180/pi*unwrap(phase([g.*Gclead])),’g’)
75 semilogx(f,180/pi*unwrap(phase([g.*Gclqg])),’r’)
76 xlabel(’Freq (rad/sec)’)
77 hold off
78 setlines(2)
79 legend(’G’,’Loop_{lead}’,’Loop_{lqg}’)
80
81 % RL of 2 closed-loop systems
88
89 % time simulations
90 Ts=0.01;
91 [y1,x,t]=impulse(gn,gd,[0:Ts:10]);
92 [y2]=impulse(nc,dc,t);
93 [y3]=impulse(ncl,dcl,t);
96
97 figure(5);clf;
98 subplot(211)
99 plot(t,[y1 y2 y3])
100 xlabel(’Time’)
101 ylabel(’y(t)’)
102 setlines(2)
103 legend(’G’,’Gcl_{lead}’,’Gcl_{lqg}’)
104 subplot(212)
105 plot(t,[ulead ulqg])
106 xlabel(’Time’)
107 ylabel(’u(t)’)
108 setlines(2)
109 legend(’Gc_{lead}’,’Gc_{lqg}’)
110
111 figure(7)
112 pzmap(tf(kn,kd),’g’,tf(knl,kdl),’r’)
113 legend(’lead’,’LQG’)
114
115 print -depsc -f1 b747_1.eps;jpdf(’b747_1’)
116 print -depsc -f2 b747_2.eps;jpdf(’b747_2’)
117 print -depsc -f3 b747_3.eps;jpdf(’b747_3’)
118 print -depsc -f4 b747_4.eps;jpdf(’b747_4’)
119 print -depsc -f5 b747_5.eps;jpdf(’b747_5’)
120 print -depsc -f6 b747_6.eps;jpdf(’b747_6’)
121 print -depsc -f7 b747_7.eps;jpdf(’b747_7’)
122