EKF: remove un-used function

EKF/ekf.h

@@ -831,14 +831,6 @@ class Ekf : public EstimatorInterface
 
 	void setVelPosFaultStatus(const int index, const bool status);
 
-	// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
-	// to the corresponding rotation matrix that rotates from frame 2 to frame 1
-	// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
-	// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
-	// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
-	// See http://www.atacolorado.com/eulersequences.doc
-	Dcmf taitBryan312ToRotMat(Vector3f &rot312);
-
 	// Declarations used to control use of the EKF-GSF yaw estimator
 
 	int64_t _emergency_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)

EKF/ekf_helper.cpp

@@ -1367,8 +1367,8 @@ void Ekf::fuse(float *K, float innovation)
 
 void Ekf::uncorrelateQuatFromOtherStates()
 {
-	P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;	
-	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;	
+	P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;
+	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
 }
 
 bool Ekf::global_position_is_valid()
@@ -1934,30 +1934,6 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
 	return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight);
 }
 
-Dcmf Ekf::taitBryan312ToRotMat(Vector3f &rot312)
-{
-		// Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence
-		const float c2 = cosf(rot312(2));
-		const float s2 = sinf(rot312(2));
-		const float s1 = sinf(rot312(1));
-		const float c1 = cosf(rot312(1));
-		const float s0 = sinf(rot312(0));
-		const float c0 = cosf(rot312(0));
-
-		Dcmf R;
-		R(0, 0) = c0 * c2 - s0 * s1 * s2;
-		R(1, 1) = c0 * c1;
-		R(2, 2) = c2 * c1;
-		R(0, 1) = -c1 * s0;
-		R(0, 2) = s2 * c0 + c2 * s1 * s0;
-		R(1, 0) = c2 * s0 + s2 * s1 * c0;
-		R(1, 2) = s0 * s2 - s1 * c0 * c2;
-		R(2, 0) = -s2 * c1;
-		R(2, 1) = s1;
-
-		return R;
-}
-
 void Ekf::runYawEKFGSF()
 {
 	float TAS;