Fix alignment of local frame

EKF/ekf_helper.cpp

@@ -1593,9 +1593,8 @@ void Ekf::setControlEVHeight()
 // and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {
-	// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
+	// calculate the quaternion delta between the EV and EKF reference frames at the EKF fusion time horizon
-	Quatf quat_inv = _ev_sample_delayed.quat.inversed();
+	Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
-	Quatf q_error =   quat_inv * _state.quat_nominal;
 	q_error.normalize();
 
 	// convert to a delta angle and apply a spike and low pass filter
@@ -1623,7 +1622,8 @@ void Ekf::calcExtVisRotMat()
 
 	// convert filtered vector to a quaternion and then to a rotation matrix
 	q_error.from_axis_angle(_ev_rot_vec_filt);
-	_ev_rot_mat = quat_to_invrotmat(q_error);
+	q_error.normalize();
+	_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
 
 }
 
@@ -1631,9 +1631,8 @@ void Ekf::calcExtVisRotMat()
 // and update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::resetExtVisRotMat()
 {
-	// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
+	// calculate the quaternion delta between the EV and EKF reference frames at the EKF fusion time horizon
-	Quatf quat_inv = _ev_sample_delayed.quat.inversed();
+	Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
-	Quatf q_error =   quat_inv * _state.quat_nominal;
 	q_error.normalize();
 
 	// convert to a delta angle and reset
@@ -1649,7 +1648,7 @@ void Ekf::resetExtVisRotMat()
 	}
 
 	// reset the rotation matrix
-	_ev_rot_mat = quat_to_invrotmat(q_error);
+	_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
 }
 
 // return the quaternions for the rotation from the EKF to the External Vision system frame of reference