midterm exam solutions - ee263.stanford.eduee263.stanford.edu/exams/midterm_sol_su18.pdf · midterm...

EE263, Summer 2017-18July 20-23, 2018

Midterm exam solutions

This is a 12 hour take-home midterm with 3 problems. Please submit your solutions toGradescope at most 12 hours after you receive the exam.

• You may use any books, notes, or computer programs (e.g., MATLAB), but you maynot discuss the exam with others until July 24, after everyone has taken the exam andthe solutions are posted. The only exception is that you can ask the course staff forclarification, by emailing the staff [k2shah,mkossyre]@stanford.edu. We have triedpretty hard to make the exam unambiguous and clear, so we’re unlikely to say much.Please do not post any exam related questions on Piazza.

• Since you have 12 hours, we expect your solutions to be legible, neat, and clear. Donot submit your rough notes, and please try to simplify your solutions as much as youcan. We will deduct points from solutions that are technically correct, but much morecomplicated than they need to be.

• Please check your email a few times during the exam, just in case we need to send outa clarification or other announcements. It’s unlikely we will need to do this, but younever know.

• If a problem asks for some specific answers, make sure they are obvious in your solu-tions. You might put a box around the answers, so they stand out from the surroundingdiscussion, justification, plots, etc.

• When a problem involves some computation (say, using MATLAB), we do not wantjust the final answers. We want a clear discussion and justification of exactly what youdid, the well-commented MATLAB source code that produces the result, and the finalnumerical result or plots. Be sure to show us your verification that your computedsolution satisfies whatever properties it is supposed to, at least up to numerical preci-sion. For example, if you compute a vector x that is supposed to satisfy Ax = b (say),show us the matlab code that checks this, and the result. (This might be done by thematlab code norm(A*x-b); be sure to show us the result, which should be very small.)We will not check your numerical solutions for you, in cases where there is more thanone solution.

• In the portion of your solutions where you explain the mathematical approach, youcannot refer to MATLAB operators, such as the backslash operator. (You can, ofcourse, refer to inverses of matrices, or any other standard mathematical construct.)

1

• Some of the problems are described in a practical setting, such as computational Biol-ogy or machine learning. You do not need to understand anything about the applicationarea to solve these problems. We have taken special care to make sure all the informa-tion and math needed to solve the problem is given in the problem description.

• Some of the problems require you to download and run a matlab file to generate thedata needed. These files can be found in the zip attached with your exam

• Please respect the honor code. Although we encourage you to work on homeworkassignments in small groups, you cannot discuss the midterm with anyone, with theexception of EE263 course staff, until July 24, when everyone has taken it and thesolutions are posted online.

2

1. Stretch Before Exercise

(a) If A ∈ Rn×m is full rank and m > n does (AAT )−1 exist? Prove or give a counterexample.

(b) Suppose the set {a1, ..., ak} is linearly independent where ai ∈ Rn and x ∈ Rn.Let A = [a1, ..., ak], show that if {a1 + x, ..., ak + x} is linearly dependent thenx ∈ span(a1, ..., ak).

(c) Let x, y ∈ Rn, show that xTy = 0 if and only if ||x|| ≤ ||x+ λy|| ∀λ ∈ R

Solution.

(a) There are many ways to show this fact, the approach here is to show that AAT

is full rank, which implies invertability because it’s square.

Showing that AAT is full rank is equivalent to showing that it is onto, i.e ∀ythere is b such that AAT b = y. Now lets AT b = x.

Since A is full rank and wide it is onto. Therefore for all y there always existsx such that for Ax = y, Moreover there will exist an x ∈ R(AT ) because for anyother x where Ax = y there is some z ∈ Null(A) such that x + z = x sinceNull(A) and R(AT ) are orthogonal, we can always find an x ∈ R(AT ).

Now since x ∈ R(AT ), but definition there is some b such that x = AT b. Weare done! We have shown the square matrix AAT is onto and therefore full rankand therefore inevitable.

(b) Since {a1 + x, ..., ak + x} is linearly dependent there exists a not zero set ofz1...zk ∈ R such that

k∑i=1

zi(ai + x) = 0

k∑i=1

zi ai = −xk∑i=1

zi

Now it is tempting to just divide both sides by −∑k

i=1 zi and say that x is a linearcombination of the ai where each weight is − zi∑k

i=1 ziand stop there

This is correct with one caveat, we must verify that∑k

i=1 zi 6= 0, because we can’t

divide by zero. If∑k

i=1 zi = 0 then there would be a set of non zero weights such

that∑k

i=1 ziai = 0 but that is not possible since the set {a1, ..., ak} is linearly

independent. Therefore∑k

i=1 zi 6= 0 must be true.

3

(c) if xTy = 0 we have

||x|| ≤ ||x+ λy||||x||2 ≤ ||x+ λy||2

xTx ≤ xTx+ λ2yTy

0 ≤ λ2yTy

which is true for all λ since everything is positive

