Changed a lot of stuff regarding quaternions, need to check if there are
any more problems with formatting quaternions into (r,x,y,z)

Anything else? Uh generally just debug the code in accordance to the
matlab code I guess. Mainly on predict feature positions, because thats
the most wrong part I can see.
Had to merge conflicts.