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

do not reset output attitude state after heading reset to avoid jumps in attitude

Browse Source
This commit is contained in:
Roman Bapst 2016-02-25 18:20:29 +01:00
parent cd5857e900
commit 9192ced7bb
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

@ -113,9 +113,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// reset the angle error variances because the yaw angle could have changed by a significant amount
// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise