Fix gyro offset calculation

2 files changed, 20 insertions, 14 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 6ca10e56a..c16e72ea0 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -784,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU;
-	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU;
-	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU;
+	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
-	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU;
-	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU;
-	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU;
+	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
 
 	/* lazily publish the attitude only once available */
 	if (_att_pub > 0) {
@@ -1069,7 +1069,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
-	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
+	printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
 	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
 	printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
 	       (double)_baro_gps_offset);
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index c313e83af..4fac83db2 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -210,7 +210,7 @@ void AttPosEKF::InitialiseParameters()
 
     yawVarScale = 1.0f;
     windVelSigma = 0.1f;
-    dAngBiasSigma = 5.0e-7f;
+    dAngBiasSigma = 5.0e-7;
     dVelBiasSigma = 1e-4f;
     magEarthSigma = 3.0e-4f;
     magBodySigma  = 3.0e-4f;
@@ -414,9 +414,10 @@ void AttPosEKF::CovariancePrediction(float dt)
     // calculate covariance prediction process noise
     for (uint8_t i= 0; i<4;  i++) processNoise[i] = 1.0e-9f;
     for (uint8_t i= 4; i<10;  i++) processNoise[i] = 1.0e-9f;
-    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
     // scale gyro bias noise when on ground to allow for faster bias estimation
-    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
+    float gyroBiasScale = (_onGround) ? 2.0f : 1.0f;
+
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale;
     processNoise[13] = dVelBiasSigma;
     if (!inhibitWindStates) {
         for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
@@ -2706,6 +2707,11 @@ void AttPosEKF::ConstrainStates()
     // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
+    // Constrain dtIMUfilt
+    if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
+        dtIMUfilt = dtIMU;
+    }
+
     // Constrain quaternion
     for (size_t i = 0; i <= 3; i++) {
         states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
@@ -2727,11 +2733,11 @@ void AttPosEKF::ConstrainStates()
 
     // Angle bias limit - set to 8 degrees / sec
     for (size_t i = 10; i <= 12; i++) {
-        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+        states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt);
     }
 
     // Constrain delta velocity bias
-    states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
+    states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt);
 
     // Wind velocity limits - assume 120 m/s max velocity
     for (size_t i = 14; i <= 15; i++) {
@@ -2795,8 +2801,8 @@ bool AttPosEKF::GyroOffsetsDiverged()
 
     // Protect against division by zero
     if (delta_len > 0.0f) {
-        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
-        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
+        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f);
+        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt;
     }
 
     bool diverged = (delta_len_scaled > 1.0f);