Description
Assignment 6 covers the fundamentals of control. Problem 1 will let you revisit the basics
of LTI systems and expose you to feedback design. In problem 2, you will practice filtering
methods that cope with noisy signals.
After completing this assignment, you should be able to formulate state-space representations
of dynamical system models, analyze the stability of LTI systems, and apply design methods
to stabilize feedback control. In addition, you should be able to use Kalman filters to cope
with noisy signals in control systems.
1. Feedback Stabilization of LTI Systems (8pts)
Consider a motor with rotational inertia J1 driving an external load J2 through a torsional
spring, as shown in Figure 1. Assume that the motor delivers a torque that is proportional to
the current, τm = kI I. The dynamics of the system can be described by
J1ϕ¨1 + c( ˙ϕ1 − ϕ˙ 2) + k(ϕ1 − ϕ2) = kI I (1)
J2ϕ¨2 + c( ˙ϕ2 − ϕ˙ 1) + k(ϕ2 − ϕ1) = τd (2)
where τd is an unknown disturbance torque. Similar equations are obtained when considering
robot dynamics with flexible links.
a) (2pts) Derive a state space model for the system by introducing the normalized state variables x1 = ϕ1, x2 = ϕ2, x3 = ˙ϕ1/ω0, and x4 = ˙ϕ2/ω0, where ω0 =
p
k(J1 + J2)/(J1J2)
is the undamped natural frequency of the system when the control signal is zero.
b) (2pts) Using the following normalized parameters, J1 = 10/9, J2 = 10, c = 0.1, k = 1,
and kI = 1, verify that the eigenvalues of the open loop system are 0, 0, and -0.05±i.
c) (2pts) Design a state feedback that gives a closed loop system with eigenvalues -2,-1,-
1±i. This choice implies that the oscillatory eigenvalues will be well damped and that the
eigenvalues at the origin are replaced by eigenvalues on the negative real axis.
d) (2pts) Simulate and plot the responses of the closed loop system to step changes in the
reference signal for ϕ2 and a step change in the disturbance torque τd on the second rotor.
Shortly describe the system behavior.
Figure 1: Motor-load coupling through series spring.
1
2. Kalman Filter (7+2pts)
You are studying an insect as a model of a high-performance Micro-UAV. The insect is flying
in an experimental lab environment with controlled airflow and you want to estimate its position and velocity (all translational, neglect rotations). The position and velocity are discretized
with time step length ∆t, and described by the linear state space:
X[k] = ”
~x[k]
˙~x[k]
#
∈ R
6×1
We assume that the insect can be viewed as a point mass M. At time step k, we can control
the airflow such that the net force imposed on the insect is:
~ftotal[k] = ~u + ~v[k] ∈ R
3×1
where the control input ~u is a constant force, and ~v[k] is a white noise process:
E(~v[k]) = [0, 0, 0]T
E(~v[i]~v[j]
T
) = Rvδij
where the Kronecker delta
δij =
1 if i = j
0 if i 6= j
At each time step, a noisy measurement ~y[k] of the true position of the insect is made. Suppose that the measurement is imprecise, with an additive, zero-mean, Gaussian white noise
~w[k]:
E( ~w[k]) = [0, 0, 0]T
E( ~w[i] ~w[j]
T
) = Rwδij
The matrices Rv and Rw are the covariance matrices for the process disturbance ~v and measurement noise ~w. We assume that the initial condition is also modeled as a Gaussian random
variable with E(X[0]) = ~0, and E(X[0]X[0]T
) = P0
a) (2pts) Compute the expected value and covariance of the insect position and velocity as a
function of k.
b) (2pts) Derive the state space representation for the discrete-time linear dynamical system
of the insect.
Next, you will implement the Kalman filter and RTS smoother. Suppose the parameters are set
2
as follows: M = 0.01kg, ∆t = 1s, ~u = [0.01, 0.01, 0.01]T P0 = diag[50, 50, 50, 10, 10, 10]
Rv =
10−5 0 0
0 10−5 0
0 0 10−5
N
2
, Rw =
50 0 0
0 50 0
0 0 50
m2
and the noisy measurements are stored in the file ydata.txt. Each line is a point on the motion
trajectory; the file should have 20 points.
c) (3pts) Implement a Kalman filter to estimate the position and velocity of the insect at each
time k, given the noisy measurement of its position. Compute and output the steady-state
expected value and covariance of the error of your estimate. Plot a figure to show your
result together with the noisy measurement. What is the the steady-state expected value
and error covariance of the last point?
d) (+2pts) Implement a two-pass Rauch-Tung-Striebel (RTS) smoother which uses regular
Kalman filter as a forward pass, and adds a backward pass to refine the previous estimate.
Compare your result with (c) in terms of the expected value and covariance of the error of
your estimate, and briefly explain the difference in one sentence. What is the the steadystate expected value and error covariance of the first point?
3
