Add floor tracking results
Finish assignment part 1