The opposite direction is a little more involved to prove we want to show thatif ||x|| ≤ ||x + λy|| ∀λ then xTy = 0. However, this statement is equivalent toshowing that if xTy 6= 0 then there is at least one λ such that ||x|| ≥ ||x+ λy||.What this is saying is: if xTy 6= 0 then I can find some λ that violates the desiredinequality, thus xTy = 0 must be true in order for the desired inequality to holdfor all λ.

The geometric idea behind this is that if two vectors are not orthogonal, then wecan find some scalar λ such that x+λy is closer to the origin (smaller norm) thanx is.

||x||2 ≤ ||x+ λy||2

xTx ≤ xTx+ λ2yTy + 2λyTx

0 ≤ λ2yTy + 2λyTx

this is equivalent to the original premise. Observe that if we set λ = −yT xyT y

andsubstitute we obtain

0 ≤ (yTx)2

yTy− 2

(yTx)2

yTy

0 ≤ −(yTx)2

yTy

Now if yTx 6= 0 then this choice of λ would violate the inequality, thus yTx = 0if ||x|| ≤ ||x+ λy|| ∀λ.

How did we magically guess λ = −yT xyT y

? Well, going back to the geometric ideamentioned above we can form an optimization problem

minimizeλ

||x+ λy||2

and solve by finding the gradient

||x+ λy||2 = xx + λy2Ty + 2λyTx

∇λ = 2λyTy + 2yTx = 0

=⇒ λ = −yTx

yTy

4

2. Avast, Hard to Port!

AHOY! Ye the captain of the mightiest of fine vessels have been given a trade contractrequiring you to visit several island towns over the course of the day. Your crew isexcited for you to lead them on a wondrous journey of sea-faring, sea-shanties, andsea-math (which is just normal math, but on the sea).

Given these island way-points {w1, ..., wn}, wj ∈ R2 and corresponding target times(in hours) {k1, ..., kn}. Your goal is to plan a path for your trusty vessel to be at thewaypoints at the target times. Let p(t) ∈ R2 be the path that the boat will take, i.e.

p(t) =

[px(t)py(t)

]Thus px(5) gives the x-coordinate of the position at t = 5. In order for the path to bedynamically feasible it must not only go through all the way-points at the correspond-ing target times, but also the velocity and acceleration must be continuous along thepath p(t). In order to achieve this, we restrict the path p(t) to be a series of piece-wise cubic polynomial function, where each polynomial joins a set of 2 way-points.Mathematically we have:

p(t) = pi(t) for ki ≤ t ≤ ki+1, i = 1, 2, · · · , (n− 1),

where

pi(t) =

[aixaiy

]t3 +

[bixbiy

]t2 +

[cixciy

]t+

[dixdiy

].

To ensure the position p(t), velocity p(t) and acceleration p(t) of the vessel are contin-uous, we should have:

pi(ki) = wi and pi(ki+1) = wi+1 ∀i ∈ {1, ...., n− 1}.

We also need:

pi(ki+1) = pi+1(ki+1) and pi(ki+1) = pi+1(ki+1) ∀i ∈ {1, ...., n− 2}

where ki+1 is the shared way-point time between polynomials pi and pi+1.

Lastly, we also want to minimize the RMS jerk, or third derivative of the path as sharpturns will cause a capsize (recall the ancient sea proverb: no boat, no float).

J1 =

(1

kn − k1

∫ kn

k1

‖...p x(t)‖2 + ‖

...p y(t)‖2dt

)1/2

,

where triple dots denotes the third derivative. Finally we get to the problem. Youneed to find the polynomial coefficients for pi(t)’s such that the cost function J1 isminimized and all the constraints are satisfied.

5

(a) Cast this problem as an optimization problem with a cost function and constraints.Your cost function should not be in terms of any integrals. We advise you todevelop a single matrix equation for the constraints.

(b) Using the setup of part (a), implement your solution with the waypoints and time.Note: Latitude is the y-coordinate and Longitude is the x-coordinate. All of thedata for this problem is in boat.json

Port Latitude Longitude Time (h)

Havana, Cuba 23.109157 -82.32847 0Kingston, Jamaica 18.0179332 -76.835675 2Port-Au-Prince, Haiti 18.594395, -72.307433 5Aruba 12.521110, -69.968338 7San Juan, Puerto Rico 18.465539, -66.105735 10Anguilla 18.220554, -63.068615 12Castries 14.510109, -60.987469 19Port of Spain 10.654901, -61.501926 22

Report the following plots:

• The trajectory overlayed on the waypoints

• The velocity and acceleration over time (they should be continuous)

Also report your final minimized RMS jerk value J .

(c) Now that you have your path, we must find the control sequence that wouldapproximately achieve this trajectory. The model of our boat is given by, thediscretized equations of motion

x(z + 1) =

1 0 .2 00 1 0 .20 0 .8 00 0 0 .8

x(z) +

.2 −.2 0.05 .05 .3.7 −.7 −.1−.1 −.1 .9

u(z), z ∈ Z

