EKF filter update. Now correctly scaling variance (well, if you could call this ever "correct") for repeated fusion of the same data quantity.

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -96,7 +96,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
 
 __EXPORT uint32_t millis();
 
+__EXPORT uint64_t getMicros();
+
 static uint64_t IMUmsec = 0;
+static uint64_t IMUusec = 0;
 static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
 
 uint32_t millis()
@@ -104,6 +107,11 @@ uint32_t millis()
 	return IMUmsec;
 }
 
+uint64_t getMicros()
+{
+	return IMUusec;
+}
+
 class FixedwingEstimator
 {
 public:
@@ -850,7 +858,8 @@ FixedwingEstimator::task_main()
 			}
 
 			_last_sensor_timestamp = _gyro.timestamp;
-			IMUmsec = _gyro.timestamp / 1e3f;
+			IMUmsec = _gyro.timestamp / 1e3;
+			IMUusec = _gyro.timestamp;
 
 			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
 			_last_run = _gyro.timestamp;
@@ -914,7 +923,8 @@ FixedwingEstimator::task_main()
 
 			// Copy gyro and accel
 			_last_sensor_timestamp = _sensor_combined.timestamp;
-			IMUmsec = _sensor_combined.timestamp / 1e3f;
+			IMUmsec = _sensor_combined.timestamp / 1e3;
+			IMUusec = _sensor_combined.timestamp;
 
 			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
@@ -994,8 +1004,6 @@ FixedwingEstimator::task_main()
 
 			if (gps_updated) {
 
-				last_gps = _gps.timestamp_position;
-
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
 
@@ -1008,11 +1016,17 @@ FixedwingEstimator::task_main()
 					_gps_start_time = hrt_absolute_time();
 
 					/* check if we had a GPS outage for a long time */
-					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
+					float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
+
+					const float pos_reset_threshold = 5.0f; // seconds
+
+					/* timeout of 5 seconds */
+					if (gps_elapsed > pos_reset_threshold) {
 						_ekf->ResetPosition();
 						_ekf->ResetVelocity();
 						_ekf->ResetStoredStates();
 					}
+					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
 
 					/* fuse GPS updates */
 
@@ -1044,6 +1058,8 @@ FixedwingEstimator::task_main()
 
 					newDataGps = true;
 
+					last_gps = _gps.timestamp_position;
+
 				}
 
 			}
@@ -1052,8 +1068,15 @@ FixedwingEstimator::task_main()
 			orb_check(_baro_sub, &baro_updated);
 
 			if (baro_updated) {
+
+				hrt_abstime baro_last = _baro.timestamp;
+
 				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 
+				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
+
+				_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
+
 				_ekf->baroHgt = _baro.altitude;
 
 				if (!_baro_init) {

src/modules/ekf_att_pos_estimator/estimator_23states.cpp

@@ -2,6 +2,11 @@
 #include <string.h>
 #include <stdio.h>
 #include <stdarg.h>
+#include <math.h>
+
+#ifndef M_PI_F
+#define M_PI_F ((float)M_PI)
+#endif
 
 #define EKF_COVARIANCE_DIVERGED 1.0e8f
 
@@ -38,6 +43,7 @@ AttPosEKF::AttPosEKF() :
     resetStates{},
     storedStates{},
     statetimeStamp{},
+    lastVelPosFusion(millis()),
     statesAtVelTime{},
     statesAtPosTime{},
     statesAtHgtTime{},
@@ -59,7 +65,12 @@ AttPosEKF::AttPosEKF() :
     accel(),
     dVelIMU(),
     dAngIMU(),
-    dtIMU(0),
+    dtIMU(0.005f),
+    dtIMUfilt(0.005f),
+    dtVelPos(0.01f),
+    dtVelPosFilt(0.01f),
+    dtHgtFilt(0.01f),
+    dtGpsFilt(0.1f),
     fusionModeGPS(0),
     innovVelPos{},
     varInnovVelPos{},
@@ -260,6 +271,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 
     // Constrain states (to protect against filter divergence)
     ConstrainStates();
+
+    // update filtered IMU time step length
+    dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
 }
 
 void AttPosEKF::CovariancePrediction(float dt)
@@ -962,6 +976,21 @@ void AttPosEKF::CovariancePrediction(float dt)
         ConstrainVariances();
 }
 
