MIT OpenCourseWare

http://ocw.mit.edu

16.323 Principles of Optimal Control

Spring 2008

For information about citing these materials or our Terms of Use, visit: http://ocw.mit.edu/terms.

16.323 Lecture 7

Numerical Solution in Matlab

Comparison with b=0.1

8
Analytic
Numerical
7

tf

1
2
10

10

10

10

10

Spr 2008

Simple Problem

Performance

tf

J=

16.323 71

(u x)2dt

t0

with dynamics x = u and BC t0 = 0, x0 = 1, tf = 1.

So this is a xed nal time, free nal state problem.

Form Hamiltonian

H = (u x)2 + pu
Necessary conditions become:
x = u
p = 2(u x)(1)
0 = 2(u x) + p

(7.25)
(7.26)
(7.27)

with BC that p(tf ) = 0.

Rearrange to get
p = p
p(t) = c1et
But now impose BC to get
p(t) = 0

(7.28)
(7.29)
(7.30)

This implies that u = x is the optimal solution, and the closed-loop

dynamics are
x = x
with solution x(t) = et.
Clearly this would be an unstable response on a longer timescale,
but given the cost and the short time horizon, this control is the
best you can do.

June 18, 2008

Spr 2008

Simple Zermelos Problem

16.323 72

Consider ship that has to travel through a region of strong currents.

The ship is assumed to have constant speed V but its heading can
be varied. The current is assumed to be in the y direction with speed
of w.

The motion of the boat is then given by the dynamics

x = V cos
y = V sin + w

(7.31)
(7.32)

The goal is to minimize time, the performance index is

tf
J=
1dt = tf
0

with BC x0 = y0 = 0, xf = 1, yf = 0
Final time is unspecied.

Dene costate p = [p1 p2]T , and in this case the Hamiltonian is

H = 1 + p1(V cos ) + p2(V sin + w)

Now use the necessary conditions to get (p = HxT )

H
=0
x
H
p 2 =
=0
y
p 1 =

June 18, 2008

p 1 = c1

(7.33)

p 2 = c2

(7.34)

Spr 2008

16.323 73

Control input (t) is unconstrained, so have (Hu = 0)

H
= p1V sin + p2V cos = 0
u
which gives the control law
p2 p2
tan =
=
p1 p1

(7.35)

(7.36)

Since p1 and p2 are constants, then (t) is also a constant.

Optimal control is constant, so can integrate the state equations:

x = V t cos
y = V t(sin + w)

(7.37)
(7.38)

Now impose the BC to get x(tf ) = 1, y(tf ) = 0 to get

tf =

1
V cos

Rearrange to get

cos =

sin =

w
V

V 2 w2
V

which gives that

1
V 2 w2
Does this make sense?
tf =

June 18, 2008

= arcsin

w
V

Spr 2008

Numerical Solutions

16.323 74

Most of the problems considered so far have been simple. Things get
more complicated by the need to solve a two-point boundary value
problem when the dynamics are nonlinear.
Numerous solution techniques exist, including shooting methods13
and collocation
Will discuss the details on these later, but for now, let us look at
how to solve these use existing codes
Matlab code called BVP4C exists that is part of the standard package 14
Solves problems of a standard form:
y = f (y, t, p)

atb

where y are the variables of interest, and p are extra variables in

the problem that can also be optimized
Where the system is subject to the boundary conditions:
g(y(a), y(b)) = 0
The solution is an approximation S(t) which is a continuous function
that is a cubic polynomial on sub-intervals [tn, tn+1] of a mesh
a = t0 < t1 < . . . < tn1 < tn = b
This approximation satises the boundary conditions, so that:
g(S(a), S(b)) = 0
And it satises the dierential equations (collocates) at both ends
and the mid-point of each subinterval:
S (tn) = f (S(tn), tn)
S ((tn + tn+1)/2) = f (S((tn + tn+1)/2), (tn + tn+1)/2)
S (tn+1) = f (S(tn+1), tn+1)
13 Online
14 Matlab

reference
help and BVP4C Tutorial

June 18, 2008

Spr 2008

16.323 75

Now constrain continuity in the solution at the mesh points converts

