The odometry with high frequency IMU (greater than 400hz) can be slow since the state is re-integrated from the previous update timestep. More ideally we should cache the result of the previous odometry result and only integrate the next IMU amount. This cache would need to be invalided after update (or propagation). This caused 3x slower simulations compared to before (see a3a209b)