fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix alignment of local frame

Browse Source
This commit is contained in:
kamilritz 2019-08-20 14:29:54 +02:00 committed by Paul Riseborough
parent 62fa464e4d
commit 05196db79e
1 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
EKF/ekf_helper.cpp
View File

@ -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