Problem found in IDP, quaternion turns into (0,0,0,0) and an exception
is thrown.

This isn't checked in the MATLAB, implying that quaternion never should
hit zero.

Possible that quaternion variables are really really small but not zero?
Merged with most recent commit.
ang hirap mag merge, so stress