problem to a series of nonlinear algebraic equations in the unknowns
Becomes a root nding problem that can be solved iteratively
(Simpsons method).
Inputs to BVP4C are functions that evaluate the dierential equation
y = f (y, t) and the residual of the boundary condition (e.g. y1(a) =
1, y2(a) = y1(b), and y3(b) = 0 ):
function
res = [

res = bvpbc(ya, yb)

ya(1) 1
ya(2) yb(1)
yb(3)];

Redo example on page 415 using numerical techniques

Finite time LQR problem with tf = 10

Figure 7.1: Results suggest a good comparison with the dynamic LQR result

June 18, 2008

Spr 2008

TPBVP for LQR

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

function m = TPBVPlqr(p1,p2,p3)

global A B x0 Rxx Ruu Ptf

t_f=10;x0=[1 1];

Rxx=p1;Ruu=p2;Ptf=p3;

solinit = bvpinit(linspace(0,t_f),@TPBVPlqrinit);

sol = bvp4c(@TPBVPlqrode,@TPBVPlqrbc,solinit);

time = sol.x;

state = sol.y([1 2],:);

adjoint = sol.y([3 4],:);

control = -inv(Ruu)*B*sol.y([3 4],:);

m(1,:) = time;m([2 3],:) = state;m([4 5],:) = adjoint;m(6,:) = control;

%-------------------------------------------------------------------------

function dydt=TPBVPlqrode(t,y)

global A B x0 Rxx Ruu Ptf

dydt=[ A -B/Ruu*B; -Rxx -A]*y;

%-------------------------------------------------------------------------

function res=TPBVPlqrbc(ya,yb)

global A B x0 Rxx Ruu Ptf

res=[ya(1) - x0(1);ya(2)-x0(2);yb(3:4)-Ptf*yb(1:2)];

%-------------------------------------------------------------------------

function v=TPBVPlqrinit(t)

global A B x0 b alp

v=[x0;1;0];

return

25
26
27
28
29
30
31
32
33
34
35
36

% 16.323 Spring 2007

% Jonathan How

% redo LQR example on page 4-15 using numerical approaches

clear all;close all;

set(0, DefaultAxesFontSize, 14, DefaultAxesFontWeight,demi)

set(0, DefaultTextFontSize, 14, DefaultTextFontWeight,demi)

global A B

Ptf=[0 0;0 4];Rxx=[1 0;0 0];Ruu=1;A=[0 1;0 -1];B=[0 1];

tf=10;dt=.01;time=[0:dt:tf];

m=TPBVPlqr(Rxx,Ruu,Ptf); % numerical result

37
38
39
40
41
42
43
44
45
46

% integrate the P backwards for LQR result

P=zeros(2,2,length(time));K=zeros(1,2,length(time));

Pcurr=Ptf;

for kk=0:length(time)-1

P(:,:,length(time)-kk)=Pcurr;

K(:,:,length(time)-kk)=inv(Ruu)*B*Pcurr;

Pdot=-Pcurr*A-A*Pcurr-Rxx+Pcurr*B*inv(Ruu)*B*Pcurr;

Pcurr=Pcurr-dt*Pdot;

end

47
48
49
50
51
52
53
54

% simulate the state

x1=zeros(2,1,length(time));xcurr1=[1 1];
for kk=1:length(time)-1
x1(:,:,kk)=xcurr1;

xdot1=(A-B*K(:,:,kk))*x1(:,:,kk);

xcurr1=xcurr1+xdot1*dt;

end

55
56
57
58
59
60
61

figure(3);clf

plot(time,squeeze(x1(1,1,:)),time,squeeze(x1(2,1,:)),--,LineWidth,2),

xlabel(Time (sec));ylabel(States);title(Dynamic Gains)

hold on;plot(m(1,:),m([2],:),s,m(1,:),m([3],:),o);hold off

legend(LQR x_1,LQR x_2,Num x_1,Num x_2)

print -dpng -r300 numreg2.png

June 18, 2008

16.323 76

Spr 2008

Conversion

16.323 77

BVP4C sounds good, but this standard form doesnt match many of
the problems that we care about
In particular, free end time problems are excluded, because the time
period is dened to be xed t [a, b]

Can convert our problems of interest into this standard form though
using some pretty handy tricks.
U. Ascher and R. D. Russell, Reformulation of Boundary Value
Problems into Standard Form, SIAM Review, Vol. 23, No. 2,
238-254. Apr., 1981.

