Partially resolves failing IMU unit test. 

Twan added some specific rng seeds to show the remaining part of the bug.
I would prefer to just call forwardKin with quaternion output. We could then use quatdot2angularvel to compute omega without having to go through rpydot and its numerical issues.