## Description

1. Estimating derivatives of functions numerically. In this problem, we introduce a method to estimate

the Jacobian of a function numerically.

(a) Compute the Jacobian of the following function f : R3 ! R analytically and evaluate it at the

point x⇤ = [1 3 1]>

f(x1, x2, x3)=3×1

⇥

2×2 (x3)

3⇤

+ (x2)4

3 . (1)

Recall that the gradient of a scalar valued function is normally written as a row vector

@f(x)

@x := [@f(x)

@x1

@f(x)

@x2

@f(x)

@x3

].

(b) In calculus, you learned that @f(x⇤)

@xi := lim!0

f(x⇤+ei)f(x⇤)

, where ei, i =1:3 are the natural

basis vectors, and thus, for a fixed > 0, you might estimate the derivative by

@f(x⇤)

@xi

⇡ f(x⇤ + ei) f(x⇤)

It turns out that better numerical accuracy is usually obtained by a symmetric difference1

@f(x⇤)

@xi

⇡ f(x⇤ + ei) f(x⇤ ei)

2 . (2)

Compute a numerical approximation to the Jacobian of the function (1) using symmetric differences and report the value(s) you used for . You are not obliged to use the same for each

partial derivative. Use the same x⇤ as in part (a).

(c) When you already know the answer, it is easy to play with and come up with a good numerical

approximation to the derivative. What will you do when you do not know the answer before

hand? Let’s find out! Download the function funcPartC.p from the MATLAB folder. It is a

hidden function f : R5 ! R. Report its Jacobian at x⇤ = [1, 1, 1, 1, 1].

Here is how to call the function

x0=[1 2 3 4 5];

f=funcPartC(x0)

f = 2.1281e+01

1It can be shown that a symmetric difference is EXACT for a quadratic function. You might want to try that out.

1

2. Download the file HW10KFdata.zip from the MATLAB folder on CANVAS, and unzip it. The file

contains a discrete-time planar model of a Segway, some data, and a test file. In MATLAB, run the

command

>> SegwayTest

You should first see a low-budget animation of a Segway, just to convince you that you are working

with a physical system. If you want to know more about the model, read the file Segway560.pdf (it

is contained in the zip file); this is optional. The state vector in the model consists of the angle of the

Segway support bar with respect to the vertical, ‘, the angle of the wheel with respect to the base, ✓,

and the corresponding velocities. Hence,

x =

2

6

6

4

‘

✓

‘˙

˙

✓

3

7

7

5 .

(a) Download the file KalmanFilterDerivationUsingConditionalRVs from the Handout folder on

CANVAS. Using the data in the file SegwayData4KF.mat, implement the one-step Kalman filter

on page 9 of the handout, for the model

xk+1 = Axk + Buk + Gwk

yk = Cxk + vk.

The model data is given to you

>> clear all

>> load SegwayData4KF.mat

>> whos

In this example, the model matrices are constant: for all k 0, Ak = A, Bk = B, Gk = G,

Ck = C, and the noise covariance matrices are constant as well Rk = R, and Qk = Q. The model

comes from a linear approximation about the origin of the nonlinear Segway model. You can

learn how to compute such approximations in EECS 562 (Nonlinear Control). A deterministic

input sequence uk is provided to excite the Segway and cause it to roll around. The measurement

sequence yk corresponds to the horizontal position of the base of the Segway. x0 and P0 are the

mean and covariance of the initial condition x0. The number of measurements is N.

(b) Run your Kalman filter using the data in SegwayData4KF.mat. Turn in the following plots:

• On one plot, ‘b and ✓

b versus time, t, or versus the time index k (either is fine).

• On a second plot, ‘b˙ and b˙

✓ versus time, t or versus the time index k (either is fine).

• On a third plot, the four components of your Kalman gain K versus time, t, or versus the

time index k (either is fine).

Remark: t(k) = kTs, where Ts is the sampling period.

(c) You should see the components of your Kalman gain Kk converging to constant values. Report

these steady-state values. Then, execute the command below and compare Kss to your steadystate value of K.

[Kss,Pss] = dlqe(A,G,C,R,Q)

2 X Axt Bu

Y X TV C YC

Using Kss in place of Kk is called the steady-state Kalman filter. When the model matrices are

time invariant, it is quite common to use the steady-state Kalman filter.

3. One step update of a robot’s state using a Kalman filter. You are given a simple robot that

moves in one dimension, say along the X-axis.

t

0

= 0 t

1

= 0.1

Landmark

@ (x = 5)

z1

X

Figure 1: Robot’s motion is along the X-axis. The landmark is a fixed point at x = 5m.

Your robot starts at a position2 x0, which is normally distributed x0 ⇠ N (1, 0.25), i.e. µ0 = 1 and

⌃0 = 2 = 0.25. The state of your robot at the next time step is given by the following equation

x1 = x0 + u1 · t (state propagation model) (3)

where u1, the action taken, is also normally distributed3, u1 ⇠ N (10, 16), i.e. uˆ1 = 10, R = 16 and

t = 0.1 is the time step.

Fortunately, your robot has a reasonably accurate time of flight sensor4 which can measure the robot’s

distance from a fixed landmark. Based on the physics of the situation, you expect the output of your

robot’s sensor (i.e., the measurement) to be related to the robot’s position by

zˆt = 2

c

(5 xt), (4)

where xt is the robot’s position at time t, c is the speed of light5 and zˆt is the corresponding expected

output6(in seconds) from the sensor.