where x1(z) and x2(z) are the position position of the vessel and and x3(z), x4(z)are the velocity of the vessel in the x-direction and y-direction, respectively, withcontrol u(t) ∈ R3. Due to the limitations of the craft we can only apply controlevery .2 hours, thus we discretize our control sequence u(z) where t = .2z forz = 0, ..., bkn/.2c − 1.

Your task is to extract the position and velocity values along your path you foundin part (b) and use them to form an optimization problem that minimizes thefollowing function.

J2 =

kn/.2∑z=1

||p(.2z)−H1,2X(z)||2 + γ||p(.2z)−H3,4X(z)||2

6

subject to the dynamics requirement. H1,2 is a matrix that returns the 1st tworows of x(z). Likewise, H3,4 returns that last two rows, this is just a easy way toget the positions and velocities separately from the state vector x.

H1,2 =

[1 0 0 00 1 0 0

]Set the initial state x(0) to be equal to what you found in part (b). Plot u againstz, it should be 3 curves on one plot, also return the value J2 let γ = 1.

(d) Forward simulate your system for two scenarios

• with the control you found in part (c)

• with noise ν which is added to the same control (let ν ∼ N(0, I3) ∗ .2).

Plot both against trajectories along with the planned path from part (b). Whatdo you observe? Briefly explain why.

(e) Since you are captain now, what would you name your boat?

Solution.

(a) [6 points] We begin by realizing we will have m = n − 1 polynomials. In orderfor the positions to pass the way points we have that

pi(ki) = wi and pi(ki+1) = wi+1 ∀i ∈ {1, ....,m}.

also we note that the velocities and accelerations needs to match so

3ajk2j + 2bjkj + cj = 2aj+1k

2j + 2bj+1kj + cj+1

6ajkj + 2bj = 6aj+1kj + 2bj+1

j = 1....m− 1

We can consider each coordinate separately, First we stack all the coefficients intoa single vector z

a1b1c1d1a2b2c2d2...ambmcmdm

7

Next we build a matrix equation Gz = h where

G =

k31 k21 k1 1 0 . . . . . . . . . . . . 0 0 0 0k32 k22 k2 1 0 . . . . . . . . . . . . 0 0 0 03k21 2k1 1 0 −3k22 −2k2 −1 0 . . . 0 0 0 06k21 2 0 0 −6k2 −2 0 0 . . . 0 0 0 00 . . . . . . . . . . . . . . . . . . . . . . . . k3n−1 k2n−1 kn−1 10 . . . . . . . . . . . . . . . . . . . . . . . . k3n k2n kn 1

and

h =

w1

w2

00...

wn−1

wn

Now we have a linear constraint, lets consider the cost function. We can squarethe function separate the x and y coordinates, for the x-coordinate we have

Jx =

∫ kn

k1

‖...px(t)‖2dt

=m∑j=1

((6aj,x)2(kj+1 − kj))

= zTFz

One of the keys to this problem was realizing a few things

• The polynomials for each co-ordinate could be found separately

• Each polynomials has 4 coefficients, so there are 4m free variables and 2m+m−1+m−1 = 4m−2 waypoints+velocity+ acceleration constraints. Indeedwe have more variables than constraints thus our problem is well posed.

Where

F = 62

k2 − k1 0 0 0 . . . . . . . . . . . . . . . . . . . . . . . . 0

0 . . . . . . . . . k3 − k2 0 0 0 . . . . . . . . . . . . 0...

......

......

......

......

......

......

0 . . . . . . . . . . . . . . . . . . . . . . . . kn − kn−1 0 0 0

Our problem is now

minimize zTFz

subject to Gz = h

8

We can set up the Lagrangian and solve the following[F GT

G 0

] [zx zyλx λy

]=

[0 0hx hy

](b) [5.5 points] The optimal cost is 1.64.

Figure 1: planned trajectory

9

Figure 2: velocity and acceleration

code that solves this problem

us ing PyPlotus ing JSON

10

#dataW raw=[23.109157 −82.328476;

18.0179332 −76.8356757;18.594395 −72.307433;12.521110 −69.968338;18.465539 −66.105735;18.220554 −63.068615;14.510109 −60.987469;10.654901 −61.501926]

#waypointswpx=W raw [ : , 2 ] ; wpy=W raw [ : , 1 ]w=[W raw [ : , 2 ] ; W raw [ : , 1 ] ]

#waypoint t imesk =[0 .0 , 2 , 5 , 7 , 10 , 12 , 19 , 2 2 ] ;

#constant snWaypoints=length ( k )dt =.2dimControl=3dimState=4gamma=1

@show npoly=length (wpx)−1;

Gpos=ze ro s ( npoly ∗2 , npoly ∗4 ) ;Gvel=ze ro s ( npoly−1, npoly ∗4 ) ;Gacc=ze ro s ( npoly−1, npoly ∗4 ) ;

hx=ze ro s ( npoly ∗2 , 1 ) ;hy=ze ro s ( npoly ∗2 , 1 ) ;

F=ze ro s ( npoly , npoly ∗4 ) ;

f o r i =1: npoly#cos tF [ i , ( i −1)∗4+1]=6∗ s q r t ( ( k [ i +1]−k [ i ] ) ) ;#p o s i t i o n

