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

EKF: Remove use of of quaternion self product operator and fix delta rotation sign

Browse Source
This commit is contained in:
Paul Riseborough
2019-06-17 19:24:55 +10:00
committed by Paul Riseborough
parent 0e946f25fd
commit a036cf82cc
3 changed files with 9 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_yaw_fusion.cpp
+2 -3
View File
@@ -367,7 +367,7 @@ bool Ekf::resetGpsAntYaw()
}
// calculate the amount that the quaternion has changed by
Quatf q_error = quat_before_reset.inversed() * _state.quat_nominal;
Quatf q_error = _state.quat_nominal * quat_before_reset.inversed();
q_error.normalize();
// convert the quaternion delta to a delta angle
@@ -407,8 +407,7 @@ bool Ekf::resetGpsAntYaw()
// add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
// Note q1 *= q2 is equivalent to q1 = q2 * q1
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
}
// apply the change in attitude quaternion to our newest quaternion estimate