ROB 501 - Mathematics for Robotics HW #10

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








⇥
2x2 − (x3)3⇤ +
(x2)4
(1)
f(x1, x2, x3) = 3x1




.




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 difference1
@f(x⇤)
⇡
f(x⇤ + δei) − f(x⇤ − δei)
.
(2)
@xi






2δ



Compute a numerical approximation to the Jacobian of the function (1) using symmetric differ-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 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,
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 difference 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 stuff, I would not pester you about it. I know, about this point in the term, everyone is pretty tired.













































6