IMU pipeline

Created a new pipeline that simulates the accelerometers and gyros from
the pose of the robot with noise and then combines with LKT and Edge
Tracking.
Uncommenting some stuff

Left some stuff commented out that should not have been commented out.
Fixed edge tracking bugs.

Fixed bug with edge tracking where the tracker wouldn't re-initialize
when no measurement was found. Also fixed bug with calculating phi.