11

#s t a r tGpos [1+( i −1)∗2 , 1+( i −1)∗4:4+( i −1)∗4]=[k [ i ] ˆ3 k [ i ] ˆ2 k [ i ] 1 ] ;hx [1+( i −1)∗2]=wpx [ i ] ;hy [1+( i −1)∗2]=wpy [ i ] ;#endGpos [2+( i −1)∗2 , 1+( i −1)∗4:4+( i −1)∗4]=[k [ i +1]ˆ3 k [ i +1]ˆ2 k [ i +1] 1 ] ;hx [2+( i −1)∗2]=wpx [ i +1] ;hy [2+( i −1)∗2]=wpy [ i +1] ;

#v e l o c i t yi f i==npoly

cont inueendt=k [ i +1]Gvel [ i , (1+( i −1)∗4):(8+( i −1)∗4)]=[3∗ t ˆ2 2∗ t 1 0 −3∗t ˆ2 −2∗t −1 0 ] ;#a c c e lGacc [ i , (1+( i −1)∗4):(8+( i −1)∗4)]=[6∗ t 2 0 0 −6∗t −2 0 0 ] ;

end#stack em upG=[Gpos ; Gvel ; Gacc ] ;hx=[hx ; z e r o s (2∗ npoly −2 ,1 ) ] ;hy=[hy ; z e r o s (2∗ npoly −2 ,1 ) ] ;#s o l v eGl=[F’∗F G’ ; G ze ro s ( npoly∗4−2, npoly ∗4−2)] ;#get c o f f sxpoly=Gl ’∗ inv ( Gl∗Gl ’ ) ∗ [ z e r o s ( npoly ∗4 , 1 ) ; hx ] ;ypoly=Gl ’∗ inv ( Gl∗Gl ’ ) ∗ [ z e r o s ( npoly ∗4 , 1 ) ; hy ] ;#t o s s lagrangexpoly=xpoly [ 1 : npoly ∗4 ]ypoly=ypoly [ 1 : npoly ∗4 ]#c a l c u l a t e co s t

@show J=s q r t (1/( k [ end]−k [ 1 ] ) ∗ sum ( [ xpoly ypoly ] ’∗F’∗F∗ [ xpoly ypoly ] ) )

#check v i o l a t i o n s#@show norm(G∗xpoly−hx )#@show norm(G∗ypoly−hy )

# reshape c o f f sxpoly=reshape ( xpoly , 4 , npoly ) ;ypoly=reshape ( ypoly , 4 , npoly ) ;

12

#get path#posdt =.2Trange=c o l l e c t ( 0 : . 2 : k [ end ] ) ;

xPos=ze ro s ( l ength ( Trange ) ) ;yPos=ze ro s ( l ength ( Trange ) ) ;

#ve lxVel=ze ro s ( l ength ( Trange ) ) ;yVel=ze ro s ( l ength ( Trange ) ) ;

#a c c e lxAcc=ze ro s ( l ength ( Trange ) ) ;yAcc=ze ro s ( l ength ( Trange ) ) ;

i =1;f o r ( tIdx , t ) in enumerate ( Trange )

i f t>k [ i +1]i+=1

end#eva l poly

xPos [ t Idx ]=dot ( xpoly [ : , i ] , [ t ˆ3 , t ˆ2 , t ˆ1 , t ˆ 0 ] )yPos [ t Idx ]=dot ( ypoly [ : , i ] , [ t ˆ3 , t ˆ2 , t ˆ1 , t ˆ 0 ] )

#ve lxVel [ t Idx ]=dot ( xpoly [ : , i ] , [ 3 ∗ t ˆ2 , 2∗ t ˆ1 , t ˆ0 , 0 ] )yVel [ t Idx ]=dot ( ypoly [ : , i ] , [ 3 ∗ t ˆ2 , 2∗ t ˆ1 , t ˆ0 , 0 ] )

#a c c e lxAcc [ t Idx ]=dot ( xpoly [ : , i ] , [ 6 ∗ t ˆ1 , 2∗ t ˆ0 , 0 , 0 ] )yAcc [ t Idx ]=dot ( ypoly [ : , i ] , [ 6 ∗ t ˆ1 , 2∗ t ˆ0 , 0 , 0 ] )

end

p lo t ( xPos , yPos )p l o t (wpx , wpy , ” or ”)t i t l e (” t r a j e c t o r y ”)

13

s a v e f i g (” t r a j . png ”)

d e f a u l t w o r k e r p o o lf i g u r e ( f i g s i z e =(8 ,12))subplot (211)p l o t ( Trange , xVel ) ; p l o t ( Trange , yVel ) ;t i t l e (” Ve loc i ty ” ) ; l egend ( [ ” x ” , ”y ” ] )subplot (212)p l o t ( xAcc ) ; p l o t ( yAcc )t i t l e (” Acce l e r a t i on ” ) ; l egend ( [ ” x ” , ”y ” ] )s a v e f i g (” derv . png ”)

