Incomplete commit:

- Danger - modified AHRS gain values without thorough testing

- Added code for testing and optimizing AHRS gain values.

- Committing before changes to velocity and gravity because the ground control station isn't properly passing altitude and gravity as negative values.