Added quaterniondifference routine
Added test for quat lerp, quat multiply can have src == dest
Error updating seems to work
Added motor rescaling, and removed rot_eulers from state