(c) [5.5 points] Once we have found our planned path we can solve for the optimalcontrol, this was very similar to the hovercraft problem. The key here was find-ing the matrix C with the vector y and noting that since γ = 1 we can mergeeverything into once matrix equation and ignore the H matrices. Also we needto account for the drift due to the nonzero initial condition.

C =

B 0 0 . . . 0A1B B 0 · · · 0

......

. . ....

Akn/.2−1B Akn/.2−2B · · · B

y =

p(1)− A1p(0)p(2)− A2p(0)

...p(kn/.2)− Akn/.2p(0)

Finally we solve the optimization problem (least-squares) and get

u∗ = (CTC)−1CTy

as our optimal control.

The optimal cost was J2 = 7.6

14

Figure 3: control

the code that solves this problem

us ing PyPlot

#optimal c o n t r o lA=[1 0 . 2 0 ;

0 1 0 . 2 ;0 0 . 8 0 ;0 0 0 . 8 ] ;

B=[ .2 −.2 0 ;. 05 . 05 . 3. 7 −.7 −.1−.1 −.1 . 9 ] ;

x0=[xPos [ 1 ] , yPos [ 1 ] , xVel [ 1 ] , yVel [ 1 ] ]

15

nControl =3; nState =4; nSteps=Int ( k [ end ] / dt )maxStep=Int ( k [ end ] / dt )@show maxStepC=B;CTM=[B ze ro s ( nState , ( nSteps −(1))∗ nControl ) ] #Control t r a n s i t i o n matrixtarg =[xPos [ 2 ] , yPos [ 2 ] , xVel [ 2 ] , yVel [ 2 ] ] #t a r g e t goa ld r i f t=A∗ [ xPos [ 1 ] , yPos [ 1 ] , xVel [ 1 ] , yVel [ 1 ] ] #d r i f t from ICf o r z=1:maxStep−1

C=A∗CCTM=[CTM;

C CTM[1+( z−1)∗nState : z∗nState , 1 : end−nControl ] ]#t a r g e t s t a t e t r a jta rg =[ targ ; xPos [ z +2] ; yPos [ z +2] ; xVel [ z +2] ; yVel [ z+2] ] #t a r g e t goa ld r i f t =[ d r i f t ; Aˆ( z+1)∗x0 ] #t a r g e t goa l

enduopt=inv (CTM’∗CTM)∗CTM’ ∗ ( targ−d r i f t ) ;#get co s t@show J2=(norm(CTM∗uopt−(targ−d r i f t ) ) ) ˆ2

zRange=c o l l e c t ( 1 : maxStep )uopt=reshape ( uopt , 3 , maxStep )p l o t ( ( zRange−1)∗dt , uopt [ 1 , : ] , ”b”)p l o t ( ( zRange−1)∗dt , uopt [ 2 , : ] , ”g ”)p l o t ( ( zRange−1)∗dt , uopt [ 3 , : ] , ” r ”)

#forward s imulatexPos Sim=[x0 [ 1 ] ] ; yPos Sim=[x0 [ 2 ] ]xVel Sim=[x0 [ 3 ] ] ; yVel Sim=[x0 [ 4 ] ]

x=x0f o r z=1:maxStep−1

x=A∗x+B∗uopt [1+ nControl ∗( z−1): nControl ∗( z ) ]push ! ( xPos Sim , x [ 1 ] )push ! ( yPos Sim , x [ 2 ] )

push ! ( xVel Sim , x [ 3 ] )push ! ( yVel Sim , x [ 4 ] )

end#forward s imulate no i s exPos Sim n=[x0 [ 1 ] ] ; yPos Sim n=[x0 [ 2 ] ]

16

xVel Sim n =[x0 [ 3 ] ] ; yVel Sim n =[x0 [ 4 ] ]x=x0f o r z=1:maxStep−1

x=A∗x+B∗( uopt [1+ nControl ∗( z−1): nControl ∗( z )]+ randn ( nControl ) ∗ . 2 )push ! ( xPos Sim n , x [ 1 ] )push ! ( yPos Sim n , x [ 2 ] )

push ! ( xVel Sim n , x [ 3 ] )push ! ( yVel Sim n , x [ 4 ] )

end

p lo t ( xPos , yPos , ”b”)p l o t (wpx , wpy , ” or ”)#simsp lo t ( xPos Sim , yPos Sim , c o l o r=”g” )p l o t ( xPos Sim n , yPos Sim n , c o l o r=”m”)t i t l e (” t r a j e c t o r y f o l l o w i n g ”)legend ( [ ” plan ” , ” waypoints ” , ”no no i s e ” , ” no i s e ” ] )s a v e f i g (” sim . png ”)

(d) [3 points]

Figure 4: Simulation

the code that runs the simulation

17





end#forward s imulate no i s exPos Sim n=[x0 [ 1 ] ] ; yPos Sim n=[x0 [ 2 ] ]xVel Sim n =[x0 [ 3 ] ] ; yVel Sim n =[x0 [ 4 ] ]x=x0f o r z=1:maxStep−1



