Calculus of Variations and

Optimal Control:
Continuous Systems
(various problem conditions)

x(0) & t
f
given

continuous time LQR

t
f
given, x(0) & x(t
f
) [partly] given

t
f
unconstrained, x(0), x(t
f
) [partly] given (example: min time)

Integral constraint over path (Eg. xed path length)

Equality constraints over functions of the state/control 1

Continuous System
x(0) and t
f
are known

Given:

Find u(t) that minimizes

Use Lagrange multiplier functions

2
x
t
f
t
f
x
0

t
f
t
0
t
f

.minimun jerk

t
f
t
0
t
f

x
t
f
t
f
x
0
x(t) = f (x(t), u(t), t) x(t
0
) given t [t
0
, t
f
]
u
J = (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt
J
J
A
= (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt +

t
f
t
0

T
(t) [f (x(t), u(t), t) x(t)] dt
(1)
1
x
t
f
t
f
x
0

t
f
t
0
t
f

.minimun jerk

t
f
t
0
t
f

x
t
f
t
f
x
0
x(t) = f (x(t), u(t), t) x(t
0
) given t [t
0
, t
f
]
u
J = (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt
J
J
A
= (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt +

t
f
t
0

T
(t) [f (x(t), u(t), t) x(t)] dt
(1)
1
x
t
f
t
f
x
0

t
f
t
0
t
f

.minimun jerk

t
f
t
0
t
f

x
t
f
t
f
x
0
x(t) = f (x(t), u(t), t) x(t
0
) given t [t
0
, t
f
]
u
J = (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt
J
J
A
= (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt +

t
f
t
0

T
(t) [f (x(t), u(t), t) x(t)] dt
(1)
1

Define a Hamiltonian

Plug into Lagrangian and integrate by parts

Its variation (w.r.t. changes in x and u) is:

3
J
A
= (x(t
f
), t
f
) +

t
f
t
0
L(x(t), u(t), t) dt +

t
f
t
0

T
(t) [f (x(t), u(t), t) x(t)] dt
(1)
H = H(x(t), u(t), t) = L(x(t), u(t), t) +
T
(t)f (x(t), u(t), t)

T
(t) x(t)
J = (x(t
f
), t
f
)
T
(t
f
) x(t
f
)+
T
(t
0
) x(t
0
)+

t
f
t
0

H(x(t), u(t), t) +

T
x(t)

dt
(2)
J J
J =

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt
(3)
J
(t) x(u(1 : t))

T
(t
f
) =

x(t
f
)
,

T
(t) =
H(t)
x(t)
=
L
x

T
(t)
f
x
t
f

J =
T
(t
0
)x(t
0
) +

t
f
t
0
H
u
udt
x(t
0
) = 0 x(0)
u
H
u
= 0 , t [t
0
, t
f
]
x(t) = f (x(t), u(t), t)

T
(t) =
L
x

T
(t)
f
x
(4)

T
(t
f
) =

x(t
f
)
(5)
H
u
= 0 (6)
x
0
given
x u

x
two point boundary-value problem
2
H = H(x(t), u(t), t) = L(x(t), u(t), t) +
T
(t)f (x(t), u(t), t)

T
(t) x(t)
J = (x(t
f
), t
f
)
T
(t
f
) x(t
f
)+
T
(t
0
) x(t
0
)+

t
f
t
0

H(x(t), u(t), t) +

T
x(t)

dt
(2)
J J
J =

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt
(3)
J
(t) x(u(1 : t))

T
(t
f
) =

x(t
f
)
,

T
(t) =
H(t)
x(t)
=
L
x

T
(t)
f
x
t
f

J =
T
(t
0
)x(t
0
) +

t
f
t
0
H
u
udt
x(t
0
) = 0 x(0)
u
H
u
= 0 , t [t
0
, t
f
]
x(t) = f (x(t), u(t), t)

T
(t) =
L
x

T
(t)
f
x
(4)

T
(t
f
) =

x(t
f
)
(5)
H
u
= 0 (6)
x
0
given
x u

x
two point boundary-value problem
2
H = H(x(t), u(t), t) = L(x(t), u(t), t) +
T
(t)f (x(t), u(t), t)

T
(t) x(t)
J = (x(t
f
), t
f
)
T
(t
f
) x(t
f
)+
T
(t
0
) x(t
0
)+

t
f
t
0

H(x(t), u(t), t) +

T
x(t)

dt
(2)
J J
J =

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt
(3)
J
(t) x(u(1 : t))

T
(t
f
) =

x(t
f
)
,

T
(t) =
H(t)
x(t)
=
L
x

T
(t)
f
x
t
f

J =
T
(t
0
)x(t
0
) +

t
f
t
0
H
u
udt
x(t
0
) = 0 x(0)
u
H
u
= 0 , t [t
0
, t
f
]
x(t) = f (x(t), u(t), t)

T
(t) =
L
x

T
(t)
f
x
(4)

T
(t
f
) =

x(t
f
)
(5)
H
u
= 0 (6)
x
0
given
x u

x
two point boundary-value problem
2
H = H(x(t), u(t), t) = L(x(t), u(t), t) +
T
(t)f (x(t), u(t), t)

T
(t) x(t)
J = (x(t
f
), t
f
)
T
(t
f
) x(t
f
)+
T
(t
0
) x(t
0
)+

t
f
t
0

H(x(t), u(t), t) +

T
x(t)

dt
(2)
J J
J =

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt
(3)
J
(t) x(u(1 : t))

T
(t
f
) =

x(t
f
)
,

T
(t) =
H(t)
x(t)
=
L
x

T
(t)
f
x
t
f

J =
T
(t
0
)x(t
0
) +

t
f
t
0
H
u
udt
x(t
0
) = 0 x(0)
u
H
u
= 0 , t [t
0
, t
f
]
x(t) = f (x(t), u(t), t)

T
(t) =
L
x

T
(t)
f
x
(4)

T
(t
f
) =

x(t
f
)
(5)
H
u
= 0 (6)
x
0
given
x u

x
two point boundary-value problem
2

Zeroing it out (+ keeping dynamics requirements) leads to the Euler

Lagrange equations:
4
H = H(x(t), u(t), t) = L(x(t), u(t), t) +
T
(t)f (x(t), u(t), t)

T
(t) x(t)
J = (x(t
f
), t
f
)
T
(t
f
) x(t
f
)+
T
(t
0
) x(t
0
)+

t
f
t
0

H(x(t), u(t), t) +

T
x(t)

dt
(2)
J J
J =

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt
(3)
J
(t) x(u(1 : t))

T
(t
f
) =

x(t
f
)
,

T
(t) =
H(t)
x(t)
=
L
x

T
(t)
f
x
t
f

J =
T
(t
0
)x(t
0
) +

t
f
t
0
H
u
udt
x(t
0
) = 0 x(0)
u
H
u
= 0 , t [t
0
, t
f
]
x(t) = f (x(t), u(t), t)

T
(t) =
L
x

T
(t)
f
x
(4)

T
(t
f
) =

x(t
f
)
(5)
H
u
= 0 (6)
x
0
given
x u

x
two point boundary-value problem
2
J =

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt

If and f are not explicit functions of t (happens often) then the

Hamiltonian is constant (i.e. invariant) on the systems trajectory!

On the extremal path both terms zero out, meaning that H(t) = constant
5
Continuous Time LQR

Given x(0), t
f
and a linear system:

Find a path that brings (some of) x close to zero without allowing it to
vary to much on the way and without too much control energy. I.e. find u
that minimizes
where S,Q and R are positive definite.

Note: this can easily be generalized to time varying systems and costs.

Solution will be very similar to the discrete case.

The resulting u turns out to be a continuous state feedback rule.

6
Solution:

The Hamiltonian is

The end constraint on is

s diff. eq. is

The third Euler Lagrange condition gives

Rearranging

This is a two point boundary value problem with x known at t

0
and
known at t
f
. The two linear differential equations are coupled by u. 7

Plugging into the state dynamics gives

One can show (see Bryson & Ho, sec. 5.2) that there exists a time
varying matrix S(t) that provides a linear relation between and x
Plugging this into the above state dynamics gives
i.e. is a linear state feedback. Similarly to the
discrete case, to know the control rule we simply need to find S(t).

We plug into and get

Next we plug () into the above equation and after a bit of rearranging
get

Since x(t)0 (otherwise there is no need for regulation) we can drop x

and get...
8
()
-
-
...

This is a quadratic differential equation in S with a boundary condition

This equation is known as a matrix Riccati equation. It can be solved

(e.g. by numeric integration from S
f
backwards) to get S(t).

Usually, this dynamic system is stable and reaches a steady state S as

t. The Matlab function care solves for S

(if a real valued solution

exists).

S

is good for regulating the system for a long duration (i.e. forever).

Since this differential equation is quadratic, it may have more than one
solution. The desired solution is PSD. Starting from S=0 (instead of
S=S
f
) and numerically integrating until convergence (i.e. till )
will give the PSD solution (see Bryson & Ho, sec. 5.4).

We will see (in a future lecture) that the HJB equations show that
J=x
T
(t
0
)S(t
0
)x
T
(t
0
). (This is also true for the discrete case, were we used
the notation P instead of S).
9
t
f
given , x(0) & x(t
f
) partly or
fully given

Same variation of J holds:

Suppose x
k
(t
f
)

is given, then x
k
(t
f
) = 0 and therefore we do not require
in order to zero out the variation

Similarly, if x
k
(t
0
) is not given, then x
k
(t
0
) 0 and to zero out J we
require that

Note that if the system is not controllable the condition may be

impossible to satisfy. 10
x = Ax +BR
1
B
T
(10)
S(t) Bryson & Ho, p150
(t) = S(t)x(t) (11)
S(t
f
) = S
f
x = Ax +BR
1
B
T
S(t)x(t) (12)
u = BR
1
B
T
S(t)x(t)
S(t)
x

Sx +S x = Qx ASx

S +SA+A
T
S SBR
1
B
T
S +A

x = 0
x(t) = 0

S = SAA
T
S +SBR
1
B
T
S A
S(t
f
) = S
f
S
Riccati
S(t)
S S
S Matlab care t
f

S = 0 PSD S
Bryson&Ho p.

S 0
167-8
t
f
t
0
t
f
J
J =

x

T

t=t
f
+ [
T
x]
t=t
0
+

t
f
t
0

H
x
+

x +
H
u
u

dt
(13)
4
x
k
(t
f
)
x
k
(t
f
) = 0

x
k
(t
f
)

T
k
= 0
x
k
(t
0
) t
f
J = 0 x
k
(t
0
) = 0

k
(t
0
) = 0
J x
k
(t
0
)
H
u
= 0
controllable
x(t) = f (x(t), u(t), t) (14)

T
(t) =
H
x
=
L
x

T
(t)
f
x
(15)

T
k
(t
f
) =

x
k
(t
f
)
or x
k
(t
f
) given (16)
H
u
= 0 (17)
k
k
(0) = 0 or x
k
(0) given (18)
x
k
(t
f
)
x
k
(t
f
) = 0

x
k
(t
f
)

T
k
= 0
x
k
(t
0
) t
f
J = 0 x
k
(t
0
) = 0

k
(t
0
) = 0
J x
k
(t
0
)
H
u
= 0
controllable
x(t) = f (x(t), u(t), t) (14)

T
(t) =
H
x
=
L
x

T
(t)
f
x
(15)

T
k
(t
f
) =

x
k
(t
f
)
or x
k
(t
f
) given (16)
H
u
= 0 (17)
k
k
(0) = 0 or x
k
(0) given (18)

The Euler Lagrange equations are now

11
x
k
(t
f
)
x
k
(t
f
) = 0

x
k
(t
f
)

T
k
= 0
x
k
(t
0
) t
f
J = 0 x
k
(t
0
) = 0

k
(t
0
) = 0
J x
k
(t
0
)
H
u
= 0
controllable
x(t) = f (x(t), u(t), t) (14)

T
(t) =
H
x
=
L
x

T
(t)
f
x
(15)

T
k
(t
f
) =

x
k
(t
f
)
or x
k
(t
f
) given (16)
H
u
= 0 (17)
k
k
(0) = 0 or x
k
(0) given (18)

Find a path x(t), starting at rest (zero velocity & acceleration) from x
0
at
time t
0
and ending at rest at x
f
at time t
f
so that the squared cumulative
change in acceleration (jerk) along the path is minimal.

Define the state to be x = [x, v, a] (position, velocity, acceleration) and

the control signal to be

The cost is:

where and = 0

We solve for the one dimensional (scalar position) case but solution
holds for the multidimensional case as well.
12
Minimum Jerk
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
L = u(t) = a [x, v, a]
= 0
1
2

t
f
t
0
u
2
(t)dt
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6

The dynamics equations are

The Jacobian is therefore

and (remembering )

so, by we get
13
x
k
(t
f
)
x
k
(t
f
) = 0

x
k
(t
f
)

T
k
= 0
x
k
(t
0
) t
f
J = 0 x
k
(t
0
) = 0

k
(t
0
) = 0
J x
k
(t
0
)
H
u
= 0
controllable
x(t) = f (x(t), u(t), t) (14)

T
(t) =
H
x
=
L
x

T
(t)
f
x
(15)

T
k
(t
f
) =

x
k
(t
f
)
or x
k
(t
f
) given (16)
H
u
= 0 (17)
k
k
(0) = 0 or x
k
(0) given (18)
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
x
k
(t
f
)
x
k
(t
f
) = 0

x
k
(t
f
)

T
k
= 0
x
k
(t
0
) t
f
J = 0 x
k
(t
0
) = 0

k
(t
0
) = 0
J x
k
(t
0
)
H
u
= 0
controllable
x(t) = f (x(t), u(t), t) (14)

T
(t) =
H
x
=
L
x

T
(t)
f
x
(15)

T
k
(t
f
) =

x
k
(t
f
)
or x
k
(t
f
) given (16)
H
u
= 0 (17)
k
k
(0) = 0 or x
k
(0) given (18)
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

T
(t) =
L
x

T
(t)
f
x
= 0 [
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

1
,

2
,

3
]
T
= [0,
1
,
2
]
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

T
(t) =
L
x

T
(t)
f
x
= 0 [
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

1
,

2
,

3
]
T
= [0,
1
,
2
]
6
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6

We can solve these differential equations:

The Hamiltonian ( H = L+
T
f ) is

so
14
minimum jerk
t
0
x
f
x
0
x(t)
x
f
t
f
x
0
u(t) = a [x, v, a]
J =
1
2

t
f
t
0
u
2
(t)dt
= 0 L =
1
2
u
2
f
1
= x = v
f
2
= v = a
f
3
= a = u
f
x
=

0 1 0
0 0 1
0 0 0

L
x
L
x
=

0
0
0

f
u
f
u
=

0
0
1

1
,

2
,

3
]
T
=

(t) =
L
x

T
(t)
f
x
= 0[
1
,
2
,
3
]

0 1 0
0 0 1
0 0 0

= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
6
[

1
,

2
,

3
]
T
= [0,
1
,
2
]

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
H = u
2
(t) +
T
f
=
1
2
u
2
(t) +
1
v +
2
a +
3
u
H
u
= 0
0 = u +
3
u =
1
2
c
1
t
2
+ c
2
t c
3
a = u
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t + c
4
v = a
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
+ c
4
t + c
5
x = v
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+
1
2
c
4
t
2
+ c
5
t + c
6
c
6
= x
0
c
4
= c
5
= 0 x(0) = x
0
v(0) = a(0) = 0
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+ x
0
t
f
x(t
f
) = x
f
v(t
f
) = a(t
f
) = 0

0
0
x
f
x
0

1
6
t
3
f
1
2
t
2
f
t
f

1
24
t
4
f
1
6
t
3
f

1
2
t
2
f

1
120
t
5
f

1
24
t
4
f

1
6
t
3
f

c
1
c
2
c
3

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
H = u
2
(t) +
T
f
=
1
2
u
2
(t) +
1
v +
2
a +
3
u
H
u
= 0
0 = u +
3
u =
1
2
c
1
t
2
c
2
t + c
3
a = u
a =
1
6
c
1
t
3

1
2
c
2
t
2
+ c
3
t + c
4
v = a
v =
1
24
c
1
t
4

1
6
c
2
t
3
+
1
2
c
3
t
2
+ c
4
t + c
5
x = v
x =
1
120
c
1
t
5

1
24
c
2
t
4
+
1
6
c
3
t
3
+
1
2
c
4
t
2
+ c
5
t + c
6
c
6
= x
0
c
4
= c
5
= 0 x(0) = x
0
v(0) = a(0) = 0
a =
1
6
c
1
t
3

1
2
c
2
t
2
+ c
3
t
v =
1
24
c
1
t
4

1
6
c
2
t
3
+
1
2
c
3
t
2
x =
1
120
c
1
t
5

1
24
c
2
t
4
+
1
6
c
3
t
3
+ x
0
t
f
x(t
f
) = x
f
v(t
f
) = a(t
f
) = 0

0
0
x
f
x
0

1
6
t
3
f

1
2
t
2
f
t
f
1
24
t
4
f

1
6
t
3
f
1
2
t
2
f
1
120
t
5
f

1
24
t
4
f
1
6
t
3
f

c
1
c
2
c
3

f L
t
dH
dt
=
d
dt
_
L(x(t), u(t)) +
T
(t)f (x(t), u(t))

L
x
f +
L
u
u +

T
f +
T
_
_
f
x
x
..
=f
+
f
u
u
_
_
=
H
u
u +
_
H
x
+

T
_
f

H = 0
LQR
x(t) = Ax(t) +Bu(t) (7)
u
J =
1
2
_
x
T
S
f
x
_
t=t
f
+
1
2
_
t
f
t
0
_
xQx +u
T
Ru
_
positive denite S
f
, Q, R t
f
x(t
0
)
H =
1
2
xQx +
1
2
u
T
Ru +
T
(Ax +Bu)
(t
f
) = S
f
x(t
f
)

= Qx A
T
(8)
0 = Ru +B
T

u = R
1
B
T
(9)
x
3

Plugging u into the dynamics equations:

Using the initial conditions x(0) = x

0
and v(0) = a(0) = 0 we see that
c
4
= c
5
=0 and c
6
= x
0
leaving us with
15
u =
1
2
c
1
t
2
+ c
2
t c
3

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
H = u
2
(t) +
T
f
=
1
2
u
2
(t) +
1
v +
2
a +
3
u
H
u
= 0
0 = u +
3
u =
1
2
c
1
t
2
+ c
2
t c
3
a = u
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t + c
4
v = a
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
+ c
4
t + c
5
x = v
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+
1
2
c
4
t
2
+ c
5
t + c
6
c
6
= x
0
c
4
= c
5
= 0 x(0) = x
0
v(0) = a(0) = 0
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+ x
0
t
f
x(t
f
) = x
f
v(t
f
) = a(t
f
) = 0

0
0
x
f
x
0

1
6
t
3
f
1
2
t
2
f
t
f

1
24
t
4
f
1
6
t
3
f

1
2
t
2
f

1
120
t
5
f

1
24
t
4
f

1
6
t
3
f

c
1
c
2
c
3

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
H = u
2
(t) +
T
f
=
1
2
u
2
(t) +
1
v +
2
a +
3
u
H
u
= 0
0 = u +
3
u =
1
2
c
1
t
2
+ c
2
t c
3
a = u
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t + c
4
v = a
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
+ c
4
t + c
5
x = v
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+
1
2
c
4
t
2
+ c
5
t + c
6
c
6
= x
0
c
4
= c
5
= 0 x(0) = x
0
v(0) = a(0) = 0
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+ x
0
t
f
x(t
f
) = x
f
v(t
f
) = a(t
f
) = 0

0
0
x
f
x
0

1
6
t
3
f
1
2
t
2
f
t
f

1
24
t
4
f
1
6
t
3
f

1
2
t
2
f

1
120
t
5
f

1
24
t
4
f

1
6
t
3
f

c
1
c
2
c
3

Using the final conditions, x(t

f
) = x
f
and v(t
f
) = a(t
f
) = 0 we get
3 equations in 3 unknowns (with t
f
as a parameter):

The solutions of above equations yields:

Resulting in
where
16
c
3
= (x
f
x
0
)
60
t
3
f
c
2
= (x
f
x
0
)
360
t
4
f
c
1
= (x
f
x
0
)
720
t
5
f
x
x(t) = x
0
+ (x
f
x
0
)(6
5
15
4
+ 10
3
)
=
t
t
f
10 20 30 40 50 60 70 80 90 100
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
x(t)
10 20 30 40 50 60 70 80 90 100
2
4
6
8
10
12
14
16
18
x 10
!3
v(t)
10 20 30 40 50 60 70 80 90
!5
!4
!3
!2
!1
0
1
2
3
4
5
x 10
!4
a(t)
10 20 30 40 50 60 70 80 90
!2
!1
0
1
2
3
4
5
x 10
!5
u(t)
c
3
= (x
f
x
0
)
60
t
3
f
c
2
= (x
f
x
0
)
360
t
4
f
c
1
= (x
f
x
0
)
720
t
5
f
x
x(t) = x
0
+ (x
f
x
0
)(6
5
15
4
+ 10
3
)
=
t
t
f
10 20 30 40 50 60 70 80 90 100
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
x(t)
10 20 30 40 50 60 70 80 90 100
2
4
6
8
10
12
14
16
18
x 10
!3
v(t)
10 20 30 40 50 60 70 80 90
!5
!4
!3
!2
!1
0
1
2
3
4
5
x 10
!4
a(t)
10 20 30 40 50 60 70 80 90
!2
!1
0
1
2
3
4
5
x 10
!5
u(t)
c
3
= (x
f
x
0
)
60
t
3
f
c
2
= (x
f
x
0
)
360
t
4
f
c
1
= (x
f
x
0
)
720
t
5
f
x
x(t) = x
0
+ (x
f
x
0
)(6
5
15
4
+ 10
3
)
=
t
t
f
10 20 30 40 50 60 70 80 90 100
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
x(t)
10 20 30 40 50 60 70 80 90 100
2
4
6
8
10
12
14
16
18
x 10
!3
v(t)
10 20 30 40 50 60 70 80 90
!5
!4
!3
!2
!1
0
1
2
3
4
5
x 10
!4
a(t)
10 20 30 40 50 60 70 80 90
!2
!1
0
1
2
3
4
5
x 10
!5
u(t)

1
= c
1

2
= c
1
t + c
2

3
=
1
2
c
1
t
2
c
2
t + c
3
H = u
2
(t) +
T
f
=
1
2
u
2
(t) +
1
v +
2
a +
3
u
H
u
= 0
0 = u +
3
u =
1
2
c
1
t
2
+ c
2
t c
3
a = u
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t + c
4
v = a
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
+ c
4
t + c
5
x = v
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+
1
2
c
4
t
2
+ c
5
t + c
6
c
6
= x
0
c
4
= c
5
= 0 x(0) = x
0
v(0) = a(0) = 0
a =
1
6
c
1
t
3
+
1
2
c
2
t
2
c
3
t
v =
1
24
c
1
t
4
+
1
6
c
2
t
3

1
2
c
3
t
2
x =
1
120
c
1
t
5
+
1
24
c
2
t
4

1
6
c
3
t
3
+ x
0
t
f
x(t
f
) = x
f
v(t
f
) = a(t
f
) = 0

0
0
x
f
x
0

1
6
t
3
f
1
2
t
2
f
t
f

1
24
t
4
f
1
6
t
3
f

1
2
t
2
f

1
120
t
5
f

1
24
t
4
f

1
6
t
3
f

c
1
c
2
c
3

17
c
3
= (x
f
x
0
)
60
t
3
f
c
2
= (x
f
x
0
)
360
t
4
f
c
1
= (x
f
x
0
)
720
t
5
f
x
x(t) = x
0
+ (x
f
x
0
)(6
5
15
4
+ 10
3
)
=
t
t
f
10 20 30 40 50 60 70 80 90 100
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
x(t)
10 20 30 40 50 60 70 80 90 100
2
4
6
8
10
12
14
16
18
x 10
!3
v(t)
10 20 30 40 50 60 70 80 90
!5
!4
!3
!2
!1
0
1
2
3
4
5
x 10
!4
a(t)
10 20 30 40 50 60 70 80 90
!2
!1
0
1
2
3
4
5
x 10
!5
u(t)

When t
f
is not constrained it becomes a parameter of the variation
problem and affects the cost of the solution. All the previous optimality
conditions still apply and an extra condition is added:
Proof sketch (see Bryson & Ho, sec 2.7 for real proof):

The Lagrange multipliers augmented cost is, as before

The variation is now also in t

f
. To simplify the proof we assume that x(t
f
)
is unchanged by the variation in t
f
, that is x(t)=x(t
f
) for t[t
f
, t
f
+ t].
18
What If t
f
Is Not Given?

The resulting variation is

where J is the variation given t
f

Note that the same result is reached without the assumption on x not
varying.
19

This is an example of a common case where t

f
is not given.

problem setting: given dynamical system

and some of the start/end state values x
k
(t
0
) and x
k
(t
f
) find the control that
minimizes the following simple cost

That is
20
Minimum Time Problems

The Euler Lagrange equations are

We have 2n boundary constraints for 2n differential equations, m

optimality constraints for m control variables and one constraint for t
f
.
21

Suppose we have an extra requirement from the optimal trajectory:

E.g. given fixed amount of fuel, reach the destination with an empty tank
(while obeying some optimality criterions) . N(x,u,t) would be the fuel
consumption and c the given fuel amount.

Solution: (by reduction to a regular problem with an extra state variable

that has given boundary values)
Add a new state variable, x
n+1
with the following dynamics function f
22
Integral Constraint On Path

It is clear that

Now require that

Obviously, the augmented system obeys the integral constraint

(see Bryson & Ho Sec 3.1)

23

Maximum area under curve of given length:

We formulate it as a control problem: A particle moves with constant

velocity (1) along the horizontal axis, starting at time 0 and ending at
time a. The position along the x
1
axis changes as a function of the angle
which is the control signal. The particles path (in the x
1
-t plane)

must
be p and the area under the particle is maximized.

We assume that -/2<</2 (true if p< a)

24
Integral Constraint Eg.

The corresponding equations are

The cost is
25

Solution:

Add another state variable, x

2,
and replace the integral constraint with

The Hamiltonian is
we know that it is a constant because L and f are not explicit functions
of time (see previous slide).

The Lagrange multipliers obay

26

The Lagrange equation for u gives

so
i.e

This means that under this control scheme the sine of (t) is linear.

One can then use the given boundary conditions to find H,c
1
,c
2
,

solve x
1

and show that the optimal path is an ark of a circle whose center is at

, its radius is p/2 and obeys
27
L = x
1
, = 0
u =
f
1
= x
1
= tan
x
1
(0) = 0 , x
1
(a) = 0
t
f
= a
p =

a
0
1
cos
dt
p < a
J =

a
0
x
1
dt
f
2
= x
2
=
1
cos
, x
2
(0) = 0 , x
2
(a) = p
H = L +
T
f = x
1
+
1
tan +

2
cos
= const (20)
f L H =

1
=
H
x
1
= 1

2
=
H
x
2
= 0

1
= t + c
1

2
= c
2
H
u
=
H

=

1
cos
2

+
2
sin
cos
2

= 0

1
=
2
sin = c
2
sin

Suppose we wish to find a trajectory that obeys an extra set of equality

constraints, c(x,u,t)=0.

This is no different than requiring that = 0.

We therefore treat the new constraint in a similar manner. We rewrite the

Hamiltonian to be:
where is an extra vector of Lagrange multipliers that gets the exact
same treatment as .

The rest of the Euler Lagrange constraint derivation is unchanged.

(see Bryson & Ho, sec. 3.3)

28
Equality Constraints Over
Functions of the State/Control

functions of the state variables given at t

0
and/or given or unconstrained
terminal time t
f
. (Eg. manipulate a robotic hand from one curved wall to
another along the shortest path).

via point problems (trajectory is constrained to obey some rule at a

certain time or position along its path).

problems where the Lagrange equations do not produce a constraint on

the control signal (although it is obviously constrained). Eg. bang-bang
control.

...
29
Some Other Variation Variants