Fix bad gyros calibration

This occured when calibrating at unusual attitudes. Because the gyro bias accumulators were working during the inital convergence phase, they would carry over false data. This resolves the bug by forcing the integrators to 0 until
the calibration phase is completed.

This also resolves some incorrect code and cleaned up the accelerometer filtering pathway a bit. This doesn't effect the filter behavior, but removes an unused CrossProduct() and makes it clearer what operations are occurring when filtering.

Fixes #908.