Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Critical big fix - states not initialised

Browse Source 
The failure to initialise the magnetometer bias states to zero can result in a large jump in yaw gyro bias and heading when a heading reset is performed.
This commit is contained in:
Paul Riseborough 2015-10-15 09:52:55 +11:00
parent e3013b493b
commit 0c61e09b70
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -185,6 +185,7 @@ void NavEKF2_core::InitialiseVariables()
gpsDriftNE = 0.0f; gpsDriftNE = 0.0f;
gpsVertVelFilt = 0.0f; gpsVertVelFilt = 0.0f;
gpsHorizVelFilt = 0.0f; gpsHorizVelFilt = 0.0f;
memset(&statesArray, 0, sizeof(statesArray));
} }
// Initialise the states from accelerometer and magnetometer data (if present) // Initialise the states from accelerometer and magnetometer data (if present)
@ -229,13 +230,20 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
// calculate initial roll and pitch orientation // calculate initial roll and pitch orientation
stateStruct.quat.from_euler(roll, pitch, 0.0f); stateStruct.quat.from_euler(roll, pitch, 0.0f);
// initialise static process state model states // initialise dynamic states
stateStruct.velocity.zero();
stateStruct.position.zero();
stateStruct.angErr.zero();
// initialise static process model states
stateStruct.gyro_bias.zero(); stateStruct.gyro_bias.zero();
stateStruct.gyro_scale.x = 1.0f; stateStruct.gyro_scale.x = 1.0f;
stateStruct.gyro_scale.y = 1.0f; stateStruct.gyro_scale.y = 1.0f;
stateStruct.gyro_scale.z = 1.0f; stateStruct.gyro_scale.z = 1.0f;
stateStruct.accel_zbias = 0.0f; stateStruct.accel_zbias = 0.0f;
stateStruct.wind_vel.zero(); stateStruct.wind_vel.zero();
stateStruct.earth_magfield.zero();
stateStruct.body_magfield.zero();
// read the GPS and set the position and velocity states // read the GPS and set the position and velocity states
readGpsData(); readGpsData();