Description
1 Theory (40 points)
In this part we are going to guide you through some steps of EKF-SLAM in math.
This will be helpful in your implementation in the next problem.
A robot is moving on the 2D ground plane. In each time step t, the robot is
controlled to move forward (the x-direction of the robot’s coordinates) dt meters,
and then rotate αt radian. The pose of the robot in the global coordinates at time
t is written as a vector pt =
h
xt yt θt
i>
, where xt and yt are the 2D coordinates
of the robot’s position, and θt
is the robot’s orientation.
1. Based on the assumption that there is no noise or error in the control system,
predict the next pose pt+1 as a nonlinear function of the current pose pt and
the control inputs dt and αt
. (5 points)
2. However, in reality there are some errors when the robot moves due to the
mechanism and the terrain. Assume the errors follow Gaussian distributions:
ex ∼ N (0, σ2
x
) in x-direction, ey ∼ N
0, σ2
y
in y-direction, and eα ∼ N (0, σ2
α
)
in rotation respectively (all in robot’s coordinates). For details, please see
Fig. 1. Now if the uncertainty of the robot’s pose at time t can be represented
as a 3-dimensional Gaussian distribution N (0, Σt), what is the predicted uncertainty of the robot at time t+1? Please express it as a Gaussian distribution
with zero mean. (5 points)
3. Consider a landmark l being observed by the robot at time t with a laser sensor
which gives a measurement of the bearing angle β (in the interval (−π, π]) and
the range r, with noise nβ ∼ N
0, σ2
β
and nr ∼ N (0, σ2
r
) respectively. Write
1
Figure 1: 0) Robot state; 1) robot moves dt along its x-axis; 2) due to noise it shifted
ex and ey along its x and y axises respectively; 3) robot rotates αt after shifting; 4)
due to noise the rotation is disturbed by eα.
down the estimated position (lx, ly) of landmark l in global coordinates as a
function of pt
, β, r, and the noise terms. (5 points)
4. According to 1.3, if we now know that l is at (lx, ly) in the global coordinates,
please predict the measurement of bearing and range based on lx, ly, pt
, and
the noise terms (use functions np.arctan2 (.) and warp2pi(.) that warps an
arbitrary angular value into the range (−π, π] if needed). (5 points)
5. An important step in EKF-SLAM is to find the measurement Jacobian Hp
with respect to the robot pose. Please apply the results in 1.4 to derive the
analytical form of Hp (Note: Hp should be a 2 × 3 matrix represented by pt
and l). (10 points)
6. For each measurement of bearing and range, we also need a measurement
Jacobian Hl with respect to its corresponding landmark l. Please again derive
the analytical form of Hl (Note: Hl should be a 2 × 2 matrix represented by
pt and l). Why do we not need to calculate the measurement Jacobian with
respect to other landmarks except for itself (Hint: based on what assumption)?
(10 points)
Note in this section, in addition to conventional linearization with additive noise
(in the form of xt+1 = g(xt
, ut) + t), you may encounter non-additive noise (in the
form of xt+1 = g(xt
, ut
, t)). Derivation of the linearization for this formulation can
be helpful. Also, all the noise reading (, n) can be regarded as infinitismal. Second
and higher order terms comprised by their products can be ignored.
2 Implementation and Evaluation (45 points)
In this part you will implement your own 2D EKF-SLAM solver in Python. The
robot in problem 1 starts with observing the surrounding environment and mea2
suring some landmarks, then executes a control input to move. The measurement
and control steps are repeated several times. For simplicity, we assume the robot
observes the same set of landmarks in the same order at each pose. We want to use
EKF-SLAM to recover the trajectory of the robot and the positions of the landmarks
from the control input and measurements.
1. Find the data file data/data.txt that contains the control inputs and measurements. Open the file to take a look. The data is in the format of
measurements= h
β1 r1 β2 r2 · · · i
, where βi
, ri correspond to landmark
li
, and control= h
d α i
(refer to problem 1 for the notations). The lines are
in sequential time order.
What is the fixed number of landmarks being observed over the entire sequence? (5 points)
2. The code/ folder contains the Python code file ekf slam.py. Follow the comments and fill in the functions warp2pi, init landmarks, predict, update
to enable the system. The visualization will show the trajectory and the landmarks with their covariances in the shape of 3-σ ellises. It will be updated
sequentially, and you may process the next step by clicking your mouse or
hitting any key.
Attach a clear figure of your visualization in the write up once all the steps
are finished. (25 points)
3. In the output figure, the magenta and blue ellipses represent the predicted and
updated uncertainties of the robot’s position (orientation not shown here) at
each time respectively. Also, the red and green ellipses represent the initial
and all the updated uncertainties of the landmarks, respectively.
Describe how EKF-SLAM improves the estimation of both the trajectory and
the map by comparing the uncertainty ellipses. (5 points)
4. Now let’s evaluate our output map. Suppose we have the ground truth positions of all the landmarks:
h
l
>
1
l
>
2
· · · l
>
k
i
=
h
3 6 3 12 7 8 7 14 11 6 11 12 i
.
Plot the ground truth positions of the landmarks in the output figure (code
already provided in function evaluate) and attach it below. Is each of them
inside the smallest corresponding ellipse? What does that mean?
Compute and report the Euclidean and Mahalanobis distances of each landmark estimation with respect to the ground truth. What do the numbers tell
you? (10 points)
3 Discussion (15 points)
1. Explain why the zero terms in the initial landmark covariance matrix become
non-zero in the final state covariance matrix (print out the final P matrix to
check it). Additionally, when setting the initial value for the full covariance
matrix P (line 201 in ekf slam.py) an assumption is made regarding certain
cross-covariances that is not necessarily correct. Can you point out what that
is? (5 points)
3
2. Play with the parameters (line 163-167). Modify each parameter σx, σy, σασb,
σr at a time and fix the others. In particular, you should set each parameter
to be 10 times larger/smaller to the original parameters to discuss how each
parameters influence the results. Attach figures for better explanation. (10
points)
3. With the same set of landmarks being observed all the time, the EKF-SLAM
system runs in constant time in each iteration. However, for real-world problems, the total number of landmarks can grow unbounded if the robot keeps
exploring new environments. This will slow down the EKF-SLAM system a lot
and become a crucial problem for real-time applications. What changes can
we make to the EKF-SLAM framework to achieve constant computation time
in each cycle or at least speed up the process (list as many possible solutions
as you can think of)? (Bonus 5 points)
4 Code Submission
Upload your ekf slam.py. Please do not upload the data/ folder or any other data
or image.