# Matlab代写 | CMPT 419/983 Assignment 2

### Matlab代写 | CMPT 419/983 Assignment 2

CMPT 419/983 Assignment 2
 Due date: October 28, 2019
 Submit zip file to CourSys
Programming to solve the NLP, introduced in class. You may use any
software to solve the quadratic subproblems. (The solution will be given
in cvx).
minimize sin(𝜋𝑥) sin(2𝜋𝑦)
subject to −1 ≤ 𝑥 ≤ 1
−1 ≤ 𝑦 ≤ 1
approximation of the objective, subject to linearization of the constraints,
centred around a given iterate 𝑥௞.
b) Write code to solve the above quadratic subproblem using cvx.
c) Write code to solve the entire NLP, starting from several different initial
points 𝑥଴. Make a plot showing several sequences of {𝑥௞
} starting these initial
points.
2) Differential Flatness. Consider the following simple model of the car (that
is slightly more complex than the one introduced in class:
𝑥̇ = 𝑣 cos 𝜃
𝑦̇ = 𝑣 sin 𝜃
𝜃̇ = 𝜔
𝑣̇ = 𝑎
The states consist of the position (𝑥, 𝑦), the heading 𝜃, and the
longitudinal speed 𝑣. The controls are the turn rate 𝜔 and the longitudinal
acceleration 𝑎.
a) Show that the system is differentially flat by letting 𝑧 = (𝑥, 𝑦), and deriving
the functions 𝛽 and 𝛾 such that
(𝑥, 𝑦, 𝜃, 𝑣) = 𝛽൫𝑧, 𝑧̇, … , 𝑧
(௤)൯,
(𝜔, 𝑎) = 𝛾൫𝑧, 𝑧̇, … , 𝑧
(௤)൯.
b) Consider a maneuver given by the following initial condition at 𝑡 = 0 and final
condition at 𝑡 = 𝑇 = 10:

𝑥(0)
𝑦(0)
𝜃(0)
𝑣(0)⎦

= ൦
0
0
0
1
൪ ,

𝑥(𝑇)
𝑦(𝑇)
𝜃(𝑇)
𝑣(𝑇)⎦

=

0
0
𝜋
2
1⎦

Using the basis functions 𝜓଴
(𝑡) = 1, 𝜓ଵ
(𝑡) = 𝑡, 𝜓ଶ = 𝑡

, 𝜓ଷ = 𝑡

, 𝜓ସ = 𝑡

, we
parameterize the flat outputs as follows:
𝑥(𝑡) = ෍𝑏଴௜𝜓௜(𝑡)

௜ୀ଴
𝑦(𝑡) = ෍𝑏ଵ௜𝜓௜(𝑡)

௜ୀ଴
Write a system of equations in terms of the coefficients {𝑏଴௜} and {𝑏ଵ௜} such
that the initial and final conditions are satisfied.
c) Solve the above systems of equations using a software of your choice to
obtain and plot the state and control trajectories.
3) Multiple Shooting with Casadi. Consider a
system involving an inverted pendulum on a
cart. For this system, the state is (𝑥, 𝑣, 𝜃, 𝜔),
representing the position of the cart, speed of
the cart, angle of the pendulum, and angular
speed of the pendulum, respectively. The initial
state at time 𝑡 = 0 is (0,0,0,0).
The problem parameters are as follows: 𝑀 = 0.5
kg is the mass of the cart, 𝑚 = 0.2 kg is the
mass of the pendulum, 𝑙 = 0.3 m is the half-length of the pendulum, 𝐼 =
0.006 kg∙m2
is the moment of inertial of the pendulum.
We would like to apply a force 𝐹(𝑡) such that at time 𝑡 = 5 s, the cart is
stopped at the origin, with the pendulum being stationary at the upright
position, represented by the state (0,0, 𝜋, 0). At the same time, we would
like to minimize the control effort, given by the integral ∫ 𝐹
ଶ(𝑡)𝑑𝑡 ହ
௧ୀ଴ , while
ensuring that the position of the cart stays within 1 m of the origin, |𝑥| ≤
1. The control is also limited by |𝐹(𝑡)| ≤ 0.2 at all times.
The equations of motion are given by
(𝑀 + 𝑚)𝑥̈ + 𝑏𝑥̇ + 𝑚𝑙𝜃̈cos 𝜃 − 𝑚𝑙𝜃̇ଶ sin 𝜃 = 𝐹
(𝐼 + 𝑚𝑙ଶ)𝜃̈+ 𝑚𝑔𝑙 sin 𝜃 = −𝑚𝑙𝑥̈ cos 𝜃
where 𝑏 = 0.1 N/m/s is the coefficient of friction, and 𝑔 = 9.81 m/s/s is
the acceleration due to gravity on Earth. In terms of a first-order ODE, we
have 𝑥̇ = 𝑣, 𝜃̇ = 𝜔. The rest of the system dynamics, for (𝑣, 𝜔), can be
implicitly written as
ቂ 𝑀 + 𝑚 𝑚𝑙 cos 𝜃
𝑚𝑙 cos 𝜃 𝐼 + 𝑚𝑙ଶ
ቃ ቂ𝑣̇
𝜔̇ቃ = ൤
−𝑏𝑣 + 𝑚𝑙𝜔ଶ sin 𝜃 + 𝐹
−𝑚𝑔𝑙 sin 𝜃 ൨
a) Write down the associated optimal control problem.
b) Discretize the optimal control problem using the multiple shooting method
and write down the resulting nonlinear program. Use 𝑁 = 50 time intervals,
forward Euler integration, and left endpoint first order integration (the same
schemes as presented in the lecture).
c) Using the Casadi toolbox, which can be found at
(𝑥(𝑡), 𝑣(𝑡), 𝜃(𝑡), 𝜔(𝑡)), and 𝐹(𝑡). A good starting point is the direct multiple

