Cosmetic refactoring
adds open loop test
adds odometry init to reset timestamp and accs