Linear-Quadratic Control

Jenny Hong Nicholas Moehle Stephen Boyd

EE103
Stanford University

November 30, 2016

 Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Linear dynamical system 2

 Linear dynamical system

xt+1 = Axt + But , t = 1, 2, . . .

I n-vector xt is state at time t

I m-vector ut is input at time t
I n × n matrix A is dynamics matrix
I n × m matrix B is input matrix
I sequence x1 , x2 , . . . is called state trajectory

Linear dynamical system 3

 Simulation

I given x1 , u1 , u2 , . . . find x2 , x3 , . . .
I can be done by recursion: for t = 1, 2, . . .,

xt+1 = Axt + But

Linear dynamical system 4

 Vehicle example

consider a vehicle moving in a plane:

I sample position and velocity at times τ = 0, h, 2h, . . .
I 2-vectors pt and vt are position and velocity at time ht
I 2-vector ut gives applied force on the vehicle time ht
I friction force is −ηvt
I vehicle has mass m
I for small h,
vt+1 − vt pt+1 − pt
m ≈ −ηvt + ut , ≈ vt
h h
I we use approximate state update

vt+1 = (1 − hη/m)vt + (h/m)ut , pt+1 = pt + hvt

Linear dynamical system 5

 I vehicle state is 4-vector xt = (pt , vt )
I dynamics recursion is

xt+1 = Axt + But ,

where
   
1 0 h 0 0 0
 0 1 0 h   0 0 
A=
 , B= 
0 0 1 − hη/m 0   h/m 0 
0 0 0 1 − hη/m 0 h/m

Linear dynamical system 6

 Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Control 7
 Control

I x1 is given
I choose u1 , u2 , . . . , uT −1 to achieve some goals, e.g.,
– terminal state should have some fixed value: xT = xdes
– u1 , u2 , . . . , uT −1 should be small, say measured as

ku1 k2 + · · · + kuT −1 k2

(sometimes called ‘energy’)

I many control problems are linearly constrained least-squares
problems

Control 8
 Minimum-energy state transfer

I given initial state x1 and desired final state xdes

I choose u1 , . . . , uT −1 to minimize ‘energy’

minimize ku1 k2 + · · · + kuT −1 k2

subject to xt+1 = Axt + But , t = 1, . . . , T − 1
xT = xdes

variables are x2 , . . . , xT , u1 , . . . , uT −1
I roughly speaking: find minimum energy inputs that steer the state
to given target state over T periods

Control 9
 State transfer example

vehicle model with T = 100, x1 = (10, 10, 10, −5), xdes = 0

10

y
0

-5
0 5 10 15 20 25

Control 10
 4
2

u 0 u1
u2
-2
-4
0 50 100

t
30
20
x1
x 10 x2
x3
0
x4
-10
0 50 100

Control 11
 Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Variations 12
 Output tracking

I yt = Cxt is output (e.g., position)

I yt should follow a desired trajectory, i.e., sum square tracking error

ky2 − y2des k2 + · · · + kyT − yTdes k2

should be small
I the output tracking problem is
PT PT −1
minimize t=2kyt − ytdes k2 + ρ t=1 kut k2
subject to xt+1 = Axt + But , t = 1, . . . , T − 1
yt = Cxt , t = 1, . . . , T − 1

variables are x2 , . . . , xT , u1 , . . . , uT −1 , y2 , . . . , yT
I parameter ρ > 0 trades off control ‘energy’ and tracking error

Variations 13
 Output tracking example

vehicle model with T = 100, ρ = 0.1, x1 = 0, yt = pt (position

tracking)
6

y 3 p
p desired
2

0
0 1 2 3 4 5 6

Variations 14
 50
40
30
u 20 u1
10 u2
0
-10
0 50 100

t
6
5
4
x 3 x1
2 x2
1
0
0 50 100

Variations 15
 Waypoints

I using output, can specify waypoints

I specify output (position) w(k) at time tk at K total places

minimize ku1 k2 + · · · + kuT −1 k2

subject to xt+1 = Axt + But , t = 1, . . . , T − 1
Cxtk = w(k) , k = 1, . . . , K

variables are x2 , . . . , xT , u1 , . . . , uT −1

Variations 16
 Waypoints example

I vehicle model
I T = 100, x1 = (10, 10, 20, 0), xdes = 0
I K = 4, t1 = 10, t2 = 30, t3 = 40, t4 = 80
       
3.5 1.5 4.5 0
I w(1) = , w(2) = , w(3) = , w(4) =
3.5 4 1 0

Variations 17
 Waypoints example

2
y
0

-2

-4
0 2 4 6 8

Variations 18
 10
