Fixed MainDriver bug where reading of the cimudataset was wrong (the dataset being read was still the imu).
This and the previous version contains code for calculating the distance of the paths in the Breadcrumb and VINS controllers.
Merged. There were conflicts according to git, but no real conflicts.
Fixed bug where first row of every data set is being ignored because it was previously assumed that there would be a header.