## 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