Updated filter to most recent version with accel scale estimation, exposed crucial parameters for cross-vehicle support

src/modules/ekf_att_pos_estimator/estimator.h

@@ -45,14 +45,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1);
 
 void swap_var(float &d1, float &d2);
 
-const unsigned int n_states = 21;
+const unsigned int n_states = 23;
 const unsigned int data_buffer_size = 50;
 
-const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
-const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
-
-// extern bool staticMode;
-
 enum GPS_FIX {
     GPS_FIX_NOFIX = 0,
     GPS_FIX_2D = 2,
@@ -82,6 +77,79 @@ public:
     AttPosEKF();
     ~AttPosEKF();
 
+
+
+    /* ##############################################
+     *
+     *   M A I N    F I L T E R    P A R A M E T E R S
+     *
+     * ########################################### */
+
+    /*
+     * parameters are defined here and initialised in
+     * the InitialiseParameters() (which is just 20 lines down)
+     */
+
+    float covTimeStepMax; // maximum time allowed between covariance predictions
+    float covDelAngMax; // maximum delta angle between covariance predictions
+    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+    float a1; // optical flow sensor misalgnment angle about X axis (rad)
+    float a2; // optical flow sensor misalgnment angle about Y axis (rad)
+    float a3; // optical flow sensor misalgnment angle about Z axis (rad)
+
+    float yawVarScale;
+    float windVelSigma;
+    float dAngBiasSigma;
+    float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float gndHgtSigma;
+
+    float vneSigma;
+    float vdSigma;
+    float posNeSigma;
+    float posDSigma;
+    float magMeasurementSigma;
+    float airspeedMeasurementSigma;
+
+    float gyroProcessNoise;
+    float accelProcessNoise;
+
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    void InitialiseParameters()
+    {
+        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+        a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
+        a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
+        a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
+
+        EAS2TAS = 1.0f;
+
+        yawVarScale = 1.0f;
+        windVelSigma = 0.1f;
+        dAngBiasSigma = 5.0e-7f;
+        dVelBiasSigma = 1e-4f;
+        magEarthSigma = 3.0e-4f;
+        magBodySigma  = 3.0e-4f;
+        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
+
+        vneSigma = 0.2f;
+        vdSigma = 0.3f;
+        posNeSigma = 2.0f;
+        posDSigma = 2.0f;
+
+        magMeasurementSigma = 0.05;
+        airspeedMeasurementSigma = 1.4f;
+        gyroProcessNoise = 1.4544411e-2f;
+        accelProcessNoise = 0.5f;
+    }
+
+
+
+
     // 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
@@ -96,6 +164,8 @@ public:
     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 statesAtRngTime[n_states]; // filter states at the effective measurement time
+    float statesAtLosMeasTime[n_states]; // filter states at the effective measurement time
 
     Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
     Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -104,6 +174,10 @@ public:
     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)
+
+    Mat3f Tbn; // transformation matrix from body to NED coordinates
+    Mat3f Tnb; // transformation amtrix from NED to body coordinates
+
     Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
     Vector3f dVelIMU;
     Vector3f dAngIMU;
@@ -115,26 +189,30 @@ public:
     float velNED[3]; // North, East, Down velocity obs (m/s)
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
+    float rngMea; // Ground distance
     float posNED[3]; // North, East Down position (m)
 
-    float innovMag[3]; // innovation output
+    float innovMag[3]; // innovation output for magnetometer measurements
-    float varInnovMag[3]; // innovation variance output
+    float varInnovMag[3]; // innovation variance output for magnetometer measurements
+    float varInnovLOS[2]; // innovation variance output for optical flow measurements
     Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-    float innovVtas; // innovation output
+    float innovVtas; // innovation output for true airspeed measurements
+    float innovRng; ///< Range finder innovation for rnge finder measurements
+    float innovLOS[2]; // Innovations for optical flow LOS rate measurements
+    float losData[2]; // Optical flow LOS rate measurements
     float varInnovVtas; // innovation variance output
     float VtasMeas; // true airspeed measurement (m/s)
-    float latRef; // WGS-84 latitude of reference point (rad)
+    double latRef; // WGS-84 latitude of reference point (rad)
-    float lonRef; // WGS-84 longitude of reference point (rad)
+    double lonRef; // WGS-84 longitude of reference point (rad)
     float hgtRef; // WGS-84 height of reference point (m)
     Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
