Description
1. You have seen that the motion of a robot arm with one revolute joint can be described in
state-space form as
x˙ =
0 1
0 −1/5
x +
0
1/5
u
y =
1 0
x
where the state elements are the angle (x1) and angular velocity (x2) of the joint, the input
u is a torque applied to the arm at the joint, and the output y is a measurement of the angle.
(a) Design an optimal controller of the form
u = −Kxˆ + kreferencer
for weights Qc = I and Rc = ρ, for your choice of ρ.
(b) Design an optimal observer of the form
˙xb = Axb + Bu − L(Cxb − y)
for weights Qo = γ and Ro = I, for your choice of γ.
(c) Express the closed-loop system in the form
d
dt
x
xb
= Acl
x
xb
+ Bclr
for some matrices Acl and Bcl. Compute the eigenvalues of this system.
(d) Simulate the closed-loop system in response to a reference signal r(t) = π/2 for all t ≥ 0
for the initial condition and initial state estimate
x(0) =
0
0
xb(0) =
−1
2
over a suitable time horizon.
(e) Repeat (a)-(d) as necessary, choosing ρ and γ so that the closed-loop system exhibits
a time-to-peak of less than 0.5 seconds and a peak overshoot of less than 10%. Briefly
explain your design process. Submit whatever plots you feel are necessary to justify this
process and to show that your final design satisfies the performance specification.
(f) Implement your controller and observer by modifying the function ControlLoop in the
MATLAB script hw6prob01.m. Submit only your ControlLoop function (this should
be the only part of the code that you change) and a snapshot of the figure after the
simulation has ended.
NOTE: In this problem, if you like and if it helps your design process, you are also welcome
to play around with the diagonal entries of Qc and Ro.
1
2. You have seen that the rotational motion of an axisymmetric spacecraft about its yaw and
roll axes can be described in state-space form as
x˙ =
0 λ
−λ 0
x +
1
0
u
y =
0 1
x
where the state elements x1 and x2 are the angular velocities about yaw and roll axes, the
input u is an applied torque, and the parameter λ = 9 is the relative spin rate.
(a) Design an optimal controller of the form
u = −Kxˆ
for weights Qc = I and Rc = ρ, for your choice of ρ.
(b) Design an optimal observer of the form
˙xb = Axb + Bu − L(Cxb − y)
for weights Qo = γ and Ro = I, for your choice of γ.
(c) Express the closed-loop system in the form
d
dt
x
xb
= Acl
x
xb
for some matrix Acl. Compute the eigenvalues of this system.
(d) Simulate the closed-loop system for the initial condition and initial state estimate
x(0) =
5
−5
xb(0) =
0
0
over a suitable time horizon.
(e) Repeat (a)-(d) as necessary, choosing ρ and γ so that the closed-loop system exhibits
a 2%-settling time of less than 4 seconds and so that |u(t)| never exceeds 20. (In the
context of a unit step response, “2%-settling time” would mean the minimum time ts
for which |1 − y(t)| < 0.02 for all t ≥ ts. In the context of a response to non-zero initial
conditions, we mean the minimum time ts for which |y(t)| < 0.02 for all t ≥ ts.) Briefly
explain your design process. Submit whatever plots you feel are necessary to justify this
process and to show that your final design satisfies the performance specification.
(f) Implement your controller and observer by modifying the function ControlLoop in the
MATLAB script hw6prob02.m. You may find that performance is not what you expect,
and that some design iteration is required. Here is the reason why. So far, you have
assumed that the spacecraft is axisymmetric. A real spacecraft is never quite axisymmetric. So, the actual ordinary differential equations describing the motion of the spacecraft
are nonlinear (see HW1, Problem 3). The script hw6prob02.m integrates these nonlinear
ODEs—in other words, it simulates the “real” closed-loop system given the controller
and observer you designed for the axisymmetric model of this system. Again, repeat
(a)-(d) as necessary, choosing ρ and γ so that the “real” closed-loop system has a 2%-
settling time of less than 4 seconds and so that |u(t)| never exceeds 20. Briefly explain
your design process, submitting whatever plots you feel are necessary to justify it. Also
submit your ControlLoop function and a snapshot of the figure for your final design.
NOTE: In this problem, if you like and if it helps your design process, you are also welcome
to play around with the diagonal entries of Qc and Ro.
2
m1 = 2
k1 = 8
b1 = 1
m2 = 2
k2 = 8
b2 = 1
u
3. You have seen that the spring-mass-damper system shown above can be described in statespace form as
x˙ =
0 0 1 0
0 0 0 1
−8 4 −1 0.5
4 −4 0.5 −0.5
x +
0
0
0
0.5
u
y =
0 1 0 0
x
where x1 and x2 are the absolute displacements of each mass from their equilibrium positions,
x3 and x4 are the corresponding velocities of each mass, u is the applied force, and y is a
measurement of x2.
(a) Design an optimal controller of the form
u = −Kxˆ
for weights Qc = I and Rc = ρ, for your choice of ρ.
(b) Design an optimal observer of the form
˙xb = Axb + Bu − L(Cxb − y)
for weights Qo = γ and Ro = I, for your choice of γ.
(c) Express the closed-loop system in the form
d
dt
x
xb
= Acl
x
xb
for some matrix Acl. Compute the eigenvalues of this system.
(d) Simulate the closed-loop system for the initial condition and initial state estimate
x(0) =
1
3
0
0
xb(0) =
1.1
2.9
−0.1
0.1
over a suitable time horizon.
(e) Repeat (a)-(d) as necessary, choosing ρ and γ so that the closed-loop system exhibits
a 5%-settling time of less than 5 seconds. Briefly explain your design process. Submit
whatever plots you feel are necessary to justify this process and to show that your final
design satisfies the performance specification. (You need not submit many plots.)
3
(f) Implement your controller and observer by modifying the function ControlLoop in the
MATLAB script hw6prob03.m. You should find that performance is not what you expect, and that some design iteration is required. Here is the reason why. The script
hw6prob03.m considers the case in which there is a third mass, connected to the second
mass with a third spring and damper. This mass causes disturbance input, since it exerts
forces on the second mass that were not modeled. Furthermore, the code hw6prob04.m
considers the case in which you measure the absolute displacement of this third mass, not
of the second mass—the effect is to cause measurement noise. So, think of hw6prob03.m
as simulating the “real” closed-loop system given the controller and observer you designed for a rough model of this system. Again, repeat (a)-(d) as necessary, choosing ρ
and γ so that the “real” closed-loop system has a 5%-settling time of less than 5 seconds
(doing so may be a challenge!). Briefly explain your design process, submitting whatever
plots you feel are necessary to justify it. Also submit your ControlLoop function and a
snapshot of the figure for your final design.
NOTE: In this problem, if you like and if it helps your design process, you are also welcome
to play around with the diagonal entries of Qc and Ro. (Doing so may be essential in this
problem, if you want to meet the performance specifications.)
4. In this problem, you will apply the Hamilton-Jacobi-Bellman equation to derive an optimal
observer for a scalar state-space system. As you have seen in class, this observer can be found
by solving the following optimal control problem:
minimize
d,x(t1)
s (cx(t0) − y(t0))2 +
Z t1
t0
q(cx − y)
2 + rd2
dt
subject to ˙x = ax + bu + d,
(1)
where q ≥ 0, r > 0, and s > 0. The interpretation of this problem is as follows. The current
time is t1. You have taken measurements y(t) over the time interval [t0, t1]. You are looking
for disturbance inputs d(t) over this same time interval and for the state estimate x(t1) that
best explain these measurements. You will find these things in a number of steps. Completing
these steps is the hardest bit of theory that I will ask of you this semester. Understanding
this theory makes you very dangerous.
The optimal control problem (1) has the general form
minimize
d,x(t1)
h(x(t0)) + Z t1
t0
g(x, d)dt
subject to ˙x = f(x, d).
(2)
The Hamilton-Jacobi-Bellman equation for a problem of this form is
∂v
∂t = min
d
g −
∂v
∂x
f
, (3)
where v(t, x) is the value function. The reason this expression looks slightly different from
what you saw in HW4 for optimal control design is that, in this case, v(t, x) is a “cost-to-go”
running backward in time, not forward in time. Note that h(x(t0)) looks like
p(t0)x(t0)
2 + 2m(t0)x(t0) + n(t0) (4)
for an appropriate choice of p(t0), m(t0), and n(t0), so it makes sense to guess a value function
of the form
v(t, x) = px2 + 2mx + n, (5)
where p, m, and n are functions of time. If this guess is correct, then the optimal choice of
state estimate x(t1) is simply the one that minimizes v(t1, x(t1)):
x(t1) = −p(t1)
−1m(t1), (6)
as you could verify by completing the square. (Here, we have assumed that p(t1) > 0. This
assumption turns out to be true.) What remains is to find p and m as functions of time.
Proceed as follows:
(a) Find f, g, and h by comparing (1) with (2).
(b) Find ∂v/∂t and ∂v/∂x by taking partial derivatives of (5).
(c) Plug f, g, h, ∂v/∂t, and ∂v/∂x into (3).
(d) Minimize the right-hand side of (3) with respect to d, either by “completing the square”
or by applying the “first and second derivative test.”
(e) Equate coefficients of x
2
, x, and 1 in the result. Doing so will produce three first-order
ordinary differential equations:
p˙ = · · · m˙ = · · · n˙ = · · ·
Write the boundary conditions p(t0), m(t0), and n(t0) for these ODEs by equating
coefficients of (4) with h(t0).
(f) Define ˆx = −p
−1m. Using your answer from part (e), show that
˙xˆ = axˆ + bu − `(cxˆ − y) (7)
for an appropriate choice of `. Because of (6), ˆx(t1) is the optimal state estimate, and
so (7) is an optimal observer.
CONGRATULATIONS! THIS IS A HUGE RESULT.
(g) If t0 is finite, then the gain ` in (7) is time-varying. Suppose t0 → −∞. This corresponds
to an assumption that we have been running our optimal observer for a long time. It
is a remarkable fact that p—hence, `—tends to a steady-state value. Write an equation
that characterizes this steady-state value. (There is no need to solve this equation yet.)
(h) The equation you found in part (g) should be quadratic in p. Rewrite this equation in
terms of ¯p = p
−1
. It should now look very familiar to you. (Compare it to the continuous
algebraic Riccati equation that corresponds to LQR.) Express ` in terms of ¯p. You have
now found the “steady-state” or “infinite-horizon” optimal observer.
(i) What happens to ` as (q/r) → 0 and how will the observer behave?
(j) What happens to ` as (q/r) → ∞ and how will the observer behave?
5
5. EXTRA CREDIT: OBSERVER FOR PARAMETER IDENTIFICATION
You owe it to yourself to solve this problem. It is the awesomest thing ever in the
world.
An optimal observer for a linear system. Consider the system
x˙(t) = Ax(t) + Bu(t)
y(t) = Cx(t).
As we have discussed in class, a finite-horizon optimal observer for this system has the form
˙xb(t) = Axb(t) + Bu(t) − L(t) (Cxb(t) − y(t))
for some initial guess
xb(0) = xb0
where
L(t) = P(t)C
TQo
and where P(t) is found by solving
P˙(t) = R
−1
o + AP(t) + P(t)A
T − P(t)C
TQoCP(t)
forward in time with the initial condition
P(0) = P0.
This observer is often referred to as a “continuous-time Kalman filter (KF).”
An “optimal” observer for a nonlinear system. Consider the system
x˙(t) = f(x(t), u(t))
y(t) = g(x(t))
where f and g are functions, perhaps nonlinear. It turns out that a finite-horizon “optimal”
observer for this system (“optimal” is in quotes because it’s not really optimal any more, but
it’s still good) has the form
˙xb(t) = f(xb(t), u(t)) − L(t) (g(xb(t)) − y(t))
for some initial guess
xb(0) = xb0
where
L(t) = P(t)G
TQo
and where P(t) is found by solving
P˙(t) = R
−1
o + F(xb(t), u(t))P(t) + P(t)F(xb(t), u(t))T − P(t)G(xb(t))TQoG(xb(t))P(t)
forward in time with initial condition
P(0) = P0.
6
In these expressions,
F(x, u) = ∇xf(x, u) and G(x) = ∇xg(x)
are gradients of f and g with respect to x. This observer is often referred to as a “continuoustime extended Kalman filter (EKF).”
Both the “KF” and the “EKF” are super-famous, and have been implemented in almost every
single aerospace system that has been flown since the 1960’s.
Parameter identification as observer design for a nonlinear system. Consider the
scalar state-space system
z˙ = az + bu
y = z
(8)
You will recognize this system as describing the hardware that I often bring to class (recall
the Millennium Falcon flying across the room). Two weeks ago, we discussed the problem of
parameter identification, which was to guess the value of a and b from data. At that time,
we took an approach that worked, but that was not very systematic. Now, we will derive a
different approach that is more systematic.
The trick is to treat a and b as variables. In particular, consider the nonlinear system
x˙(t) = f(x(t), u(t))
y(t) = g(x(t))
(9)
where
f(x, u) =
x2x1 + x3u
0
0
g(x) = x1
and, as usual, our notation means
x =
x1
x2
x3
.
In this system:
• the variable x1 plays the role of the angular velocity z, which behaves as in (8);
• the variable x2 plays the role of the parameter a, which is constant;
• the variable x3 plays the role of the parameter b, which is also constant.
Note that, if
x(0) =
z(0)
a
b
,
then the output of (9) will be exactly the same as the output of (8). Suppose you were to
apply the “EKF” that is described above to estimate x given y. You would recover both an
estimate xb1 of the angular velocity z and estimates xb2 and xb3 of the parameters a and b.
7
What you need to do.
• The files datasec01.txt (for the 11AM section) and datasec02.txt (for the 12PM
section) contain the experimental data that you generated in class with the hardware,
in the week between HW4 and HW5. Download the data file for your section.
• The script hw6prob05.m opens the data file. Change the name in Line 7 of the code so
that it opens the right data file. The result is that you are given an array of times t, as
well as the inputs u and outputs y at those times.
• Implement the “EKF” described above to find xb at the array of times, given
xb(0) =
0
0
0
P(0) = I.
You’ll note that this initial guess is completely wrong—in class, we found that a ≈ −1
and b ≈ 100. You’ll likely need to iterate a little bit to find values of Q and R that work.
HINT: remember that
˙xb(t) ≈
xb(t + ∆t) − xb(t)
∆t
and
P˙(t) ≈
P(t + ∆t) − P(t)
∆t
.
These approximations should allow you to integrate to find xb(t) and P(t) just like you
did in the “implementation” part of Problems 1-4.
• Create four plots:
– u(t)
– y(t) and xb1(t)
– xb2(t), i.e., your estimate of a
– xb3(t), i.e., your estimate of b
Once you see these results, you will no longer regret doing this problem. AWESOME!
8