RigidBodyInertialMeasurementUnit

Updated RigidBodyInertialMeasurementUnit and added test. Output of IMU changed from [quat; rpydot; point acceleration in world frame] to [quat; angular velocity in body frame; point acceleration rotated to body frame]. Comparison between 2d and 3d currently fails because it's not possible to construct a RigidBodyInertialMeasurementUnit for a PlanarRigidBodyManipulator.