Rewrote the filter mainloop to match the order in the offboard simulator, added a number of scaling fixes, initializing all structs correctly

src/modules/fw_att_pos_estimator/estimator.cpp

@@ -1,5 +1,8 @@
 #include "estimator.h"
 
+// For debugging only
+#include <stdio.h>
+
 // Global variables
 float KH[n_states][n_states]; //  intermediate result used for covariance updates
 float KHP[n_states][n_states]; // intermediate result used for covariance updates
@@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m)
 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
+float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+
 float innovMag[3]; // innovation output
 float varInnovMag[3]; // innovation variance output
 Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
 float innovVtas; // innovation output
 float varInnovVtas; // innovation variance output
 float VtasMeas; // true airspeed measurement (m/s)
-float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
 float latRef; // WGS-84 latitude of reference point (rad)
 float lonRef; // WGS-84 longitude of reference point (rad)
 float hgtRef; // WGS-84 height of reference point (m)
@@ -1125,6 +1129,8 @@ void FuseVelposNED()
             }
         }
     }
+
+    //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
 }
 
 void FuseMagnetometer()
@@ -1586,13 +1592,15 @@ float sq(float valIn)
 }
 
 // Store states in a history array along with time stamp
-void StoreStates()
+void StoreStates(uint64_t timestamp_ms)
 {
     static uint8_t storeIndex = 0;
-    if (storeIndex == data_buffer_size) storeIndex = 0;
-    for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i];
-    statetimeStamp[storeIndex] = millis();
-    storeIndex = storeIndex + 1;
+    for (unsigned i=0; i<n_states; i++)
+        storedStates[i][storeIndex] = states[i];
+    statetimeStamp[storeIndex] = timestamp_ms;
+    storeIndex++;
+    if (storeIndex == data_buffer_size)
+        storeIndex = 0;
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
@@ -1791,8 +1799,29 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
     initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
 }
 
-void InitialiseFilter()
+void InitialiseFilter(float initvelNED[3])
 {
+    // Do the data structure init
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
+            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+            P[i][j] = 0.0f; // covariance matrix
+        }
+
+        Kfusion[i] = 0.0f; // Kalman gains
+        states[i] = 0.0f; // state matrix
+    }
+
+    for (unsigned i = 0; i < data_buffer_size; i++) {
+
+        for (unsigned j = 0; j < n_states; j++) {
+
+        }
+
+        statetimeStamp[i] = 0;
+    }
+
     // Calculate initial filter quaternion states from raw measurements
     float initQuat[4];
     Vector3f initMagXYZ;
@@ -1809,10 +1838,6 @@ void InitialiseFilter()
     initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
     initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
 
-    // calculate initial velocities
-    float initvelNED[3];
-    calcvelNED(initvelNED, gpsCourse, gpsGndSpd, gpsVelD);
-
     //store initial lat,long and height
     latRef = gpsLat;
     lonRef = gpsLon;

src/modules/fw_att_pos_estimator/estimator.h

@@ -110,7 +110,6 @@ extern float EAS2TAS; // ratio f true to equivalent airspeed
 
 // GPS input data variables
 extern float gpsCourse;
-extern float gpsGndSpd;
 extern float gpsVelD;
 extern float gpsLat;
 extern float gpsLon;
@@ -122,7 +121,7 @@ extern float baroHgt;
 
 extern bool statesInitialised;
 
-const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+const float covTimeStepMax = 0.02f; // maximum time allowed between covariance predictions
 const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions
 
 void  UpdateStrapdownEquationsNED();
@@ -144,7 +143,7 @@ float sq(float valIn);
 void quatNorm(float quatOut[4], float quatIn[4]);
 
 // store staes along with system time stamp in msces
-void StoreStates();
+void StoreStates(uint64_t timestamp_ms);
 
 // recall stste vector stored at closest time to the one specified by msec
 void RecallStates(float statesForFusion[n_states], uint32_t msec);
@@ -169,7 +168,7 @@ void OnGroundCheck();
 
 void CovarianceInit();
 
-void InitialiseFilter();
+void InitialiseFilter(float initvelNED[3]);
 
 uint32_t millis();
 

src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

@@ -92,7 +92,7 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
 __EXPORT uint32_t millis();
 
 static uint64_t last_run = 0;