4) Reachability Analysis. Consider a kinematic model of a fixed-wing aircraft
flying in the presence of wind, given as follows:
𝑥̇ = 𝑣 cos 𝜃 + 𝑑௫
𝑦̇ = 𝑣 sin 𝜃 + 𝑑௬
𝜃̇ = 𝑢 + 𝑑ఏ
The state (𝑥, 𝑦, 𝜃) consists of 𝑥- and 𝑦- position, and heading 𝜃. The
aircraft flies at a constant speed 𝑣 = 0.3 km/s, and controls its turn rate 𝑢,
with |𝑢| ≤ 0.5 rad/s. The effect of wind is modeled by the disturbance 𝑑 =
൫𝑑௫, 𝑑௬, 𝑑ఏ൯ with |𝑑௫
|,ห𝑑௬ห ≤ 50 m/s, and |𝑑ఏ
The aircraft aims to arrive at its next waypoint at 𝑥 = 2 km, 𝑦 = 2 km, 𝜃 =
ଷగ

, with an acceptable tolerance of 200 m in distance and గ
ଵ଴
We now proceed to compute all possible initial states and times that can
reach the setpoint within tolerance at time 𝑡 = 0.
a) What is the target set 𝒯 representing the acceptable set of states for
reaching the waypoint within the tolerances? What is a suitable function
𝑙(𝑥, 𝑦, 𝜃) such that 𝑙(𝑥, 𝑦, 𝜃) ≤ 0 ⇔ (𝑥, 𝑦, 𝜃) ∈ 𝒯?
b) The value function 𝑉(𝑡, 𝑥, 𝑦, 𝜃) representing the set of states from which the
aircraft can reach the target set within 𝒕 seconds is the solution to the HJI
variational inequality
min ൜
𝜕𝑉
𝜕𝑡 (𝑡, 𝑥, 𝑦, 𝜃) + min
௨∈𝒰
max
ௗ∈𝒟
𝜕𝑉
𝜕𝑥 (𝑡, 𝑥, 𝑦, 𝜃)ୃ𝑓(𝑥, 𝑦, 𝜃, 𝑢, 𝑑) , 𝑙(𝑥) − 𝑉(𝑡, 𝑥)ൠ = 0
where 𝑓(𝑥, 𝑦, 𝜃, 𝑢, 𝑑) is given by the system dynamics. Determine the optimal 𝑢
and 𝑑 optimizes the min-max expression. That is find an analytic expression
for 𝑢

and 𝑑

, where
𝑢
∗(𝑡, 𝑥, 𝑦, 𝜃) = arg min
௨∈𝒰
max
ௗ∈𝒟
∇𝑉(𝑡, 𝑥, 𝑦, 𝜃)ୃ𝑓(𝑥, 𝑦, 𝜃, 𝑢, 𝑑)
𝑑
∗(𝑡, 𝑥, 𝑦, 𝜃) = arg max
ௗ∈𝒟
∇𝑉(𝑡, 𝑥, 𝑦, 𝜃)ୃ𝑓(𝑥, 𝑦, 𝜃, 𝑢

, 𝑑)
and 𝑢 ∈ 𝒰 and 𝑑 ∈ 𝒟 represents the bounds on the control and disturbances.
c) Compute 𝑉(𝑡, 𝑥, 𝑦, 𝜃) from 𝑡 = −10 to 𝑡 = 0 using the helperOC toolbox, which
can be found at https://github.com/HJReachability/helperOC. A good starting
point is tutorial.m. Use the visSetIm and proj functions to visualize the
following: 𝑉(𝑡 = −10, 𝑥, 𝑦, 𝜃), 𝑉(𝑡 = −5, 𝑥, 𝑦, 𝜃), 𝑉(𝑡 = −10, 𝑥, 𝑦, 𝜃 = 0), 𝑉(𝑡 =
−10, 𝑥, 𝑦 = 0, 𝜃).
For your computation, use the following recommended grid bounds and
resolution: 𝑥 ∈ [−2,5] with 45 grid points, 𝑦 ∈ [−2.5,4] with 45 grid points, 𝜃 ∈
[−𝜋, 𝜋] with 35 grid points.