EKF: Fix sign error in increaseQuatYawErrVariance function

EKF/ekf_helper.cpp

@@ -1661,7 +1661,8 @@ void Ekf::get_ekf2ev_quaternion(float *quat)
 // Argument is additional yaw variance in rad**2
 void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 {
-	// See DeriveYawResetEquations.m for derivation
+	// See DeriveYawResetEquations.m for derivation which produces code fragments in C_code4.txt file
+	// The auto-code was cleaned up and had terms multiplied by zero removed to give the following:
 
 	// Intermediate variables
 	float SG[3];
@@ -1683,8 +1684,8 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 	P[0][2] += yaw_variance*SQ[0]*SQ[2];
 	P[1][2] += yaw_variance*SQ[0]*SQ[1];
 	P[2][2] += yaw_variance*sq(SQ[0]);
-	P[0][3] += yaw_variance*SQ[2]*SQ[3];
-	P[1][3] += yaw_variance*SQ[1]*SQ[3];
-	P[2][3] += yaw_variance*SQ[0]*SQ[3];
+	P[0][3] -= yaw_variance*SQ[2]*SQ[3];
+	P[1][3] -= yaw_variance*SQ[1]*SQ[3];
+	P[2][3] -= yaw_variance*SQ[0]*SQ[3];
 	P[3][3] += yaw_variance*sq(SQ[3]);
 }