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

EKF: correct quaternion order

Browse Source
This commit is contained in:
CarlOlsson 2019-03-11 15:40:34 +01:00 committed by Paul Riseborough
parent 000aa0de5d
commit 1d91785a8e
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/ekf_helper.cpp
View File

@ -379,7 +379,7 @@ void Ekf::resetHeight()
void Ekf::alignOutputFilter()
{
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
Quatf q_delta = _state.quat_nominal * _output_sample_delayed.quat_nominal.inversed();
Quatf q_delta = _output_sample_delayed.quat_nominal.inversed() * _state.quat_nominal;
q_delta.normalize();
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
@ -387,7 +387,7 @@ void Ekf::alignOutputFilter()
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
// loop through the output filter state history and add the deltas
// Note q1 *= q2 is equivalent to q1 = q2 * q1
// Note q1 *= q2 is equivalent to q1 = q1 * q2
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal *= q_delta;
_output_buffer[i].quat_nominal.normalize();