+void AttPosEKF::updateDtGpsFilt(float dt)
+{
+    dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
+}
+
+void AttPosEKF::updateDtHgtFilt(float dt)
+{
+    dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
+}
+
+void AttPosEKF::updateDtVelPosFilt(float dt)
+{
+    dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
+}
+
 void AttPosEKF::FuseVelposNED()
 {
 
@@ -998,6 +1027,18 @@ void AttPosEKF::FuseVelposNED()
 // associated with sequential fusion
     if (fuseVelData || fusePosData || fuseHgtData)
     {
+        uint64_t tNow = getMicros();
+        updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
+        lastVelPosFusion = tNow;
+
+        // scaler according to the number of repetitions of the
+        // same measurement in one fusion step
+        float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
+
+        // scaler according to the number of repetitions of the
+        // same measurement in one fusion step
+        float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
+
         // set the GPS data timeout depending on whether airspeed data is present
         if (useAirspeed) horizRetryTime = gpsRetryTime;
         else horizRetryTime = gpsRetryTimeNoTAS;
@@ -1010,12 +1051,12 @@ void AttPosEKF::FuseVelposNED()
         // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
         velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
         posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
-        R_OBS[0] = sq(vneSigma) + sq(velErr);
+        R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
         R_OBS[1] = R_OBS[0];
-        R_OBS[2] = sq(vdSigma) + sq(velErr);
+        R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
-        R_OBS[3] = sq(posNeSigma) + sq(posErr);
+        R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
         R_OBS[4] = R_OBS[3];
-        R_OBS[5] = sq(posDSigma) + sq(posErr);
+        R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
 
         // calculate innovations and check GPS data validity using an innovation consistency check
         if (fuseVelData)
@@ -1995,9 +2036,8 @@ void AttPosEKF::FuseOptFlow()
             varInnovOptFlow[0] = 1.0f/SK_LOS[0];
             innovOptFlow[0] = losPred[0] - losData[0];
 
-            // reset the observation index to 0 (we start by fusing the X
+            // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
-            // measurement)
+            obsIndex = 1;
-            obsIndex = 0;
             fuseOptFlowData = false;
         }
         else if (obsIndex == 1) // we are now fusing the Y measurement
@@ -2041,6 +2081,10 @@ void AttPosEKF::FuseOptFlow()
             Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
             varInnovOptFlow[1] = 1.0f/SK_LOS[1];
             innovOptFlow[1] = losPred[1] - losData[1];
+
+            // reset the observation index
+            obsIndex = 0;
+            fuseOptFlowData = false;
         }
 
         // Check the innovation for consistency and don't fuse if > 3Sigma
@@ -2102,10 +2146,9 @@ void AttPosEKF::FuseOptFlow()
                 P[i][j] = P[i][j] - KHP[i][j];
             }
         }
+        ForceSymmetry();
+        ConstrainVariances();
     }
-    obsIndex = obsIndex + 1;
-    ForceSymmetry();
-    ConstrainVariances();
 }
 
 void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
@@ -3006,9 +3049,14 @@ void AttPosEKF::ZeroVariables()
 {
 
     // Initialize on-init initialized variables
-
+    dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
+    dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
+    dtGpsFilt = 1.0f / 5.0f;
+    dtHgtFilt = 1.0f / 100.0f;
     storeIndex = 0;
 
+    lastVelPosFusion = millis();
+
     // Do the data structure init
     for (unsigned i = 0; i < n_states; i++) {
         for (unsigned j = 0; j < n_states; j++) {

src/modules/ekf_att_pos_estimator/estimator_23states.h

@@ -116,6 +116,9 @@ public:
     float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
     uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
 
+    // Times
+    uint64_t lastVelPosFusion;  // the time of the last velocity fusion, in the standard time unit of the filter
+
     float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
     float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
     float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
@@ -140,7 +143,12 @@ public:
     Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
     Vector3f dVelIMU;
     Vector3f dAngIMU;
-    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
+    float dtIMUfilt; // average time between IMU measurements (sec)
+    float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
+    float dtVelPosFilt; // average time between position / velocity fusion steps
+    float dtHgtFilt; // average time between height measurement updates
+    float dtGpsFilt; // average time between gps measurement updates
     uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
     float innovVelPos[6]; // innovation output
     float varInnovVelPos[6]; // innovation variance output
@@ -211,6 +219,9 @@ public:
 
     unsigned storeIndex;
 
+void updateDtGpsFilt(float dt);
+
+void updateDtHgtFilt(float dt);
 
 void  UpdateStrapdownEquationsNED();
 
@@ -300,6 +311,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
 
 protected:
 
+void updateDtVelPosFilt(float dt);
+
 bool FilterHealthy();
 
 bool GyroOffsetsDiverged();
@@ -314,3 +327,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
 
 uint32_t millis();
 
+uint64_t getMicros();
+