Because your sensor is a real device, it has error in its measurement; the actual sensor output is modeled

to be, z1 ⇠ N (ˆzt, Q), where Q is the uncertainty in the measurement. In this case, the manufacturer

tells you that your LiDAR has uncertainty, Q = 1018.

Problem: Suppose at time t = 0.1s your robot takes action u1 and moves according to (3). Your

sensor outputs z1 = 2.2 ⇥ 108s. Estimate the position x1 of your robot at time t = 0.1 and the

uncertainty in its position. Give your answer in the form of a normal distribution x1 ⇠ N (µ1, ⌃1).

2We have used the notation, x ⇠ N (µ, ⌃), in previous HW sets. 3Why would the control signal be random? Well, what if the feet or the wheels of your robot are slipping in sand or wet

grass? Then the control signal you command to the motor is not the action that will be applied to the body of the robot! 4Understanding how LiDAR works https://www.youtube.com/watch?v=EYbhNSUnIdU. More information about LiDARs

specifically in mobile robots can be found here, https://www.clearpathrobotics.com/2015/04/robots-101-lasers/ 5Use c = 3 ⇥ 108 m/s 6The expected output, E{z} = µz, is the mean value

3

keck A kick Bu Xo tu St It 10 0.1 2

Pictelic A Peck Att BRB Pryce o.c.io o I 0.25 0.16 0 41

Kke PlaneCT Prey Tta 0.41 Y 442 o 41 10 181

Nn 11,0 25 2

50036944

E 2 4

Untrivn

viatavirition an

4. The following problem arises in camera calibration:

xb = arg min

x>x=1

x>A>Ax.

Show that if A is real7, then xb is given by the last column of V where A = U⌃V > is the SVD of A.

5. Compute a rank 2 approximation of

A =

2

4

4.041 7.046 3.014

10.045 17.032 7.027

16.006 27.005 11.048

3

5 .

In particular, find A of smallest norm such that Ab := A + A has rank 2. The matrix norm being

used is

||M||2 =

q

max(M>M).

It is enough to give Ab, the rank 2 approximation, in your solution.

7When we write down arg min, we should also have conditions so that the answer is unique. For this problem, we need the

smallest e-value of A>A to be unique, and even then, it is only unique up to a sign (i.e., if xb is an answer, then so is xb).

Unfortunately, in the literature, you typically see the problem given as stated.

4

ATAUn Tn Un

Hints

Hints: Prob. 1 Oh, the age-old problem, how to tune parameters in algorithms? In this case, a rule of

thumb is, if decreasing by a factor of 2 does not significantly change the estimate, then you can stop.

Remark on exactness for a quadratic function: Let f(x) = ax2 + bx + c be a quadratic. Performing

the symmetric difference quotient about a point x⇤, we have

f(x⇤ + h) f(x⇤ h)

2h = a(x⇤ + h)2 + b(x⇤ + h) + c (a(x⇤ h)2 + b(x⇤ h) + c)

2h

= ax⇤2 + 2ax⇤h + ah2 + bx⇤ + bh + c ax⇤2 + 2ax⇤h ah2 bx⇤ + bh c

2h

= 4ax⇤h + 2bh

2h

= 2h(2ax⇤ + b)

2h

= 2ax⇤ + b,

where we recognize the last line as the derivative at x⇤.

Hints: Prob. 2

(a) The file testSegway illustrates how to simulate a deterministic discrete-time model using a for loop.

While the physical model is assumed to be subjected to random perturbations, the noise terms themselves are not part of the Kalman filter: it uses the noise statistics, such as the covariance matrices.

Hence, your implementation of the Kalman filter is a deterministic system and can be done in a manner

similar to the for loop in the file testSegway.

(b) If you get stuck, it is OK to post MATLAB questions on Piazza.

Hints: Prob. 3 You have all the values for the Kalman filter available to you. Recall that you have seen

in the handout on Gaussian Random Variables that if x1 and x2 are uncorrelated and Y = Ax1 + Bx2, then

µy = Aµx1 + Bµx2

⌃y = A⌃x1A> + B⌃x2B>

In this problem, you can see that A = 1 and B = 0.1, which will allow you to compute xˆ1|0 and P1|0. The

rest of the data is available to you. Plug the values into the Kalman filter equations and note that you want

to compute xˆ1|1 and P1|1, i.e. x1 ⇠ N (ˆx1|1, P1|1)

Kalman Filter Equations with action model:

Prediction Step:

xˆk+1|k = Axˆk|k + Buˆ1

Pk+1|k = APk|kA> + BRB>

Measurement Update Step:

Kk+1 = Pk+1|kC>(CPk+1|kC> + Q)

1

xˆk+1|k+1 = ˆxk+1|k + Kk+1(zk+1 zˆk+1|k)

Pk+1|k+1 = Pk+1|k Kk+1CPk+1|k

5

zˆk+1|k is the expected output if the position and measurement were not stochastic, i.e. using (4),

zˆk+1|k = 2

c

(5 xˆk+1|k)

Hints: Prob. 4 We have seen this problem before. Recall HW #2, Prob. 7-(b). How does the e-vector of

A>A corresponding to the minimum e-value relate to the columns of V ? (See statement of SVD Theorem

given in lecture).

Hints: Prob. 5 See the handout SVD_Rob501 in the resource folder…the one everyone slept through! If

this were not very useful and practical stuff, I would not pester you about it. I know, about this point in

the term, everyone is pretty tired.

6