HW 2: Kalman Filters

CS 545: Introduction to Robotics Fall 2021
HW 2: Kalman Filters
1. Answer each of the following questions. Submit your answers in a single pdf in
the root of the repository.
(a) Do robot actions always increase uncertainty? Explain your answer in 2-3
sentences.
(b) What happens if at any point in Bayesian filtering the probability of a state
assignment becomes 1? What are ways to avoid that? Explain your answer in
2-3 sentences.
(c) If an earthquake occurs, or there is a burglary, the alarm is likely to go off. If
the alarm goes off, a police may arrive. Design a Bayesian network illustrating
the causal relationships.
(d) In the recursive estimation case, what if the controls were dependent on observations? Visualize a Bayesian network showing this dependence.
(e) Why do Extended Kalman Filters (EKFs) fail in handling multiple hypotheses?
Explain your answer in 2-3 sentences.
2. We want to track the position of an object of unknown dynamics. We assume that
the object moves in one dimension. A common model for unkown dynamics is the
constant jerk model, that is assume that the acceleration is linear. The constant jerk
model can be represented as follows: x(t) = [pt
, vt
, at
, jt
]
T
, with the elements being
position, velocity, acceleration and jerk. Assuming discrete time-steps of ∆t = 0.1,
the dynamics based on that model are:
x(t + 1) = Ax(t)
A =




1 0.1 0 0
0 1 0.1 0
0 0 1 0.1
0 0 0 1




1
CS 545: Introduction to Robotics Fall 2021
We additionally assume that we can measure the object’s position with some noise:
z(t) = Cx(t) + v(t) (1)
C = [1 0 0 0] (2)
v(t) is a zero-mean Gaussian sensor noise with variance Q = 1.0.
µ0
and Σ0 represent the initial belief state of the Kalman Filter. We have that:
µ0 = (5, 1, 0, 0), Σ0 =




10 0 0 0
0 10 0 0
0 0 10 0
0 0 0 10




The true position of the object changes as follows:
p(t) = sin(0.1 ∗ t)
with p(0) = 0. You must generate your own noisy sensor data ˜p(t) by adding zero
mean Gaussian noise, vm(t), with variance Q = 1.0.
˜p(t) = p(t) + vm(t)
(a) Implement a Kalman Filter for T = 100 timesteps and plot how the error
evolves over time. Save your figure as problem2a kf estimation.png. Compute the Mean Squared Error (MSE) of the position of the object, averaged
over N = 10000 trials. Save your figure as problem2a kf mse.png.
(b) To deal with model uncertainty, a technique frequently used is adding “fictitious” process noise. That is, assume that there is noise in the state dynamics.
Rewrite your system dynamics as:
x(t + 1) = Ax(t) + w(t)
Add fictitious noise in the form of w(t) in the dynamics, with zero mean and
covariance1
:
R =




0.1 0 0 0
0 0.1 0 0
0 0 0.1 0
0 0 0 0.1




Run the KF again and compare the MSE error with the absence of fictitious
noise. Save your figure as problem2b kf mse.png.
1Note that the discretized covariance matrix of the process noise has typically also non-diagonal elements,
but we assume a diagonal matrix for simplicity
2
CS 545: Introduction to Robotics Fall 2021
3. Consider the following scalar system:
x(t + 1) = αx(t) + w(t)
z(t) = q
x(t)
2 + 1 + v(t)
w(t) is zero-mean Gaussian process noise with variance R. v(t) is zero-mean Gaussian sensor noise with variance Q.
(a) Write the equations for the Kalman filter to estimate the unknown constant α
given z(t).
Hint: The state should be augmented with the unknown parameter α.
(b) Using Q = 1, R = 0.5, write a script to test the algorithm in Python. Let true
x(0) = 2, α = 0.1. Assume initial estimates as follows, where αˆ is the initial
estimate of α:
µ0 = 1 and E[(x(0) − µ0)(x(0) − µ0)] = 2
αˆ = 2 and E[(x(0) − αˆ)(x(0) − αˆ)] = 2
How well does it work? Visualize the results for T = 20, and save your figure
as problem3 ekf estimation.png.
3