Key step is to re-scale time so that = t/tf , then [0, 1].

Implications of this scaling are that the derivatives must be changed
since d = dt/tf
d
d
= tf
d
dt

Final step is to introduce a dummy state r that corresponds to tf with

the trivial dynamics r = 0.
Now replace all instances of tf in the necessary/boundary conditions
for state r.
Optimizer will then just pick an appropriate constant for r = tf

June 18, 2008

Spr 2008

16.323 78

Recall that our basic set of necessary conditions are, for t [t0, tf ]
x = a(x, u, t)
p = HxT
Hu = 0

And we considered various boundary conditions x(t0) = x0, and:

If tf is free: ht + g + pT a = ht + H(tf ) = 0
If xi(tf ) is xed, then xi(tf ) = xif

If xi(tf ) is free, then pi(tf ) =

(tf )
xi

Then
x = a(x, u, t) x = tf a(x, u, )
and
p = HxT

June 18, 2008

p = tf HxT

Spr 2008

Example: 71

16.323 79

Revisit example on page 66

Linear system with performance/time weighting and free end time
Necessary conditions are:
x = Ax + Bu
p = AT p

0 = bu + 0 1 p
with state conditions
x1(0)
x2(0)
x1(tf )
x2(tf )
0.5bu2(tf ) + tf

=
=
=
=
=

10
0
0
0
0

Dene the state of interest z = [xT pT r]T and note that

dz
dz
= tf
d
dt

A B 0 1 /b 0
= z5 0
AT
0 z
0
0
0
z = f (z) which is nonlinear
with BC:
z1(0)
z2(0)
z1(1)
z2(1)

=
=
=
=

10
0
0
0

0.5 2
z (1) + z5(1) = 0
b 4
June 18, 2008

Spr 2008

16.323 710

Code given on following pages

Note it is not particularly complicated
Solution time/iteration count is a strong function of the initial so
lution not a particularly good choice for p is used here

Analytic solution gave tf = (1800b/)1/5

Numerical result give close agreement in prediction of the nal time
Comparison with b=0.1
8
Analytic
Numerical
7

tf

1
2
10

10

10

10

10

Figure 7.2: Comparison of the predicted completion times for the maneuver

June 18, 2008

Spr 2008

16.323 711

3
2

u(t)

1
0
1
2
3

0.5

1.5

2.5

3.5

4.5

Time
8

uAnalytic(t)UNumerical

x 10

Analytic

3.5
3
2.5
2
1.5
1
0.5

0.5

1.5

2.5

3.5

4.5

Time

Figure 7.3: Control Inputs

10

1
Analytic
Numerical

dX(t)/dt

X(t)

8
6
4
2
0

Analytic
Numerical

1
2
3

Time
8

Time
7

x 10

x 10

0.8

Error

Error

0.6
0.4

2
0.2

1
0

Time

Time

Figure 7.4: State response

June 18, 2008

Spr 2008

TPBVP

1
2
3
4
5

function m = TPBVP(p1,p2)
% 16.323 Spring 2007
% Jonathan How
%
global A B x0 b alp

6
7
8
9
10
11

A=[0 1;0 0];

B=[0 1];
x0=[10 0];
b=p1;
alp=p2;

12
13
14

solinit = bvpinit(linspace(0,1),@TPBVPinit);
sol = bvp4c(@TPBVPode,@TPBVPbc,solinit);

15
16
17
18
19
20
21
22
23

time = sol.y(5)*sol.x;
state = sol.y([1 2],:);
adjoint = sol.y([3 4],:);
control = -(1/b)*sol.y(4,:);
m(1,:) = time;
m([2 3],:) = state;
m([4 5],:) = adjoint;
m(6,:) = control;

24
25
26
27
28

%-------------------------------------------------------------------------
function dydt=TPBVPode(t,y)
global A B x0 b alp
dydt=y(5)*[ A -B*[0 1]/b zeros(2,1); zeros(2,2) -A zeros(2,1);zeros(1,5)]*y;

29
30
31
32
33

%-------------------------------------------------------------------------
function res=TPBVPbc(ya,yb)
global A B x0 b alp
res=[ya(1) - x0(1);ya(2)-x0(2);yb(1);yb(2);-0.5*yb(4)^2/b+ alp*yb(5)];

