[State estimation]: Fix bad attitude initialization.
[State estimation] Apply z-axis accelerometer correction to the gyro bias as well as the
gyros.
[State estimation]: Miscellaneous improvements.