ekf2: add unaided_yaw for more resilient yaw control

This estimate doesn't converge to the true yaw but can be used as a
more consistent but drifting heading source.
It can be used by a setpoint generator to adjust its heading setpoint
while the true yaw estimate is converging in order to keep a constant
course over ground.

src/modules/ekf2/EKF/output_predictor.cpp

@@ -237,6 +237,11 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 		// rotate the relative velocity into earth frame
 		_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
 	}
+
+	// update auxiliary yaw estimate
+	const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled;
+	const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
+	_unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D);
 }
 
 void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,