34
35
36
37
38

%-------------------------------------------------------------------------
function v=TPBVPinit(t)
global A B x0 b alp
v=[x0;1;0;1];

39
40

return

41

June 18, 2008

16.323 712

Spr 2008

TPBVP Main

1
2
3
4
5
6
7
8
9
10
11
12

% 16.323 Spring 2007

% Jonathan How
% TPmain.m
%
b=0.1;
%alp=[.05 .1 1 10 20];
alp=logspace(-2,2,10);
t=[];
for alpha=alp
m=TPBVP(b,alpha);
t=[t;m(1,end)];
end

13
14
15
16
17
18
19
20

figure(1);clf

semilogx(alp,(1800*b./alp).^0.2,-,Linewidth,2)

hold on;semilogx(alp,t,rs);hold off

xlabel(\alpha,FontSize,12);ylabel(t_f,FontSize,12)

legend(Analytic,Numerical)

title(Comparison with b=0.1)

print -depsc -f1 TPBVP1.eps;jpdf(TPBVP1)

21
22
23
24
25
26
27
28
29
30

% code from opt1.m on the analytic solution

b=0.1;alpha=0.1;

m=TPBVP(b,alpha);

tf=(1800*b/alpha)^0.2;

c1=120*b/tf^3;

c2=60*b/tf^2;

u=(-c2+c1*m(1,:))/b;

A=[0 1;0 0];B=[0 1];C=eye(2);D=zeros(2,1);G=ss(A,B,C,D);X0=[10 0];

[y3,t3]=lsim(G,u,m(1,:),X0);

31
32
33
34
35
36
37
38
39
40
41
42

figure(2);clf

subplot(211)

plot(m(1,:),u,g-,LineWidth,2);

xlabel(Time,FontSize,12);ylabel(u(t),FontSize,12)

hold on;plot(m(1,:),m(6,:),--);hold off

subplot(212)

plot(m(1,:),abs(u-m(6,:)),-)

xlabel(Time,FontSize,12)

ylabel(u_{Analytic}(t)-U_{Numerical},FontSize,12)

legend(Analytic,Numerical)

print -depsc -f2 TPBVP2.eps;jpdf(TPBVP2)

43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

figure(3);clf

subplot(221)

plot(m(1,:),y3(:,1),c-,LineWidth,2);

xlabel(Time,FontSize,12);ylabel(X(t),FontSize,12)

hold on;plot(m(1,:),m([2],:),k--);hold off

legend(Analytic,Numerical)

subplot(222)

plot(m(1,:),y3(:,2),c-,LineWidth,2);

xlabel(Time,FontSize,12);ylabel(dX(t)/dt,FontSize,12)

hold on;plot(m(1,:),m([3],:),k--);hold off

legend(Analytic,Numerical)

subplot(223)

plot(m(1,:),abs(y3(:,1)-m(2,:)),k-)

xlabel(Time,FontSize,12);ylabel(Error,FontSize,12)

subplot(224)

plot(m(1,:),abs(y3(:,2)-m(3,:)),k-)

xlabel(Time,FontSize,12);ylabel(Error,FontSize,12)

print -depsc -f3 TPBVP3.eps;jpdf(TPBVP3)

June 18, 2008

16.323 713

Spr 2008

Zermelos Problem

16.323 714

Simplied dynamics of a UAV ying in a horizontal plane can be mod

eled as:
x (t) = V cos (t)
y(t) = V sin (t) + w
where (t) is the heading angle (control input) with respect to the x
axis, V is the speed.

Objective: y from point A to B in minimum time:

tf
min J =
(1)dt
0

where tf is free.
Initial conditions are:
x(0) = x0

y(0) = y0

Final conditions are:

x(tf ) = x1

June 18, 2008

y(tf ) = y1

Spr 2008

16.323 715

Apply the standard necessary conditions with

H = 1 + p1V (cos (t)) + p2(V sin (t) + w)
x = a(x, u, t)
p = HxT
Hu = 0
x (t) = V cos (t)
y(t) = V sin (t) + w
p1(t) = 0
p2(t) = 0
0 = p1 sin (t) + p2 cos (t)
Then add extra state for the time.
Since tf is free, must add terminal condition that H(tf ) = 0, which
gives a total of 5 conditions (2 initial, 3 terminal).

