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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/ekf_helper.cpp
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
Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
q_error.normalize();
q_error.canonicalize();
// convert to a delta angle and apply a spike and low pass filter
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
q_error.from_axis_angle(_ev_rot_vec_filt);
q_error.normalize();
q_error.canonicalize();
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
}