end


The key observation here is that the control does not take into a account thestate, it is basically only a function of the discrete time, z goes in and out comesa u. This is called an ”open-loop” control as there is no feedback to adjust thecontrol to compensate for noises to create a ”closed-loop” system. Points wereawarded as long as you mentioned this lack of compensation issue or that werewas no way for the controller to detect if it was off course, simply saying ”the

18

noise version was bad” was not acceptable.

there with how the matlab parse we gave you read the julia made json file as aresult the A matrix was transposed. this was not held against you, the controlplots were similar but your final cost was 184094.345 If this is not rectified in yourscore please submit a regrade request!





end#forward s imulate no i s exPos Sim n=[x0 [ 1 ] ] ; yPos Sim n=[x0 [ 2 ] ]xVel Sim n =[x0 [ 3 ] ] ; yVel Sim n =[x0 [ 4 ] ]x=x0f o r z=1:maxStep−1



end


0 points The most common boat name was Boaty McBoatface

19

3. Nonlinear System Control

In this problem, we’ll be working with a 1-D nonlinear system, which has the followingequation of motion:

x− 5x(1− x2) + x = 0

This type of oscillator has been used in many fields, including biology in the analysis ofneurons, and seismology to model plate activity. Today, we’ll just be trying to controlit.

(a) We define the state as z = [x, x]T . Linearize the system around x = 2, x = −2.Your equation will be of the form z = Az + b. Report A and b (hint: start withz = [f1(z), f2(z)]T ).

(b) Now, discretize the system using a time-step of 0.01. Use forward Euler as yourstate propagation algorithm. Your equation should end up in zk+1 = Adisczk+bdisc.Give the linearized, discretized A matrix and b vector.

(c) Let’s assume we have an actuator that can impart force on the system. Mathe-matically, it just means we have

zk+1 = Azk + b+

[01

]uk

Use the linear, discrete Adisc and bdisc from part (b), and z0 = [2,−2]T . Usingyour favorite numerical computing program, what is the minimum time it wouldtake to bring this system to rest (ie x = x = 0) You may need to use an iterativemethod to find minimum time. If you do, make sure to stop once you’re closeenough to the origin (on the order of 10−6). Once you have it, plot the graph ofx vs x. Include your code in your solution.

(d) In general, for a system zk+1 = Azk + Buk (which isn’t quite the same as oursystem), to be at rest at some time N, what must be true about the vector ANz0in relation to a particular matrix that can be constructed from this system? 1-2sentence explanation with the appropriate form of the matrix will suffice.

(e) Now, part (c)’s graph is not very interesting, so let’s say we want to bring thesystem to rest in 2 seconds, while minimizing ||u||. Plot the new graph of x vs x.Keep this force profile (u outputs) for the next part. Include your code in yoursolution.

(f) What we did above doesn’t actually work for our nonlinear system, but we canget around this using a receding horizon method. First, you set your horizon,which is the amount of time you want to take to get to rest. Do this in discrete-time units. Then, you linearize (and discretize) the system around your currentposition. Then, you calculate the min-energy force profile over the time horizon

20

to get the linear system to rest. Then, throw away all of your u’s except the firstone, ie u = u(1). Then, you take one Euler step forward. ie you do

znew = Adisczcurrent + bdisc +Bu

Then, you decrease your horizon by 1 time-step (this is the receding part). Then,linearize and discretize your system around your new position, calculate your newmin-force profile given your new horizon, take one more Euler step forward, etc.Do this until your horizon is 0, at which point you should be at or near rest.

Plot the graph of x vs x. Keep your force profile for the next part. Include yourcode in your solution.

(g) Using your nonlinear simulator that you developed in part (f), run both the forceprofiles from (e) and (f) (input your force profiles step-by-step in the simulator).Plot the x vs x paths for both profiles on the same graph. What do you see?Why? Which is better? Include your code in your solution.

Solution

(a) We start with the basic linearization shown in the lecture notes, using the JacobianDij = ∂zi

∂zj. We’ll do the general form of the linearization first, since that will be

useful for later. We define zc = [xc, xc]T to be the state around which we’re

linearizing.

z ≈[

xc5(xc)(1− x2c)− xc

]+D

[x− xcx− xc

][xx

]=

[xc

5(xc)(1− x2c)− xc

]+

[0 1

−10xcxc − 1 5(1− x2c)

] [x− xcx− xc

]=

[xc

5(xc)(1− x2c)− xc

]−[

0 1−10xcxc − 1 5(1− x2c)

] [xcxc

]+

[0 1

−10xcxc − 1 5(1− x2c)

] [xx

]=

[0

10xcx2c

]+

[0 1

−10xcxc − 1 5(1− x2c)

] [xx

]In our particular case, this ends up being

z =

[0 139 −15

] [xx

]+

[0−80

](b) Forward Euler is just the current position plus δ times the linear approximation

at the current position.

zk+1 = zk + δz|zk= zk + δ(Azk + b)

= (δA+ I)zk + δb

21

Around our linearization point, and with δ = 0.01, we get:

