GPS Yaw: fix heading initialisation and add unit tests

When the antennas are not parallel to the x body axis, the GPS message
contains the angular offset but the data is already corrected in the
driver. EKF2 should then not add this offset during the initialisation.

EKF/gps_yaw_fusion.cpp

@@ -273,8 +273,8 @@ bool Ekf::resetGpsAntYaw()
 		return false;
 	}
 
-	// get measurement and correct for antenna array yaw offset
-	const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
+	// GPS yaw measurement is alreday compensated for antenna offset in the driver
+	const float measured_yaw = _gps_sample_delayed.yaw;
 
 	const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);

test/sensor_simulator/gps.cpp

@@ -58,6 +58,10 @@ void Gps::setYaw(float yaw) {
 	_gps_data.yaw = yaw;
 }
 
+void Gps::setYawOffset(float yaw_offset) {
+	_gps_data.yaw_offset = yaw_offset;
+}
+
 void Gps::setFixType(int n)
 {
 	_gps_data.fix_type = n;

test/sensor_simulator/gps.h

@@ -59,6 +59,7 @@ public:
 	void setLongitude(int32_t lon);
 	void setVelocity(const Vector3f& vel);
 	void setYaw(float yaw);
+	void setYawOffset(float yaw);
 	void setFixType(int n);
 	void setNumberOfSatellites(int n);
 	void setPdop(float pdop);

test/test_EKF_gps_yaw.cpp

@@ -54,6 +54,9 @@ class EkfGpsHeadingTest : public ::testing::Test {
 	SensorSimulator _sensor_simulator;
 	EkfWrapper _ekf_wrapper;
 
+	void runConvergenceScenario(float yaw_offset_rad = 0.f, float antenna_offset_rad = 0.f);
+	void checkConvergence(float truth, float tolerance = FLT_EPSILON);
+
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
@@ -67,6 +70,29 @@ class EkfGpsHeadingTest : public ::testing::Test {
 	}
 };
 
+void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad)
+{
+	// GIVEN: an initial GPS yaw, not aligned with the current one
+	float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad);
+
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps.setYawOffset(antenna_offset_rad);
+
+	// WHEN: the GPS yaw fusion is activated
+	_ekf_wrapper.enableGpsHeadingFusion();
+	_sensor_simulator.runSeconds(5);
+
+	// THEN: the estimate is reset and stays close to the measurement
+	checkConvergence(gps_heading, 0.05f);
+}
+
+void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance)
+{
+	const float yaw_est = _ekf_wrapper.getYawAngle();
+	EXPECT_NEAR(yaw_est, truth, math::radians(tolerance))
+		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(truth);
+}
+
 TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
 {
 	// GIVEN:EKF that fuses GPS
@@ -96,7 +122,9 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
 TEST_F(EkfGpsHeadingTest, yawConvergence)
 {
 	// GIVEN: an initial GPS yaw, not aligned with the current one
-	float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
+	const float initial_yaw = math::radians(10.f);
+	float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + initial_yaw);
+
 	_sensor_simulator._gps.setYaw(gps_heading);
 
 	// WHEN: the GPS yaw fusion is activated
@@ -104,9 +132,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 	_sensor_simulator.runSeconds(5);
 
 	// THEN: the estimate is reset and stays close to the measurement
-	float yaw_est = _ekf_wrapper.getYawAngle();
-	EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.05f))
-		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
+	checkConvergence(gps_heading, 0.05f);
 
 	// AND WHEN: the the measurement changes
 	gps_heading += math::radians(2.f);
@@ -115,9 +141,40 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 
 	// THEN: the estimate slowly converges to the new measurement
 	// Note that the process is slow, because the gyro did not detect any motion
-	yaw_est = _ekf_wrapper.getYawAngle();
-	EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f))
-		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
+	checkConvergence(gps_heading, 0.5f);
+}
+
+TEST_F(EkfGpsHeadingTest, yaw0)
+{
+	runConvergenceScenario();
+}
+
+TEST_F(EkfGpsHeadingTest, yaw60)
+{
+	const float yaw_offset_rad = math::radians(60.f);
+	const float antenna_offset_rad = math::radians(80.f);
+	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+}
+
+TEST_F(EkfGpsHeadingTest, yaw180)
+{
+	const float yaw_offset_rad = math::radians(180.f);
+	const float antenna_offset_rad = math::radians(-20.f);
+	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+}
+
+TEST_F(EkfGpsHeadingTest, yawMinus120)
+{
+	const float yaw_offset_rad = math::radians(120.f);
+	const float antenna_offset_rad = math::radians(-42.f);
+	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+}
+
+TEST_F(EkfGpsHeadingTest, yawMinus30)
+{
+	const float yaw_offset_rad = math::radians(-30.f);
+	const float antenna_offset_rad = math::radians(10.f);
+	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
 }
 
 TEST_F(EkfGpsHeadingTest, fallBackToMag)