Fix SO3 initialisation and update more non_const access to unit_quaternion

This update fixes the problem I was having. It turns out I was relying on default construction which was broken when you changed the default constructor for SO3. This update fixes the scalar order so SO3 is default identity constructed.