Adisc =

[1 0.01

0.39 0.85

]bdisc =

[0−0.8

](c) The minimum time is 0.02 seconds (can also say 2 steps), given our discretization.

This is a min-norm problem. First, we notice

z1 = Az0 + b+Bu0

z2 = A2z0 + Ab+ ABu0 + b+Bu1

z3 = A3z0 + A2b+ A2Bu0 + ABu1 + b+Bu2

zn = Anz0 +

( n−1∑j=0

Aj)b+

[An−1B An−2B . . . B

]u

If we define our constant-propagation matrix Bp =

(∑n−1j=0 A

j

)and our con-

trollability matrix C =[An−1B An−2B . . . B

], and we know that we want

zn = 0, we can solve for u using least-norms:

0 = Aˆn∗ z 0 + B p∗b + C∗uu = −C \ (Aˆn∗ z 0 + B p∗b)

We can then forward-propagate the state from x0 repeatedly by doing

xk+1 = Axk + b+Buk

until we get to xn. If xn is close enough to 0, then we can stop. The code doingthis procedure is shown below.

%i n i t i a l i z e parametersx 0 = [ 2 ; −2 ] ;d e l t = 0 . 0 1 ;B = [ 0 ; 1 ] ;

%genera l d e f i n i t i o n o f cont inuous A given our eqn o f motionAc = [ 0 1 ; −10∗x 0 (2)∗ x 0 (1)−1 5∗(1−x 0 ( 1 ) ˆ 2 ) ] ;

%Euler d i s c r e t i z a t i o n o f AcA = eye (2 ) + d e l t ∗Ac ;

22

%de f i n e our Euler−d i s c r e t i z e d bb = d e l t ∗ [ 0 ; 10∗ x 0 (2)∗ x 0 ( 1 ) ˆ 2 ] ;

for k = 1:10 %outer loop f o r f i n d i n g minimum timex l i s t = x 0 ; %l i s t f o r s t o r i n g x and x−dotC = [ ] ; %Con t r o l l a b i l i t y matrix ( pre−mu l t i p l i e s u)B p = 0 ; %matrix t ha t propaga tes b forward ( exp l a ined in so l n s )for j = 0 : k−1

B p = B p+Aˆ j ; %sum of Aˆ j ’ sC = [Aˆ j ∗B C ] ; %Bui ld ing up C i t e r a t i v e l y

endu min = −C\ ( (Aˆk)∗ x 0+B p∗b ) ; %lea s t−norm so ln o f ufor i = 1 : k%forward−propagate from x 0x l i s t = [ x l i s t A∗ x l i s t ( : , i )+B∗u min ( i )+b ] ;

end%Check to see i f we ’ re at the o r i g i n by the endi f norm( x l i s t ( : , k+1) ,2) < 10ˆ(−6)

break ;end

end

%p l o t the phase p o r t r a i tplot ( x l i s t ( 1 , : ) , x l i s t ( 2 , : ) , ’ l i n ew id th ’ , 3 )xlim ( [ −1 ,5 ] )ylim ( [ −250 ,10 ] )set (gca , ’ f o n t s i z e ’ , 20)

23

Figure 5: Phase portrait of x vs x

What we notice is this controller shunts the velocity hardcore to the negative sothat the system starts moving in the -x direction, then uses the second step toslow down the system while it’s getting to 0.

(d) This question refers to the controllability matrix of the system. For this type ofsystem, we have the following:

z1 = Az0 +Bu0

z2 = A2z0 + ABu0 +Bu1

zn = Anz0 + Cnu

Cn =[An−1B An−2B . . . B

]Where C is known as the controllability matrix. Because we’re going to rest,zN = 0, which means we have

−ANz0 = CNu

Therefore, the vector ANz0 must be in the range of the controllability matrix,CN . Note that you did not have to use the words “controllability”, just find it.

24

(e) This question is almost identical to (c), except you need to change from an iter-ative method to setting the number of steps to 200. 2s

0.01= 200 steps.

x 0 = [ 2 ; −2 ] ;d e l t = 0 . 0 1 ;B = [ 0 ; 1 ] ;

Ac = [ 0 1 ; −10∗x 0 (2)∗ x 0 (1)−1 5∗(1−x 0 ( 1 ) ˆ 2 ) ] ;

A = eye (2 ) + d e l t ∗Ac ;

b = d e l t ∗ [ 0 ; 10∗ x 0 (2)∗ x 0 ( 1 ) ˆ 2 ] ;

k = 200 ; %200∗0.01 = 2 seconds . Don ’ t i t e r a t e over k anymorex l i s t = x 0 ;C = [ ] ;B p = 0 ;for j = 0 : k−1

B p = B p+Aˆ j ;C = [Aˆ j ∗B C ] ;

endu OS = −C\ ( (Aˆk)∗ x 0+B p∗b ) ; %lea s t−norm so ln o f u , OS = one−sho tfor i = 1 : k%forward−propagate from x 0x l i s t = [ x l i s t A∗ x l i s t ( : , i )+B∗u OS( i )+b ] ;

