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