Optimal Control for Engineers
Optimal Control for Engineers
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