ECE 411/511: Sensor Fusion for Robotics Mini-Project 1: Graph-Based SLAM

Problem 1: Graph-based SLAM

In this problem, you will use the graph-based SLAM algorithm to construct a map of the envi- ronment and the robot poses over time using odometry measurements and the measurements from a laser range finder to landmarks with known correspondence.

To support this task, you are provided a MATLAB framework with the following folders. • /data contains a .mat file containing the data in a structure

- simulation-pose-pose.mat contains only simulated odometry data

- simulation-pose-landmark.mat contains only simulated sensor measurement data - intel.mat and dlr.mat both contain real odometry and sensor measurement data

• /code contains the graph-based SLAM framework with stubs to complete

• /images contains the resulting maps and poses that you should get for each dataset For the project, you should complete the following stubs of code in the /code folder.

• least_squares.m

• linearization.m

• linearize_pose_pose_constraint.m

• linearize_pose_landmark_constraint.m

After implementing the missing parts, you can run the framework by running the script main.m.

Implementation tips

• While debugging, you may want to only do a few iterations of the least squares algorithm.

• Use the initial condition x0 = data.initialization.

• When computing the linearized system of equations about the current iterate, the informa- tion matrix is large but sparse.

• The functions get_pose_index(i,data) and get_landmark_index(j,data) return the set of indices in the decision vector for the ith pose and jth landmark, respectively

• Many of the functions in MATLAB can handle matrices and compute values along the rows or columns of a matrix. Some useful functions that support this are sum, log, sqrt, sin, cos, and many others.

Results. To verify your results, the table below shows the initial and final error that you should have for each dataset. The error is the least squares cost, which is the first argument returned by the function linearization(x,data).

 dataset

simulation-pose-pose

simulation-pose-landmark

intel

dlr

1/1

initial error 138,862,234 3,030 1,795,139 369,655,336

final error 8,269 474 360 56,860