-static uint32_t IMUmsec = 0;
+static uint64_t IMUmsec = 0;
 
 uint32_t millis()
 {
@@ -383,11 +383,11 @@ FixedwingEstimator::task_main()
 
 	/* rate limit gyro updates to 50 Hz */
 	/* XXX remove this!, BUT increase the data buffer size! */
-	orb_set_interval(_gyro_sub, 17);
+	orb_set_interval(_gyro_sub, 6);
 	#else
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	/* XXX remove this!, BUT increase the data buffer size! */
-	orb_set_interval(_sensor_combined_sub, 17);
+	orb_set_interval(_sensor_combined_sub, 6);
 	#endif
 
 	parameters_update();
@@ -459,6 +459,10 @@ FixedwingEstimator::task_main()
 
 			perf_count(_perf_gyro);
 
+			/**
+			 *    PART ONE: COLLECT ALL DATA
+			 **/
+
 			hrt_abstime last_sensor_timestamp;
 
 			/* load local copies */
@@ -498,7 +502,10 @@ FixedwingEstimator::task_main()
 
 			dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
 			lastAngRate = angRate;
-			dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+			// dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+			dVelIMU.x = 0.0f;
+			dVelIMU.y = 0.0f;
+			dVelIMU.z = 0.0f;
 			lastAccel = accel;
 
 
@@ -522,7 +529,7 @@ FixedwingEstimator::task_main()
 			last_run = _sensor_combined.timestamp;
 
 			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
+			if (deltaT > 1.0f || deltaT < 0.000001f)
 				deltaT = 0.01f;
 
 			// Always store data, independent of init status
@@ -549,41 +556,13 @@ FixedwingEstimator::task_main()
 
 			#endif
 
-			if (_initialized) {
+			bool airspeed_updated;
+			orb_check(_airspeed_sub, &airspeed_updated);
+			if (airspeed_updated) {
+				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+				perf_count(_perf_airspeed);
 
-				/* predict states and covariances */
-
-				/* run the strapdown INS every sensor update */
-				UpdateStrapdownEquationsNED();
-
-				/* store the predictions */
-				StoreStates();
-
-				/* evaluate if on ground */
-				OnGroundCheck();
-
-				/* prepare the delta angles and time used by the covariance prediction */
-				summedDelAng = summedDelAng + correctedDelAng;
-				summedDelVel = summedDelVel + correctedDelVel;
-				dt += dtIMU;
-
-				/* predict the covairance if the total delta angle has exceeded the threshold
-				 * or the time limit will be exceeded on the next measurement update
-				 */
-				if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
-					CovariancePrediction();
-					summedDelAng = summedDelAng.zero();
-					summedDelVel = summedDelVel.zero();
-					dt = 0.0f;
-				}
-			}
-
-			bool baro_updated;
-			orb_check(_baro_sub, &baro_updated);
-			if (baro_updated) {
-				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-
-				baroHgt = _baro.altitude;
+				VtasMeas = _airspeed.true_airspeed_m_s;
 			}
 
 			bool gps_updated;
@@ -592,65 +571,36 @@ FixedwingEstimator::task_main()
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
 
-				if (_gps.fix_type > 2) {
+				if (_gps.fix_type < 3) {
+					gps_updated = false;
+				} else {
 					/* fuse GPS updates */
 
 					//_gps.timestamp / 1e3;
 					GPSstatus = _gps.fix_type;
-					gpsCourse = _gps.cog_rad;
-					gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s);
-					gpsVelD = _gps.vel_d_m_s;
+					velNED[0] = _gps.vel_n_m_s;
+					velNED[1] = _gps.vel_e_m_s;
+					velNED[2] = _gps.vel_d_m_s;
+
+					// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
+
 					gpsLat = math::radians(_gps.lat / (double)1e7);
 					gpsLon = math::radians(_gps.lon / (double)1e7)  - M_PI;
 					gpsHgt = _gps.alt / 1e3f;
 
-					if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) {
-						InitialiseFilter();
-						_initialized = true;
-
-						warnx("init done.");
-						continue;
-					}
-
-					if (_initialized) {
-
-						/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
-						velNED[0] = _gps.vel_n_m_s;
-						velNED[1] = _gps.vel_e_m_s;
-						velNED[2] = _gps.vel_d_m_s;
-						calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
-
-						posNE[0] = posNED[0];
-						posNE[1] = posNED[1];
-						hgtMea =  -posNED[2];
-
-						// set flags for further processing 
-						fuseVelData = true;
-						fusePosData = true;
-						fuseHgtData = true;
-
-						/* recall states after adjusting for delays */
-						RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-						RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-						RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
-
-						/* run the actual fusion */
-						FuseVelposNED();
-					}
-
-				} else {
-
-					/* do not fuse anything, we got no position / vel update */
-					fuseVelData = false;
-					fusePosData = false;
-					fuseHgtData = false;
 				}
 
