SO3 comp cleanup

Major cleanup of SO3 comp attitude estimator.
Should estimate gyro bias drift. Performs 3-seconds gyro bias estimation on startup, so vehicle must be still.
WIP, not ready for merging yet.
Not flown yet.