From 0c61e09b707a5a71fd91fd19f17588d6f835d166 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 15 Oct 2015 09:52:55 +1100 Subject: [PATCH] AP_NavEKF2: Critical big fix - states not initialised 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. --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 4fec03cab7..dbdd492a9b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -185,6 +185,7 @@ void NavEKF2_core::InitialiseVariables() gpsDriftNE = 0.0f; gpsVertVelFilt = 0.0f; gpsHorizVelFilt = 0.0f; + memset(&statesArray, 0, sizeof(statesArray)); } // 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 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_scale.x = 1.0f; stateStruct.gyro_scale.y = 1.0f; stateStruct.gyro_scale.z = 1.0f; stateStruct.accel_zbias = 0.0f; stateStruct.wind_vel.zero(); + stateStruct.earth_magfield.zero(); + stateStruct.body_magfield.zero(); // read the GPS and set the position and velocity states readGpsData();