-    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+    unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
-    float EAS2TAS; // ratio f true to equivalent airspeed
 
     // GPS input data variables
     float gpsCourse;
     float gpsVelD;
-    float gpsLat;
+    double gpsLat;
-    float gpsLon;
+    double gpsLon;
     float gpsHgt;
     uint8_t GPSstatus;
 
@@ -148,11 +226,15 @@ public:
     bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
     bool fuseMagData; // boolean true when magnetometer data is to be fused
     bool fuseVtasData; // boolean true when airspeed data is to be fused
+    bool fuseRngData;   ///< true when range data is fused
+    bool fuseOptData;   // true when optical flow data is fused
 
     bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
     bool useAirspeed;    ///< boolean true if airspeed data is being used
     bool useCompass;    ///< boolean true if magnetometer data is being used
+    bool useRangeFinder;     ///< true when rangefinder is being used
+    bool useOpticalFlow; // true when optical flow data is being used
 
     struct ekf_status_report current_ekf_state;
     struct ekf_status_report last_ekf_error;
@@ -172,6 +254,10 @@ void FuseMagnetometer();
 
 void FuseAirspeed();
 
+void FuseRangeFinder();
+
+void FuseOpticalFlow();
+
 void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
 
 void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
@@ -192,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms);
  *         time-wise where valid states were updated and invalid remained at the old
  *         value.
  */
-int RecallStates(float statesForFusion[n_states], uint64_t msec);
+int RecallStates(float *statesForFusion, uint64_t msec);
 
 void ResetStoredStates();
 
@@ -206,7 +292,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
 static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
 
-static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
 static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
 

src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp

@@ -210,6 +210,7 @@ private:
 		float abias_pnoise;
 		float mage_pnoise;
 		float magb_pnoise;
+		float eas_noise;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -229,6 +230,7 @@ private:
 		param_t abias_pnoise;
 		param_t mage_pnoise;
 		param_t magb_pnoise;
+		param_t eas_noise;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 	AttPosEKF					*_ekf;
@@ -335,6 +337,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
 	_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
 	_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
+	_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -410,6 +413,25 @@ FixedwingEstimator::parameters_update()
 	param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
 	param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
 	param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
+	param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
+
+	if (_ekf) {
+		// _ekf->yawVarScale = 1.0f;
+		// _ekf->windVelSigma = 0.1f;
+		_ekf->dAngBiasSigma = _parameters.gbias_pnoise;
+		_ekf->dVelBiasSigma = _parameters.abias_pnoise;
+		_ekf->magEarthSigma = _parameters.mage_pnoise;
+		_ekf->magBodySigma  = _parameters.magb_pnoise;
+		// _ekf->gndHgtSigma  = 0.02f;
+		_ekf->vneSigma = _parameters.velne_noise;
+		_ekf->vdSigma = _parameters.veld_noise;
+		_ekf->posNeSigma = _parameters.posne_noise;
+		_ekf->posDSigma = _parameters.posd_noise;
+		_ekf->magMeasurementSigma = _parameters.mag_noise;
+		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
+        	_ekf->accelProcessNoise = _parameters.acc_pnoise;
+		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
+	}
 
 	return OK;
 }
@@ -473,6 +495,7 @@ FixedwingEstimator::task_main()
 	orb_set_interval(_sensor_combined_sub, 4);
 #endif
 
+	/* sets also parameters in the EKF object */
 	parameters_update();
 
 	/* set initial filter state */
@@ -914,7 +937,7 @@ FixedwingEstimator::task_main()
 
 				// perform a covariance prediction if the total delta angle has exceeded the limit
 				// or the time limit will be exceeded at the next IMU update
-				if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
+				if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
 					_ekf->CovariancePrediction(dt);
 					_ekf->summedDelAng = _ekf->summedDelAng.zero();
 					_ekf->summedDelVel = _ekf->summedDelVel.zero();

src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c

@@ -116,7 +116,19 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
 PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
 
 /**
- * Velocity noise in north-east (horizontal) direction.
+ * Airspeed measurement noise.
+ *
+ * Increasing this value will make the filter trust this sensor
+ * less and trust other sensors more.
+ *
+ * @min 0.5
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
+
+/**
+ * Velocity measurement noise in north-east (horizontal) direction.
  *
  * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
  *