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.

msg/VehicleLocalPosition.msg

@@ -39,6 +39,7 @@ float32 ay        # East velocity derivative in NED earth-fixed frame, (metres/s
 float32 az        # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
 
 float32 heading				# Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI,  (radians)
+float32 unaided_heading                 # Same as heading but generated by integrating corrected gyro data only
 float32 delta_heading
 uint8 heading_reset_counter
 bool heading_good_for_control

src/modules/ekf2/EKF/estimator_interface.h

@@ -224,6 +224,7 @@ public:
 	int getNumberOfActiveVerticalVelocityAidingSources() const;
 
 	const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
+	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
 	const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
 	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }

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,

src/modules/ekf2/EKF/output_predictor.h

@@ -94,6 +94,9 @@ public:
 
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
+	// get a yaw value solely based on bias-removed gyro integration
+	float getUnaidedYaw() const { return _unaided_yaw; }
+
 	// get the velocity of the body frame origin in local NED earth frame
 	matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
 
@@ -184,6 +187,8 @@ private:
 
 	matrix::Vector3f _imu_pos_body{};                ///< xyz position of IMU in body frame (m)
 
+	float _unaided_yaw{};
+
 	// output complementary filter tuning
 	float _vel_tau{0.25f};                   ///< velocity state correction time constant (1/sec)
 	float _pos_tau{0.25f};                   ///< position state correction time constant (1/sec)

src/modules/ekf2/EKF2.cpp

@@ -1490,6 +1490,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
 
 	lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
+	lpos.unaided_heading = _ekf.getUnaidedYaw();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();