Figure 7.5: Zermelo examples

June 18, 2008

Spr 2008

TPBVPZermelo

1
2

function m = TPBVPzermelo(p1,p2)

global x0 x1 V w

3
4
5

solinit = bvpinit(linspace(0,1),@TPBVPinit);

sol = bvp6c(@TPBVPode,@TPBVPbc,solinit);

6
7
8
9
10

time = sol.y(5)*sol.x;

state = sol.y([1 2],:);

adjoint = sol.y([3 4],:);

control = atan2(-sol.y([4],:),-sol.y([3],:));

11
12
13
14
15
16

m(1,:) = time;

m([2 3],:) = state;

m([4 5],:) = adjoint;

m(6,:) = control;

return

17
18
19
20

%-------------------------------------------------------------------------
function dydt=TPBVPode(t,y)
global x0 x1 V w

21
22
23
24
25

% x y p1 p2 t

% minimizing form

sinth=-y(4)/sqrt(y(3)^2+y(4)^2);

costh=-y(3)/sqrt(y(3)^2+y(4)^2);

26
27
28
29
30
31
32
33
34

dydt=y(5)*[V*costh ; V*sinth+w;0;0;0];

%-------------------------------------------------------------------------

function res=TPBVPbc(ya,yb)

global x0 x1 V w

% x y p1 p2 t

% minimizing form

costhb=-yb(3)/sqrt(yb(3)^2+yb(4)^2);

sinthb=-yb(4)/sqrt(yb(3)^2+yb(4)^2);

35
36
37
38

res=[ya(1) - x0(1);ya(2)-x0(2);

yb(1) - x1(1);yb(2)-x1(2);

1+V*costhb*yb(3)+V*(sinthb+w)*yb(4)];

39
40
41
42
43
44
45

%-------------------------------------------------------------------------

function v=TPBVPinit(t)

global x0 x1 V w

%v=[x0;-1;-1;norm(x1-x0)/(V-w)];

v=[x0;1;1;norm(x1-x0)/(V-w)];

return

46
47
48
49
50
51

clear all

global x0 x1 V w

w=1/sqrt(2);

x0=[-1 0];x1=[0 0];V = 1;

mm=TPBVPzermelo;

52
53
54
55
56
57
58
59
60
61

figure(1);clf

plot(mm(2,:),mm([3],:),LineWidth,2);axis(square);grid on

axis([-2 5 -2 1.5 ])

xlabel(x,FontSize,12);ylabel(y,FontSize,12);

hold on;

plot(x0(1),x0(2),rs);plot(x1(1),x1(2),bs);

text(x0(1),x0(2),Start,FontSize,12)

text(x1(1),x1(2),End,FontSize,12)

hold off

62
63
64
65

figure(2);clf

plot(mm(1,:),180/pi*mm([6],:),LineWidth,2);grid on;axis(square)

xlabel(t,FontSize,12);ylabel(u,FontSize,12);

66
67

print -dpng -r300 -f1 BVP_zermelo.png;

June 18, 2008

16.323 716

Spr 2008

68

print -dpng -r300 -f2 BVP_zermelo2.png;

69
70
71
72
73
74

clear all
global x0 x1 V w
w=1/sqrt(2);
x0=[0 1];x1=[0 0];V = 1;
mm=TPBVPzermelo;

75
76
77
78
79
80
81
82
83
84

figure(1);clf
plot(mm(2,:),mm([3],:),LineWidth,2);axis(square);grid on
axis([-2 5 -2 1.5 ])
xlabel(x,FontSize,12);ylabel(y,FontSize,12);
hold on;
plot(x0(1),x0(2),rs);plot(x1(1),x1(2),bs);
text(x0(1),x0(2),Start,FontSize,12)
text(x1(1),x1(2),End,FontSize,12)
hold off

85
86
87
88

figure(2);clf
plot(mm(1,:),180/pi*mm([6],:),LineWidth,2);grid on;axis(square)
xlabel(t,FontSize,12);ylabel(u,FontSize,12);

89
90
91

print -dpng -r300 -f1 BVP_zermelo3.png;

