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

EKF: Also update the newest and oldest quaternion estimate in

Browse Source 
alignOutputFilter()

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson 2019-03-08 14:40:11 +01:00 committed by Paul Riseborough
parent 78122b20a6
commit 6cf512f103
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/ekf_helper.cpp
View File

@ -394,6 +394,12 @@ void Ekf::alignOutputFilter()
_output_buffer[i].vel += vel_delta;
_output_buffer[i].pos += pos_delta;
}
_output_new.quat_nominal *= q_delta;
_output_new.quat_nominal.normalize();
_output_sample_delayed.quat_nominal *= q_delta;
_output_sample_delayed.quat_nominal.normalize();
}
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.