The 2D IMU case has been implemented and works. Work now begins on implementing the slanted plane case, but this branch can be merged already