print -dpng -r300 -f2 BVP_zermelo4.png;

92

June 18, 2008

16.323 717

Orbit Raising Example

Spr 2008

16.323 718

Goal: (Bryson page 66) determine the maximum radius orbit transfer
in a given time tf assuming a constant thrust rocket (thrust T ).15
Must nd the thrust direction angle (t)
Assume a circular orbit for the initial and nal times

Nomenclature:
r radial distance from attracting center, with gravitational con
stant
v, u tangential, radial components of the velocity
m mass of s/c, and m
is the fuel consumption rate (constant)

Problem: nd (t) to maximize r(tf ) subject to:

r = u
v2

T sin
u =
2+
r
r

m0 |m|t
uv
T cos
v = +
r
m0 |m|t

Dynamics :

with initial conditions

r(0) = r0

u(0) = 0

v(0) =
r0

and terminal conditions

u(tf ) = 0

v(tf )

=0
r(tf )

With pT = [p1 p2 p3] this gives the Hamiltonian (since g = 0)

u
2

T sin
H
= p
T

v
r

r
2 + m0|m |t

T cos
uv
r + m0 |m
|t
15 Thanks

to Geo Huntington

June 18, 2008

Spr 2008

16.323 719

Then Hu = 0 with u(t) = (t) gives

T cos
T sin
p2
+ p3
=0
m0 |m|t
m0 |m|t

which gives that

p2(t)
p3(t)
that can be solved for the control input given the costates.
tan =

Note that this is a problem of the form on 66, with

u(tf
)
m=
=0
v(tf ) r(t )
f

which gives

w = r + 1u(tf ) + 2 v(tf )

r(tf )

Since the rst state r is not specied at the nal time, must have that

w
2

p1(tf ) =
(tf ) = 1 +
r
2 r(tf )3
And note that

w
(tf ) = 2
v

which gives 2 in terms of the costate.

p3(tf ) =

June 18, 2008

Spr 2008

16.323 720

Figure 7.6: Orbit raising examples

June 18, 2008

Spr 2008

16.323 721

Figure 7.7: Orbit raising examples

June 18, 2008

Spr 2008

Orbit Raising

1
2
3
4
5
6
7

%orbit_bvp_how created by Geoff Huntington 2/21/07

%Solves the Hamiltonian Boundary Value Problem for the orbit-raising optimal
%control problem (p.66 Bryson & Ho). Computes the solution using BVP4C
%Invokes subroutines orbit_ivp and orbit_bound
clear all;%close all;
set(0, DefaultAxesFontSize, 14, DefaultAxesFontWeight,demi)
set(0, DefaultTextFontSize, 14, DefaultTextFontWeight,demi)

8
9
10
11

%Fixed final time %Tf = 3.3155;

Tf = 4;

four=0; % not four means use bvp6c

12
13
14
15
16

%Constants

global mu m0 m1 T

mu=1; m0=1; m1=-0.07485; T= 0.1405;

%mu=1; m0=1; m1=-.2; T= 0.1405;

17
18
19
20
21
22
23
24
25
26
27
28
29
30

%Create initial Guess

n=100;

y = [ones(1,n); %r

zeros(1,n); %vr
ones(1,n); %vt

-ones(1,n); %lambda_r

-ones(1,n); %lambda_vr

-ones(1,n)]; %lambda_vt

x = linspace(0,Tf,n); %time

solinit.x = x;solinit.y = y;

%Set optimizer options

tol = 1E-10;

options = bvpset(RelTol,tol,AbsTol,[tol tol tol tol tol tol],Nmax, 2000);

31
32
33
34
35
36
37
38
39

%Solve

if four

sol = bvp4c(@orbit_ivp,@orbit_bound,solinit,options);

Nstep=40;

else

sol = bvp6c(@orbit_ivp,@orbit_bound,solinit,options);

Nstep=30;

end

40
41
42
43
44
45
46
47
48

%Plot results

figure(1);clf

plot(sol.x,sol.y(1:3,:),LineWidth,2)

legend(r,v_r,v_t,Location,NorthWest)

grid on;

axis([0 4 0 2])

title(HBVP Solution)

xlabel(Time);ylabel(States)

49
50
51
52
53
54
55
56

figure(2);clf