-			} else {
-				/* do not fuse anything, we got no position / vel update */
-				fuseVelData = false;
-				fusePosData = false;
-				fuseHgtData = false;
+			}
+
+			bool baro_updated;
+			orb_check(_baro_sub, &baro_updated);
+			if (baro_updated) {
+				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+				baroHgt = _baro.altitude;
+
+				// Could use a blend of GPS and baro alt data if desired
+				hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt;
 			}
 
 			#ifndef SENSOR_COMBINED_SUB
@@ -689,41 +639,107 @@ FixedwingEstimator::task_main()
 
 				#endif
 
-				if (_initialized) {
+			}
 
-					fuseMagData = true;
-					RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
-					FuseMagnetometer();
+
+			/**
+			 *    PART TWO: EXECUTE THE FILTER
+			 **/
+
+			if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) {
+				InitialiseFilter(velNED);
+				_initialized = true;
+
+				warnx("init done.");
+			}
+
+			if (_initialized) {
+
+				/* predict states and covariances */
+
+				/* run the strapdown INS every sensor update */
+				UpdateStrapdownEquationsNED();
+
+				/* store the predictions */
+				StoreStates(IMUmsec);
+
+				/* evaluate if on ground */
+				OnGroundCheck();
+
+				/* prepare the delta angles and time used by the covariance prediction */
+				summedDelAng = summedDelAng + correctedDelAng;
+				summedDelVel = summedDelVel + correctedDelVel;
+				dt += dtIMU;
+
+				/* predict the covairance if the total delta angle has exceeded the threshold
+				 * or the time limit will be exceeded on the next measurement update
+				 */
+				if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
+					CovariancePrediction();
+					summedDelAng = summedDelAng.zero();
+					summedDelVel = summedDelVel.zero();
+					dt = 0.0f;
 				}
+			}
+
+
+			if (gps_updated && _initialized) {
+
+				/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
+				calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
+
+				posNE[0] = posNED[0];
+				posNE[1] = posNED[1];
+
+				// set flags for further processing 
+				fuseVelData = true;
+				fusePosData = true;
+
+				/* recall states after adjusting for delays */
+				RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+				RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+
+				/* run the actual fusion */
+				FuseVelposNED();
+			} else {
+				fuseVelData = false;
+				fusePosData = false;
+			}
+
+			if (baro_updated && _initialized) {
+
+				fuseHgtData = true;
+				// recall states stored at time of measurement after adjusting for delays
+				RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+				// run the fusion step
+				FuseVelposNED();
+			} else {
+				fuseHgtData = false;
+			}
+
+			if (mag_updated && _initialized) {
+				fuseMagData = true;
+				RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
 
 			} else {
 				fuseMagData = false;
 			}
 
-			bool airspeed_updated;
-			orb_check(_airspeed_sub, &airspeed_updated);
-			if (airspeed_updated && _initialized) {
-				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-				perf_count(_perf_airspeed);
+			if (_initialized) {
+				FuseMagnetometer();
+			}
 
-				if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
-
-					VtasMeas = _airspeed.true_airspeed_m_s;
-
-					if (_initialized) {
-
-						fuseVtasData = true;
-						RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
-						FuseAirspeed();
-					}
-				} else {
-					fuseVtasData = false;
-				}
+			if (airspeed_updated && _initialized
+				&& _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
 
+				fuseVtasData = true;
+				RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+				FuseAirspeed();
 			} else {
 				fuseVtasData = false;
 			}
 
+			// Publish results
 			if (_initialized) {
 
 				// State vector:
@@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
 			// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
 			// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
 
+			printf("dt: %8.6f\n", dtIMU);
 			printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
 			printf("states (vel m/s)     [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
 			printf("states (pos m)      [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);