5
0
u -5
u1
u2
-10
-15
0 50 100

t
10

5 x1
x x2
0 waypoints
waypoints
-5
0 50 100

Variations 19
 Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Examples 20
 Rendezvous

I we control two vehicles with dynamics

xt+1 = Axt + But , zt+1 = Azt + Bvt

I final relative state constraint xT = zT

I formulate as state transfer problem:
PT −1 2 2
minimize t=1 (kut k + kvt k )
subject to xt+1 = Axt + But , t = 1, . . . , T − 1,
zt+1 = Azt + Bvt , t = 1, . . . , T − 1,
xT = zT

variables are x2 , . . . , xT , u1 , . . . , uT −1 , z2 , . . . , zT , v1 , . . . , vT −1

Examples 21
 Rendezvous example

x1 = (0, 0, 0, −5), z1 = (10, 10, 5, 0)

10

0
y
-5

-10

-15
0 5 10 15 20 25

Examples 22
 Formation

I generalize rendezvous example to several vehicles

I final position for each vehicle defined relative to others (e.g., relative
to a ’leader’)
I leader has a final velocity constraint

Examples 23
 Formation example

40

30

20
y
10

-10
-5 0 5 10 15

Examples 24
 Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Linear quadratic regulator 25

 Linear quadratic regulator

I minimize energy while driving state to the origin:

PT PT −1
minimize t=2 kxt k2 + ρ t=1 kut k2
subject to xt+1 = Axt + But , t = 1, . . . , T − 1

variables are x2 , . . . , xT , u1 , . . . , uT −1
PT 2
t=2 kxt k is (sum square) regulation
I

I x = 0 is some desired (equilibrium, target) state

I parameter ρ > 0 trades off regulation versus control ‘energy’

Linear quadratic regulator 26

 I LQR problem is a linearly constrained least-squares problem:

minimize kF zk2
subject to Gz = d

I variable z is (x2 , . . . , xT , u1 , . . . , uT −1 )
I F , G depend on A, B, ρ; d depends (linearly) on x1
I solution is ẑ = Hd for some H
I optimal first input û1 is a linear function of x1 , i.e.,

û1 = Kx1

for some m × n matrix K (called LQR gain matrix)

I finding K involves taking correct ‘slice’ of inverse KKT matrix
I entries of K depend on horizon T , and converge as T grows large

Linear quadratic regulator 27

 State feedback control

I find K for LQR problem (with large T )

I for each t,
– measure state xt
– implement control ut = Kxt
I with ut = Kxt is called state feedback control policy
I combine with (‘open-loop dynamics’) xt+1 = Axt + But to get
closed-loop dynamics

xt+1 = (A + BK)xt

I we can simulate open- and closed-loop dynamics to compare

Linear quadratic regulator 28

 Example: longitudinal flight control

body axis
theta
horiz

variables are (small) deviations from operating point or trim conditions;

state is xt = (wt , vt , θt , qt ):
I wt : velocity of aircraft along body axis
I vt : velocity of aircraft perpendicular to body axis (down is positive)
I θt : angle between body axis and horizontal (up is positive)
I qt = θ̇t : angular velocity of aircraft (pitch rate)
input is ut = (et , ft ):
I et : elevator angle (et > 0 is down)
I ft : thrust

Linear quadratic regulator 29

 Linearized dynamics

for 747, level flight, 40000 ft, 774 ft/sec, dynamics are
xt+1 = Axt + But , where
   
.99 .03 −.02 −.32 0.01 0.99
 .01 .47 4.7 .00   −3.44 1.66 
A=  .02 −.06 .40 −.00  ,
 B=  −0.83 0.44


.01 −.04 .72 .99 −0.47 0.25

I units: ft, sec, crad (= 0.01rad ≈ 0.57◦ )

I discretization is 1 sec

Linear quadratic regulator 30

 LQR gain

gain matrix K converged for T ≈ 30

0.4

0.3

0.2

K 0.1

0.0

-0.1

-0.2
0 50 100

Linear quadratic regulator 31

 LQR for 747 model

I LQR gain matrix (for T = 100, ρ = 100) is:

 
−.038 .021 .319 −.270
K=
−.061 −.004 −.120 .007

I e.g., K14 = −.27 is gain from pitch rate ((xt )4 ) to elevator angle
((ut )1 )

Linear quadratic regulator 32

 747 simulation

ut = 0 (’open loop’)

u 0

0 50 100

x 0

-5
0 50 100

Linear quadratic regulator 33

 747 simulation

ut = Kxt (’closed loop’)

0.3
0.2

u 0.1
0.0
-0.1
0 50 100

1
0

x -1
-2
-3
0 50 100

Linear quadratic regulator 34