NaNs have been removed from IDP State Vector, but NaNs in IDP Features
still exist.

Quaternion functions fixed, and confirmed from mostly wikipedia to be
working.

Some tiny mistake in the driver causing NaN's in the state vector fixed.
Merge branch 'master' of https://github.com/darkaeon10/EKFSLAM.git