end

plot ( x l i s t ( 1 , : ) , x l i s t ( 2 , : ) , ’ l i n ew id th ’ , 3 )set (gca , ’ f o n t s i z e ’ , 20)

25

(f) This question essentially has you build a receding-horizon model-predictive control(MPC) system, which are used everywhere in the real world. This is why we didthe general case for parts (a) and (b), because we’ll need to update Adisc and bdiscat every step now using the equation of motion.

x 0 = [ 2 ; −2 ] ;d e l t = 0 . 0 1 ;B = [ 0 ; 1 ] ;

x = x 0 ; %curren t p o s i t i o n t r a c k e rx l i s t = x 0 ;u MPC = [ ] ; %for c e p r o f i l e t racker , Model−Pred i c t i v e Contro lt ime hor i zon = 200 ;

for t = 1 : t ime hor i zon

%l i n e a r i z e around current p o s i t i o n xAc = [ 0 1 ; −10∗x (2)∗ x(1)−1 5∗(1−x ( 1 ) ˆ 2 ) ] ;A = eye (2 ) + d e l t ∗Ac ;

26

b = d e l t ∗ [ 0 ; 10∗x (2)∗ x ( 1 ) ˆ 2 ] ;

%k = l eng t h o f u f o r t h i s i t e r a t i o n%reced ing hor i zon means ( k + t ) = 200 alwaysk = time hor izon−t +1;

%ca l c u l a t e uC = [ ] ;B p = 0 ;for j = 0 : k−1

B p = B p + Aˆ j ;C = [Aˆ j ∗B C ] ;

endu min = −C\ ( (Aˆk)∗x+B p∗b ) ;

%Take ONE Euler s t ep forwardx = A∗x + B∗u min (1)+b ;

%Add va l ue s to t h e i r r e s p e c t i v e l i s t sx l i s t = [ x l i s t x ] ;u MPC = [u MPC u min ( 1 ) ] ;

endplot ( x l i s t ( 1 , : ) , x l i s t ( 2 , : ) , ’ l i n ew id th ’ , 3 )set (gca , ’ f o n t s i z e ’ , 20)

27

(g) We’re going to use the linearization code as well as the 1-step Euler code fromthe last section, but instead of finding u, we’ll just use the 2 u’s calculated frombefore: u lin and u NL.

%tra c k e r s f o r s t a t ex OS a l l = [ ] ;x MPC all = [ ] ;

x 0 = [ 2 ; −2 ] ;x OS = x 0 ; %curren t s t a t e , one−sho tx MPC = x 0 ; %curren t s t a t e , MPC

for t = 1 : t ime hor i zon

%Linea r i z a t i on f o r one−sho t c on t r o lAc OS = [ 0 1 ; −10∗x OS (2)∗x OS(1)−1 5∗(1−x OS ( 1 ) ˆ 2 ) ] ;A OS = eye (2 ) + d e l t ∗Ac OS ;b OS = d e l t ∗ [ 0 ; 10∗x OS (2)∗x OS ( 1 ) ˆ 2 ] ;

28

%Linea r i z a t i on f o r MPC con t r o lAc MPC = [ 0 1 ; −10∗x MPC(2)∗x MPC(1)−1 5∗(1−x MPC ( 1 ) ˆ 2 ) ] ;A MPC = eye (2 ) + d e l t ∗Ac MPC;b MPC = d e l t ∗ [ 0 ; 10∗x MPC(2)∗x MPC( 1 ) ˆ 2 ] ;

%propagate each s t a t e forward us ing%t h e i r r e s p e c t i v e f o r c e p r o f i l e sx OS = A OS∗x OS + B∗u OS( t)+ b OS ;x MPC = A MPC∗x MPC + B∗u MPC( t ) + b MPC;

x OS a l l = [ x OS a l l x OS ] ;x MPC all = [ x MPC all x MPC ] ;

end

plot ( x OS a l l ( 1 , : ) , x OS a l l ( 2 , : ) , ’ l i n ew id th ’ , 3 )hold onplot ( x MPC all ( 1 , : ) , x MPC all ( 2 , : ) , ’ l i n ew id th ’ , 3 )legend ({ ’ l i n e a r ’ , ’ non l i n ea r ’ })set (gca , ’ f o n t s i z e ’ , 20)

29

Clearly, the linear force profile does not push the state to 0 anymore, while thenonlinear one does. This is because the linear force profile does not take intoaccount the fact that A changes as we move away from our initial linearizationpoint. Obviously, the MPC method is better, because it pushes the “real” systemto 0. However, MPC is much more computationally expensive. Note that this issimilar to the “Hard to Port!” problem, in the sense that with the linear forceprofile, you calculate it in one shot, then use it in the real world and pray you getto rest at the end. With MPC, at every step, you’re feeding back where you’re atto recalculate a better u. That means MPC is actually robust to noise, as well asnonlinearities.

30

midterm exam solutions - ee263.stanford.eduee263.stanford.edu/exams/midterm_sol_su18.pdf · midterm...

Documents