Fixed the a priori mean attitude calculation

The attitude component of the a priori mean wasn’t being normalised
correctly. Also, it’s now used in the calculation of the a posteriori
attitude estimate like it’s supposed to be.