diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 072b6f233a..338997c027 100644 --- a/EKF/ekf_helper.cpp +++ b/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 - Quatf quat_inv = _ev_sample_delayed.quat.inversed(); - Quatf q_error = quat_inv * _state.quat_nominal; + // calculate the quaternion delta between the EV and EKF reference frames at the EKF fusion time horizon + Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed(); 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 - Quatf quat_inv = _ev_sample_delayed.quat.inversed(); - Quatf q_error = quat_inv * _state.quat_nominal; + // calculate the quaternion delta between the EV and EKF reference frames at the EKF fusion time horizon + Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed(); 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