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