EKF: Make use of inverse rotation function consistent with name

The quaternion to inverse rotation matrix function has been updated so that the rotation it produces is the inverse to that produced by the matrix library and the the inverse of the quaternion is uses. This function is now used to directly calculate an inverse rotation matrix rather than calculating the forward rotation and then transposing it.

EKF/drag_fusion.cpp

@@ -82,8 +82,7 @@ void Ekf::fuseDrag()
 	rel_wind(0) = vn - vwn;
 	rel_wind(1) = ve - vwe;
 	rel_wind(2) = vd;
-	Dcmf earth_to_body(_state.quat_nominal);
-	earth_to_body = earth_to_body.transpose();
+	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 	rel_wind = earth_to_body * rel_wind;
 
 	// perform sequential fusion of XY specific forces

EKF/ekf_helper.cpp

@@ -1382,6 +1382,7 @@ Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3
 }
 
 // calculate the inverse rotation matrix from a quaternion rotation
+// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
 Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
 {
 	float q00 = quat(0) * quat(0);
@@ -1399,12 +1400,12 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
 	dcm(0, 0) = q00 + q11 - q22 - q33;
 	dcm(1, 1) = q00 - q11 + q22 - q33;
 	dcm(2, 2) = q00 - q11 - q22 + q33;
-	dcm(0, 1) = 2.0f * (q12 - q03);
-	dcm(0, 2) = 2.0f * (q13 + q02);
-	dcm(1, 0) = 2.0f * (q12 + q03);
-	dcm(1, 2) = 2.0f * (q23 - q01);
-	dcm(2, 0) = 2.0f * (q13 - q02);
-	dcm(2, 1) = 2.0f * (q23 + q01);
+	dcm(1, 0) = 2.0f * (q12 - q03);
+	dcm(2, 0) = 2.0f * (q13 + q02);
+	dcm(0, 1) = 2.0f * (q12 + q03);
+	dcm(2, 1) = 2.0f * (q23 - q01);
+	dcm(0, 2) = 2.0f * (q13 - q02);
+	dcm(1, 2) = 2.0f * (q23 + q01);
 
 	return dcm;
 }

EKF/mag_fusion.cpp

@@ -73,8 +73,7 @@ void Ekf::fuseMag()
 	SH_MAG[8] = 2.0f*magE*q3;
 
 	// rotate magnetometer earth field state into body frame
-	Dcmf R_to_body(_state.quat_nominal);
-	R_to_body = R_to_body.transpose();
+	Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal);
 
 	Vector3f mag_I_rot = R_to_body * _state.mag_I;
 

EKF/optflow_fusion.cpp

@@ -68,8 +68,7 @@ void Ekf::fuseOptFlow()
 	float Kfusion[24][2] = {}; // Optical flow Kalman gains
 
 	// get rotation matrix from earth to body
-	Dcmf earth_to_body(_state.quat_nominal);
-	earth_to_body = earth_to_body.transpose();
+	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 
 	// calculate the sensor position relative to the IMU
 	Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

EKF/sideslip_fusion.cpp

@@ -73,8 +73,7 @@ void Ekf::fuseSideslip()
 	rel_wind(1) = ve - vwe;
 	rel_wind(2) = vd;
 
-	Dcmf earth_to_body(_state.quat_nominal);
-	earth_to_body = earth_to_body.transpose(); //Why transpose?
+	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 
 	// rotate into body axes
 	rel_wind = earth_to_body * rel_wind;

EKF/terrain_estimator.cpp

@@ -204,8 +204,7 @@ void Ekf::fuseFlowForTerrain()
 	float R_LOS = calcOptFlowMeasVar();
 
 	// get rotation matrix from earth to body
-	Dcmf earth_to_body(_state.quat_nominal);
-	earth_to_body = earth_to_body.transpose();
+	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 
 	// calculate the sensor position relative to the IMU
 	Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;