Fix coning handling in att_pos EKF. Fixes #2663

src/modules/ekf_att_pos_estimator/estimator_22states.cpp

@@ -96,6 +96,7 @@ AttPosEKF::AttPosEKF() :
     correctedDelVel(),
     summedDelAng(),
     summedDelVel(),
+    prevDelAng(),
     accNavMag(),
     earthRateNED(),
     angRate(),
@@ -273,12 +274,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     delAngTotal.y += correctedDelAng.y;
     delAngTotal.z += correctedDelAng.z;
 
-    // Save current measurements
-    Vector3f  prevDelAng = correctedDelAng;
-
     // Apply corrections for earths rotation rate and coning errors
     // * and + operators have been overloaded
     correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
+    prevDelAng = correctedDelAng;
 
     // Convert the rotation vector to its equivalent quaternion
     rotationMag = correctedDelAng.length();
@@ -3290,6 +3289,7 @@ void AttPosEKF::ZeroVariables()
     correctedDelAng.zero();
     summedDelAng.zero();
     summedDelVel.zero();
+    prevDelAng.zero();
     dAngIMU.zero();
     dVelIMU.zero();
     lastGyroOffset.zero();

src/modules/ekf_att_pos_estimator/estimator_22states.h

@@ -139,6 +139,7 @@ public:
     Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
     Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
     Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f prevDelAng;    ///< previous delta angle, used for coning correction
     float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
     Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
     Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)