Add tests with IMU and 3D odometry. All tests pass and therefore this MR.
merged
mentioned in commit 80dcabd4