## Description

Remarks: In Problem 2, we start playing with the Kalman filter. Start Problem 2 early enough that you can seek MATLAB help in time!

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

⇥

2×2 − (x3)3⇤ +

(x2)4

(1)

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

.

3

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

@f(x)

:= [

@f(x)

@f(x)

@f(x)

].

@x

@x2

@x1

@x3

(b) In calculus, you learned that

@f(x⇤)

:= limδ!0

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

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

@xi

δ

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

@f(x⇤) ⇡ f(x⇤ + δei) − f(x⇤)

@xiδ

It turns out that better numerical accuracy is usually obtained by a symmetric diﬀerence1

@f(x⇤)

⇡

f(x⇤ + δei) − f(x⇤ − δei)

.

(2)

@xi

2δ

Compute a numerical approximation to the Jacobian of the function (1) using symmetric diﬀer-ences 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

1 It can be shown that a symmetric diﬀerence 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,

2 3

‘

6 ✓ 7

6 7

x = 4 ‘˙ 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, ‘ and ✓ versus time, t, or versus the time index k (either is fine).

b

b

b

˙

• On a second plot,

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

b

◦ 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 steady-state value of K.

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

X

Axt Bu

2

Y

X

TV

CYC

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

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.

0.41 Y

442 o

41 10 181

Kke PlaneCT

Prey

Tta

z1

X

t0 = 0

t1 = 0.1

Landmark

Nn 11,0 25

2

@ (x = 5)

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

2

⌃0=σ

= 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

50036944 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

2

(5 − xt),

E

2

4

(4)

zˆt =

c

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 = 10 18.

Problem: Suppose at time t = 0.1s your robot takes action u 1 and moves according to (3). Your sensor outputs z1 = 2.2 ⇥ 10 8 s. 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

6 The expected output, E{z} = µz , is the mean value

3

Untrivn

viatavirition an

ATAUn Tn Un

4. The following problem arises in camera calibration:

xb = arg

min

x

>

A

>

Ax.

7

x>x=1

Show that if A is real

, then x is given by the last column of V where A = U⌃V > is the SVD of A.

5. Compute a rank 2

approximation of

b

2

3

4.041

7.046

3.014

A =

10.045

17.032

7.027

.

4

16.006

27.005

11.048

5

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

used is

||M||2 = q

b

It is enough to give

A

λmax(M>M)

.

b, 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

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 diﬀerence quotient about a point x⇤, we have

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

=

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

2h

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 them-selves 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⌃x1 A> + B⌃x2 B>

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 = xˆk+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),

2

zˆk+1|k = 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 stuﬀ, I would not pester you about it. I know, about this point in the term, everyone is pretty tired.

6