ekf_helper: restore oridinal quaternion decorrelation

The intent of the function "uncorrelateQuatStates()" is to uncorrelathe quaternions from the other states but not between each other as they are and should remain linked alltogether.
Clearing the quaternions to quaternion covariances introduced unstabilities in other states (especially accel biases).

EKF/ekf_helper.cpp

@@ -1372,7 +1372,25 @@ void Ekf::fuse(float *K, float innovation)
 
 void Ekf::uncorrelateQuatStates()
 {
+	// save 4x4 elements
+	uint32_t row;
+	uint32_t col;
+	float variances[4][4];
+	for (row = 0; row < 4; row++) {
+		for (col = 0; col < 4; col++) {
+			variances[row][col] = P(row, col);
+		}
+	}
+
+	// zero rows and columns
 	P.uncorrelateCovariance<4>(0);
+
+	// restore 4x4 elements
+	for (row = 0; row < 4; row++) {
+		for (col = 0; col < 4; col++) {
+			P(row, col) = variances[row][col];
+		}
+	}
 }
 
 bool Ekf::global_position_is_valid()