dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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

4
EKF/ekf_helper.cpp
View File

@ -353,9 +353,7 @@ void Ekf::alignOutputFilter()
_output_buffer[i].pos += pos_delta;
}
_output_new.quat_nominal = q_delta * _output_new.quat_nominal;
_output_new.quat_nominal.normalize();
// TODO: what about vel and pos of _output_new, shouldn't they be aligned too?
_output_new = _output_buffer.get_newest();
}
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.