attitude_estimator_q: Fix NaN protection

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -268,7 +268,7 @@ void AttitudeEstimatorQ::task_main() {
 			}
 		}
 
-		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) {
+		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
 			/* position data is actual */
 			if (gpos_updated) {
 				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
@@ -450,6 +450,8 @@ bool AttitudeEstimatorQ::update(float dt) {
 		PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
 		// Reset quaternion to last good state
 		_q = q_last;
+		_rates.zero();
+		_gyro_bias.zero();
 		return false;
 	}