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

EKF: update all output states when aligning output time horizon filter

Browse Source
This commit is contained in:
Paul Riseborough
2020-08-16 09:39:51 +10:00
committed by Paul Riseborough
parent 3bd9415498
commit 1e6c6f2d3c
1 changed files with 1 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+1 -3
View File
@@ -353,9 +353,7 @@ void Ekf::alignOutputFilter()
_output_buffer[i].pos += pos_delta; _output_buffer[i].pos += pos_delta;
} }
_output_new.quat_nominal = q_delta * _output_new.quat_nominal; _output_new = _output_buffer.get_newest();
_output_new.quat_nominal.normalize();
// TODO: what about vel and pos of _output_new, shouldn't they be aligned too?
} }
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.