plot(sol.x,sol.y(4:6,:),LineWidth,2)

legend(p_1(t),p_2(t),p_3(t),Location,NorthWest)

grid on;

axis([0 4 -3 2])

title(HBVP Solution)

xlabel(Time);ylabel(Costates)

57
58
59
60
61
62
63
64
65

ang2=atan2(sol.y([5],:),sol.y([6],:))+pi;

figure(3);clf

plot(sol.x,180/pi*ang2,LineWidth,2)

grid on;

axis([0 4 0 360])

title(HBVP Solution)

xlabel(Time);ylabel(Control input angle \phi(t))

norm([tan(ang2)-(sol.y([5],:)./sol.y([6],:))])

66
67
68
69

print -f1 -dpng -r300 orbit1.png

print -f2 -dpng -r300 orbit2.png

print -f3 -dpng -r300 orbit3.png

70
71

% Code below adapted inpart from Bryson "Dynamic Optimization"

June 18, 2008

16.323 722

Spr 2008

72
73
74
75
76

dt=diff(sol.x);
dth=(sol.y(3,1:end-1)./sol.y(1,1:end-1)).*dt; % \dot \theta = v_t/r
th=0+cumsum(dth);
pathloc=[sol.y(1,1:end-1).*cos(th) sol.y(1,1:end-1).*sin(th)];

77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95

figure(4);clf
plot(pathloc(:,1),pathloc(:,2),k-,LineWidth,2)
hold on
zz=exp(sqrt(-1)*[0:.01:pi]);
r0=sol.y(1,1);rf=sol.y(1,end);
plot(r0*real(zz),r0*imag(zz),r--,LineWidth,2)
plot(rf*real(zz),rf*imag(zz),b--,LineWidth,2)
plot(r0,0,ro,MarkerFace,r)
plot(rf*cos(th(end)),rf*sin(th(end)),bo,MarkerFace,b)
fact=0.2;ep=ones(size(th,1),1)*pi/2+th-ang2(1:end-1);
xt=pathloc(:,1)+fact*cos(ep); yt=pathloc(:,2)+fact*sin(ep);
for i=1:Nstep:size(th,1),
pltarrow([pathloc(i,1);xt(i)],[pathloc(i,2);yt(i)],.05,m,-);
end;
%axis([-1.6 1.6 -.1 1.8]);
axis([-2 2 -.1 1.8]);
axis(equal)
hold off

96
97

1
2

print -f4 -dpng -r300 orbit4.png;

function [dx] = orbit_ivp(t,x)

global mu m0 m1 T

4
5
6

%State

r = x(1);u = x(2);v = x(3);

lamr = x(4);lamu = x(5);lamv = x(6);

8
9
10

%Substitution for control

sinphi = -lamu./sqrt(lamu.^2+lamv.^2);

cosphi = -lamv./sqrt(lamu.^2+lamv.^2);

11
12
13
14
15

%Dynamic Equations
dr = u;
du = v^2/r - mu/r^2 + T*sinphi/(m0 + m1*t);
dv = -u*v/r + T*cosphi/(m0 + m1*t);

16
17
18
19

dlamr = -lamu*(-v^2/r^2 + 2*mu/r^3) - lamv*(u*v/r^2);

dlamu = -lamr + lamv*v/r;
dlamv = -lamu*2*v/r + lamv*u/r;

20
21

1
2

dx = [dr; du; dv; dlamr; dlamu; dlamv];

function [res] = orbit_bound(x,x2)
global mu m0 m1 T

3
4
5
6

%Initial State
r = x(1);u = x(2);v = x(3);
lamr = x(4);lamu = x(5);lamv = x(6);

7
8
9
10

%Final State
r2 = x2(1);u2 = x2(2);v2 = x2(3);
lamr2 = x2(4);lamu2 = x2(5);lamv2 = x2(6);

11
12
13
14
15
16
17
18

%Boundary Constraints
b1 = r - 1;
b2 = u;
b3 = v - sqrt(mu/r);
b4 = u2;
b5 = v2 - sqrt(mu/r2);
b6 = lamr2 + 1 - lamv2*sqrt(mu)/2/r2^(3/2);

19
20
21

%Residual
res = [b1;b2;b3;b4;b5;b6];

June 18, 2008

16.323 723