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

Canonicalize alignment quaternion

Browse Source
This commit is contained in:
kamilritz
2019-08-21 13:59:50 +02:00
committed by Paul Riseborough
parent 933c32c921
commit 53eac6e94e
1 changed files with 2 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+2
View File
@@ -1596,6 +1596,7 @@ void Ekf::calcExtVisRotMat()
// calculate the quaternion delta between the EV and EKF 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 q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed(); Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
q_error.normalize(); q_error.normalize();
q_error.canonicalize();
// convert to a delta angle and apply a spike and low pass filter // convert to a delta angle and apply a spike and low pass filter
Vector3f rot_vec = q_error.to_axis_angle(); Vector3f rot_vec = q_error.to_axis_angle();
@@ -1623,6 +1624,7 @@ void Ekf::calcExtVisRotMat()
// convert filtered vector to a quaternion and then to a rotation matrix // convert filtered vector to a quaternion and then to a rotation matrix
q_error.from_axis_angle(_ev_rot_vec_filt); q_error.from_axis_angle(_ev_rot_vec_filt);
q_error.normalize(); q_error.normalize();
q_error.canonicalize();
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference _ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
} }