## Description

## Instructions

• This assignment is designed to get you familiar with localization using an Extended Kalman filter.

• Code can be written in Python or C++ only. Make sure your code is modular since you might be reusing

them for future assignments.

• Submit your code files and a report (or as a single Jupyter notebook if you wish) as a zip file, named as

〈team id〉 assignment4.zip.

• The report should include all outputs and results and a description of your approach. It should also briefly

describe what each member worked on. The scoring will be primarily based on the report.

• Refer to the late days policy and plagiarism policy in the course information document.

## Q) Localization using sensor fusion

A ground robot is driving

amongst a set of known landmarks. The robot has a wheel odometer

that measures its translational and rotational speeds, and a laser rangefinder that measures the range

and bearing to the landmarks. Both the sensors are noisy.

Your task in this assignment is to estimate the

2D pose of the robot throughout its traverse using these sensor measurements by applying an Extended

Kalman filter.

In this problem, the motion model of the robot and the observation model are non-linear and are as

follows,

xk

yk

θk

=

xk−1

yk−1

θk−1

+ dt

cos θk−1 0

sin θk−1 0

0 1

vk

ωk

+ wk

r

l

k

φ

l

k

=

p

(xl − xk − d cos θk)

2 + (yl − yk − d sin θk)

2

atan2(yl − yk − d sin θk, xl − xk − d cos θk) − θk

+ nk

(xk, yk, θk) is the robot’s 2D pose that we want to estimate, (vk, ωk) is the robot’s translational and

rotational speed measured using its odometer, dt is the sampling period, wk is the process noise, (r

l

k

, φl

k

)

is the range and bearing to the landmark l, (x

l

, yl

) is the position of the landmark l, nk is the sensor noise,

and d is the distance between the robot center and the laser rangefinder. Note that at each timestep, k,

there can be many laser scan observations.

Dataset:

The data is provided to you in the form of a numpy archive file (dataset.npz). It contains the following

variables,

t: a 12609 × 1 array containing the data timestamps [s].

x true: a 12609 × 1 array containing the true x-position, xk, of the robot [m].

y true: a 12609 × 1 array containing the true y-position, yk, of the robot [m].

th true: a 12609 × 1 array containing the true heading, θk, of the robot [rad].

l: a 17 × 2 array containing the true xy-positions, (x

l

, yl

), of all the 17 landmarks [m].

r: a 12609 × 17 array containing the range, r

l

k

, between the robot’s laser rangefinder and each

landmark as measured by the laser rangefinder sensor [m] (a range of 0 indicates the landmark was

not visible at that timestep).

r var: the variance of the range readings (based on groundtruth) [m2

].

b: a 12609 × 17 array containing the bearing, φ

l

k

, to each landmark in a frame attached to the

laser rangefinder, as measured by the laser rangefinder sensor [rad] (if the range is 0 it indicates the

landmark was not visible at that timestep and thus the bearing is meaningless). The measurements

are spread over a 240° horizontal-FoV, and the bearing is 0° when the landmark is straight ahead

of the rangefinder.

b var: the variance of the bearing readings (based on groundtruth) [rad2

].

v: a 12609×1 array containing the translational speed, vk, of the robot as measured by the robot’s

odometers [m/s].

v var: the variance of the translational speed readings (based on groundtruth) [m2/s2

].

om: a 12609×1 array containing the rotational speed, ωk, of the robot as measured by the robot’s

odometers [rad/s].

om var: the variance of the rotational speed readings (based on groundtruth) [rad2/s2

].

d: the distance, d, between the center of the robot and the laser rangefinder [m].

Use dataset = np.load(‘dataset.npz’) to load the archive, and dataset[‘array name’] to access

each individual array contained within it. The groudtruth data, including the locations of the landmarks,

were captured using a ten-camera motion capture system

. Use the groundtruth pose of the robot

as ˆx0 to initialize your filter and choose Pˆ

0 = diag{1, 1, 0.1}. Also assume a uniform 0.1 second sampling

period.

Acknowledgement: This is from the “Lost in the woods” dataset created by Prof. Tim Barfoot.

Deliverables

Code aside, your report must include the following,

• A description of your filter design including expressions for all the Jacobians involved.

• A plot of the trajectory estimated only using the wheel odometer measurements, along with the

groundtruth trajectory and the landmark locations. This is just to appreciate why we cannot rely

only on wheel odometry.

• A plot of your EKF-estimated robot trajectory, along with the groundtruth trajectory and the

landmark locations.

• [Bonus] A video showing your EKF estimate as the robot drives around, as a dot on a map,

along with the true robot position and the landmark locations. Also show the estimated 3-sigma

covariance ellipse associated with every position.