AP_NavEKF3: Add multicopter wind estimation

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -39,6 +39,7 @@
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
 #define FLOW_USE_DEFAULT        1
+#define WIND_P_NSE_DEFAULT      0.2
 
 #elif APM_BUILD_TYPE(APM_BUILD_Rover)
 // rover defaults
@@ -64,6 +65,7 @@
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
 #define FLOW_USE_DEFAULT        1
+#define WIND_P_NSE_DEFAULT      0.1
 
 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
 // plane defaults
@@ -89,6 +91,7 @@
 #define FLOW_I_GATE_DEFAULT     500
 #define CHECK_SCALER_DEFAULT    150
 #define FLOW_USE_DEFAULT        2
+#define WIND_P_NSE_DEFAULT      0.1
 
 #else
 // build type not specified, use copter defaults
@@ -98,7 +101,7 @@
 #define ALT_M_NSE_DEFAULT       2.0f
 #define MAG_M_NSE_DEFAULT       0.05f
 #define GYRO_P_NSE_DEFAULT      1.5E-02f
-#define ACC_P_NSE_DEFAULT       3.5E-01f
+#define ACC_P_NSE_DEFAULT       3.5E-01ff
 #define GBIAS_P_NSE_DEFAULT     1.0E-03f
 #define ABIAS_P_NSE_DEFAULT     3.0E-03f
 #define MAGB_P_NSE_DEFAULT      1.0E-04f
@@ -114,6 +117,7 @@
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
 #define FLOW_USE_DEFAULT        1
+#define WIND_P_NSE_DEFAULT      0.1
 
 #endif // APM_BUILD_DIRECTORY
 
@@ -184,9 +188,30 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Units: m
     AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
 
-    // 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
-    // The EKF now takes its GPS delay form the GPS library with the default delays
-    // specified by the GPS_DELAY and GPS_DELAY2 parameters.
+    // bluff body drag fusion
+
+    // @Param: BCOEF_X
+    // @DisplayName: Ballistic coefficient measured in X direction
+    // @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared.  Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
+    // @Units: kg/m^2
+    // @Range: 0.0 100.0
+    // @Increment: 1.0
+    // @User: Advanced
+
+    // @Param: BCOEF_Y
+    // @DisplayName: Ballistic coefficient measured in Y direction
+    // @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared.  Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
+    // @Units: kg/m^2
+    // @Range: 50.0 1000.0
+    // @Increment: 10.0
+    // @User: Advanced
+    // @Param: BCOEF_Y
+
+    // @Param: BCOEF_Z
+    // @DisplayName: unused
+    // @Description: unused
+    // @User: Advanced
+    AP_GROUPINFO("BCOEF", 8, NavEKF3, _ballisticCoef, 0.0f),
 
     // Height measurement parameters
 
@@ -352,7 +377,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Units: rad/s/s
     AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
 
-    // 27 previously used for EK2_GSCL_P_NSE parameter that has been removed
+    // @Param: DRAG_M_NSE
+    // @DisplayName: Observation noise for drag acceleration
+    // @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_BCOEF_X and EK3_BCOEF_Y parameters
+    // @Range: 0.1 2.0
+    // @Increment: 0.1
+    // @User: Advanced
+    // @Units: m/s/s
+    AP_GROUPINFO("DRAG_M_NSE", 27, NavEKF3, _dragObsNoise, 0.5f),
 
     // @Param: ABIAS_P_NSE
     // @DisplayName: Accelerometer bias stability (m/s^3)
@@ -362,7 +394,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Units: m/s/s/s
     AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
 
-    // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
+    // @Param: MCOEF
+    // @DisplayName: Momentum drag coefficient
+    // @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. It represents the ratio of rotor drag induced acceleration to airspeed. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on an will change with different propellers. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEFto a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_BCOEF_X and EK3_BCOEF_Y parameters.
+    // @Range: 0.0 1.0
+    // @Increment: 0.01
+    // @User: Advanced
+    // @Units: 1/s
+    AP_GROUPINFO("MCOEF", 29, NavEKF3, _momentumDragCoef, 0.0f),
 
     // @Param: WIND_P_NSE
     // @DisplayName: Wind velocity process noise (m/s^2)
@@ -371,7 +410,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Increment: 0.1
     // @User: Advanced
     // @Units: m/s/s
-    AP_GROUPINFO("WIND_P_NSE", 30, NavEKF3, _windVelProcessNoise, 0.1f),
+    AP_GROUPINFO("WIND_P_NSE", 30, NavEKF3, _windVelProcessNoise, WIND_P_NSE_DEFAULT),
 
     // @Param: WIND_PSCALE
     // @DisplayName: Height rate to wind process noise scaler
@@ -1108,6 +1147,18 @@ void NavEKF3::getVelNED(int8_t instance, Vector3f &vel) const
     }
 }
 
+// return estimate of true airspeed vector in body frame in m/s for the specified instance
+// An out of range instance (eg -1) returns data for the primary instance
+// returns false if estimate is unavailable
+bool NavEKF3::getAirSpdVec(int8_t instance, Vector3f &vel) const
+{
+    if (instance < 0 || instance >= num_cores) instance = primary;
+    if (core) {
+        return core[instance].getAirSpdVec(vel);
+    }
+    return false;
+}
+
 // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
 float NavEKF3::getPosDownDerivative(int8_t instance) const
 {
@@ -1363,6 +1414,15 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI
     }
 }
 
+// publish output observer angular, velocity and position tracking error
+void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const
+{
+    if (instance < 0 || instance >= num_cores) instance = primary;
+    if (core) {
+        core[instance].getOutputTrackingError(error);
+    }
+}
+
 // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
 void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
 {

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -82,6 +82,11 @@ public:
     // An out of range instance (eg -1) returns data for the primary instance
     void getVelNED(int8_t instance, Vector3f &vel) const;
 
+    // return estimate of true airspeed vector in body frame in m/s for the specified instance
+    // An out of range instance (eg -1) returns data for the primary instance
+    // returns false if estimate is unavailable
+    bool getAirSpdVec(int8_t instance, Vector3f &vel) const;
+
     // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance
     // An out of range instance (eg -1) returns data for the primary instance
     // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
@@ -176,6 +181,9 @@ public:
     // An out of range instance (eg -1) returns data for the primary instance
     void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
 
+    // publish output observer angular, velocity and position tracking error
+    void getOutputTrackingError(int8_t instance, Vector3f &error) const;
+
     // return the innovation consistency test ratios for the specified instance
     // An out of range instance (eg -1) returns data for the primary instance
     void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
@@ -453,6 +461,9 @@ private:
     AP_Int8 _gsfResetMaxCount;      // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate
     AP_Float _err_thresh;           // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError
     AP_Int32 _affinity;             // bitmask of sensor affinity options
+    AP_Float _dragObsNoise;         // drag specific force observatoin noise (m/s/s)**2
+    AP_Vector3f _ballisticCoef;     // ballistic coefficient measured for flow in X and Y body frame directions (Z is unused)
+    AP_Float _momentumDragCoef;     // lift rotor momentum drag coefficient
 
 // Possible values for _flowUse
 #define FLOW_USE_NONE    0

libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@@ -211,10 +211,11 @@ void NavEKF3_core::SelectTasFusion()
 }
 
 
-// select fusion of synthetic sideslip measurements
+// select fusion of synthetic sideslip measurements or body frame drag
 // synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
-// it requires a stable wind for best results and should not be used for aerobatic flight with manoeuvres that induce large sidslip angles (eg knife-edge, spins, etc)
-void NavEKF3_core::SelectBetaFusion()
+// body frame drag only works for bluff body multi rotor vehices with thrust forces aligned with the Z axis
+// it requires a stable wind for best results and should not be used for aerobatic flight
+void NavEKF3_core::SelectBetaDragFusion()
 {
     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
     // If so, don't fuse measurements on this time step to reduce frame over-runs
@@ -227,15 +228,20 @@ void NavEKF3_core::SelectBetaFusion()
     }
 
     // set true when the fusion time interval has triggered
-    bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend->betaAvg_ms);
+    bool f_timeTrigger = ((imuSampleTime_ms - prevBetaDragStep_ms) >= frontend->betaAvg_ms);
     // set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
-    bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->posRetryTimeNoVel_ms));
+    bool f_beta_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->posRetryTimeNoVel_ms));
     // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
-    bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
+    bool f_beta_feasible = (assume_zero_sideslip() && !inhibitWindStates);
     // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
-    if (f_feasible && f_required && f_timeTrigger) {
+    if (f_beta_feasible && f_beta_required && f_timeTrigger) {
         FuseSideslip();
-        prevBetaStep_ms = imuSampleTime_ms;
+        prevBetaDragStep_ms = imuSampleTime_ms;
+    }
+
+    // fusion of XY body frame aero specific forces is done at a slower rate and only if alternative methods of wind estimation are not available
+    if (!inhibitWindStates && storedDrag.recall(dragSampleDelayed,imuDataDelayed.time_ms)) {
+        FuseDragForces();
     }
 }
 
@@ -261,7 +267,6 @@ void NavEKF3_core::FuseSideslip()
     Vector8 SK_BETA;
     Vector3f vel_rel_wind;
     Vector24 H_BETA;
-    float innovBeta;
 
     // copy required states to local variable names
     q0 = stateStruct.quat[0];
@@ -444,6 +449,323 @@ void NavEKF3_core::FuseSideslip()
     ConstrainVariances();
 }
 
+/*
+ * Fuse X and Y body axis specific forces using explicit algebraic equations generated with SymPy.
+ * See AP_NavEKF3/derivation/main.py for derivation
+ * Output for change reference: AP_NavEKF3/derivation/generated/acc_bf_generarted.cpp
+*/
+void NavEKF3_core::FuseDragForces()
+{
+	// drag model parameters
+    const Vector3f bcoef = frontend->_ballisticCoef.get();
+    const float mcoef = frontend->_momentumDragCoef.get();
+    const bool using_bcoef_x = bcoef.x > 1.0f;
+    const bool using_bcoef_y = bcoef.y > 1.0f;
+    const bool using_mcoef = mcoef > 0.001f;
+
+    memset (&Kfusion, 0, sizeof(Kfusion));
+	Vector24 Hfusion; // Observation Jacobians
+	const float R_ACC = sq(fmaxf(frontend->_dragObsNoise, 0.5f));
+	const float density_ratio = sqrtf(dal.get_EAS2TAS());
+	const float rho = fmaxf(1.225f * density_ratio, 0.1f); // air density
+
+	// get latest estimated orientation
+    const float &q0 = stateStruct.quat[0];
+    const float &q1 = stateStruct.quat[1];
+    const float &q2 = stateStruct.quat[2];
+    const float &q3 = stateStruct.quat[3];
+
+	// get latest velocity in earth frame
+	const float &vn = stateStruct.velocity.x;
+	const float &ve = stateStruct.velocity.y;
+	const float &vd = stateStruct.velocity.z;
+
+	// get latest wind velocity in earth frame
+	const float &vwn = stateStruct.wind_vel.x;
+	const float &vwe = stateStruct.wind_vel.y;
+
+	// predicted specific forces
+	// calculate relative wind velocity in earth frame and rotate into body frame
+	const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
+	const Vector3f rel_wind_body = prevTnb * rel_wind_earth;
+
+	// perform sequential fusion of XY specific forces
+	for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
+        // correct accel data for bias
+        const float mea_acc = dragSampleDelayed.accelXY[axis_index]  - stateStruct.accel_bias[axis_index] / dtEkfAvg;
+
+        // Acceleration in m/s/s predicfed using vehicle and wind velocity estimates
+        // Initialised to measured value and updated later using available drag model
+        float predAccel = mea_acc;
+
+		// predicted sign of drag force
+		const float dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f;
+
+        float airSpd; // Airspeed estimated using drag model and used for linearisation point
+        float Kacc; // Derivative of specific force wrt airspeed
+		if (axis_index == 0) {
+            if (!using_bcoef_x && !using_mcoef) {
+                // skip this axis
+                continue;
+            }
+
+            // drag can be modelled as an arbitrary  combination of bluff body drag that proportional to
+            // speed squared, and rotor momentum drag that is proportional to speed.
+            if (using_mcoef && using_bcoef_x) {
+                // mixed bluff body and propeller momentum drag
+                airSpd = (bcoef.x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.x) * fabsf(mea_acc)));
+                Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd + mcoef * density_ratio);
+                predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio;
+            } else if (using_mcoef) {
+                // propeller momentum drag only
+                airSpd = fabsf(mea_acc) / mcoef;
+                Kacc = fmaxf(1e-1f, mcoef * density_ratio);
+                predAccel = - rel_wind_body[0] * mcoef * density_ratio;
+            } else if (using_bcoef_x) {
+                // bluff body drag only
+                airSpd = sqrtf((2.0f * bcoef.x * fabsf(mea_acc)) / rho);
+                Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd);
+                predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign;
+            }
+
+			// intermediate variables
+			const float HK0 = vn - vwn;
+			const float HK1 = ve - vwe;
+			const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
+			const float HK3 = 2*Kacc;
+			const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
+			const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
+			const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
+			const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
+			const float HK8 = HK7*Kacc;
+			const float HK9 = q0*q3 + q1*q2;
+			const float HK10 = HK3*HK9;
+			const float HK11 = q0*q2 - q1*q3;
+			const float HK12 = 2*HK9;
+			const float HK13 = 2*HK11;
+			const float HK14 = 2*HK4;
+			const float HK15 = 2*HK2;
+			const float HK16 = 2*HK5;
+			const float HK17 = 2*HK6;
+			const float HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4];
+			const float HK19 = HK12*P[5][23];
+			const float HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23];
+			const float HK21 = powf(Kacc, 2);
+			const float HK22 = HK12*HK21;
+			const float HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22];
+			const float HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22];
+			const float HK25 = HK7*P[4][22];
+			const float HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4];
+			const float HK27 = HK21*HK7;
+			const float HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22];
+			const float HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4];
+			const float HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4];
+			const float HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4];
+			// const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
+
+			// calculate innovation variance and exit if badly conditioned
+			innovDragVar.x = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
+			if (innovDragVar.x < R_ACC) {
+				return;
+			}
+			const float HK32 = Kacc / innovDragVar.x;
+
+			// Observation Jacobians
+			Hfusion[0] = -HK2*HK3;
+			Hfusion[1] = -HK3*HK4;
+			Hfusion[2] = HK3*HK5;
+			Hfusion[3] = -HK3*HK6;
+			Hfusion[4] = -HK8;
+			Hfusion[5] = -HK10;
+			Hfusion[6] = HK11*HK3;
+			Hfusion[22] = HK8;
+			Hfusion[23] = HK10;
+
+			// Kalman gains
+			// Don't allow modification of any states other than wind velocity - we only need a wind estimate.
+			// Kfusion[0] = -HK18*HK32;
+			// Kfusion[1] = -HK29*HK32;
+			// Kfusion[2] = -HK30*HK32;
+			// Kfusion[3] = -HK31*HK32;
+			// Kfusion[4] = -HK26*HK32;
+			// Kfusion[5] = -HK23*HK32;
+			// Kfusion[6] = -HK24*HK32;
+			// Kfusion[7] = -HK32*(HK12*P[5][7] - HK12*P[7][23] - HK13*P[6][7] + HK14*P[1][7] + HK15*P[0][7] - HK16*P[2][7] + HK17*P[3][7] + HK7*P[4][7] - HK7*P[7][22]);
+			// Kfusion[8] = -HK32*(HK12*P[5][8] - HK12*P[8][23] - HK13*P[6][8] + HK14*P[1][8] + HK15*P[0][8] - HK16*P[2][8] + HK17*P[3][8] + HK7*P[4][8] - HK7*P[8][22]);
+			// Kfusion[9] = -HK32*(HK12*P[5][9] - HK12*P[9][23] - HK13*P[6][9] + HK14*P[1][9] + HK15*P[0][9] - HK16*P[2][9] + HK17*P[3][9] + HK7*P[4][9] - HK7*P[9][22]);
+			// Kfusion[10] = -HK32*(-HK12*P[10][23] + HK12*P[5][10] - HK13*P[6][10] + HK14*P[1][10] + HK15*P[0][10] - HK16*P[2][10] + HK17*P[3][10] - HK7*P[10][22] + HK7*P[4][10]);
+			// Kfusion[11] = -HK32*(-HK12*P[11][23] + HK12*P[5][11] - HK13*P[6][11] + HK14*P[1][11] + HK15*P[0][11] - HK16*P[2][11] + HK17*P[3][11] - HK7*P[11][22] + HK7*P[4][11]);
+			// Kfusion[12] = -HK32*(-HK12*P[12][23] + HK12*P[5][12] - HK13*P[6][12] + HK14*P[1][12] + HK15*P[0][12] - HK16*P[2][12] + HK17*P[3][12] - HK7*P[12][22] + HK7*P[4][12]);
+			// Kfusion[13] = -HK32*(-HK12*P[13][23] + HK12*P[5][13] - HK13*P[6][13] + HK14*P[1][13] + HK15*P[0][13] - HK16*P[2][13] + HK17*P[3][13] - HK7*P[13][22] + HK7*P[4][13]);
+			// Kfusion[14] = -HK32*(-HK12*P[14][23] + HK12*P[5][14] - HK13*P[6][14] + HK14*P[1][14] + HK15*P[0][14] - HK16*P[2][14] + HK17*P[3][14] - HK7*P[14][22] + HK7*P[4][14]);
+			// Kfusion[15] = -HK32*(-HK12*P[15][23] + HK12*P[5][15] - HK13*P[6][15] + HK14*P[1][15] + HK15*P[0][15] - HK16*P[2][15] + HK17*P[3][15] - HK7*P[15][22] + HK7*P[4][15]);
+			// Kfusion[16] = -HK32*(-HK12*P[16][23] + HK12*P[5][16] - HK13*P[6][16] + HK14*P[1][16] + HK15*P[0][16] - HK16*P[2][16] + HK17*P[3][16] - HK7*P[16][22] + HK7*P[4][16]);
+			// Kfusion[17] = -HK32*(-HK12*P[17][23] + HK12*P[5][17] - HK13*P[6][17] + HK14*P[1][17] + HK15*P[0][17] - HK16*P[2][17] + HK17*P[3][17] - HK7*P[17][22] + HK7*P[4][17]);
+			// Kfusion[18] = -HK32*(-HK12*P[18][23] + HK12*P[5][18] - HK13*P[6][18] + HK14*P[1][18] + HK15*P[0][18] - HK16*P[2][18] + HK17*P[3][18] - HK7*P[18][22] + HK7*P[4][18]);
+			// Kfusion[19] = -HK32*(-HK12*P[19][23] + HK12*P[5][19] - HK13*P[6][19] + HK14*P[1][19] + HK15*P[0][19] - HK16*P[2][19] + HK17*P[3][19] - HK7*P[19][22] + HK7*P[4][19]);
+			// Kfusion[20] = -HK32*(-HK12*P[20][23] + HK12*P[5][20] - HK13*P[6][20] + HK14*P[1][20] + HK15*P[0][20] - HK16*P[2][20] + HK17*P[3][20] - HK7*P[20][22] + HK7*P[4][20]);
+			// Kfusion[21] = -HK32*(-HK12*P[21][23] + HK12*P[5][21] - HK13*P[6][21] + HK14*P[1][21] + HK15*P[0][21] - HK16*P[2][21] + HK17*P[3][21] - HK7*P[21][22] + HK7*P[4][21]);
+			Kfusion[22] = -HK28*HK32;
+			Kfusion[23] = -HK20*HK32;
+
+
+		} else if (axis_index == 1) {
+            if (!using_bcoef_y && !using_mcoef) {
+                // skip this axis
+                continue;
+            }
+
+            // drag can be modelled as an arbitrary  combination of bluff body drag that proportional to
+            // speed squared, and rotor momentum drag that is proportional to speed.
+            if (using_mcoef && using_bcoef_y) {
+                // mixed bluff body and propeller momentum drag
+                airSpd = (bcoef.y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.y) * fabsf(mea_acc)));
+                Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd + mcoef * density_ratio);
+                predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio;
+            } else if (using_mcoef) {
+                // propeller momentum drag only
+                airSpd = fabsf(mea_acc) / mcoef;
+                Kacc = fmaxf(1e-1f, mcoef * density_ratio);
+                predAccel = - rel_wind_body[1] * mcoef * density_ratio;
+            } else if (using_bcoef_y) {
+                // bluff body drag only
+                airSpd = sqrtf((2.0f * bcoef.y * fabsf(mea_acc)) / rho);
+                Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd);
+		        predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign;
+            }
+
+			// intermediate variables
+			const float HK0 = ve - vwe;
+			const float HK1 = vn - vwn;
+			const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
+			const float HK3 = 2*Kacc;
+			const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
+			const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
+			const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
+			const float HK7 = q0*q3 - q1*q2;
+			const float HK8 = HK3*HK7;
+			const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
+			const float HK10 = HK9*Kacc;
+			const float HK11 = q0*q1 + q2*q3;
+			const float HK12 = 2*HK11;
+			const float HK13 = 2*HK7;
+			const float HK14 = 2*HK5;
+			const float HK15 = 2*HK2;
+			const float HK16 = 2*HK4;
+			const float HK17 = 2*HK6;
+			const float HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5];
+			const float HK19 = powf(Kacc, 2);
+			const float HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23];
+			const float HK21 = HK13*P[4][22];
+			const float HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22];
+			const float HK23 = HK13*HK19;
+			const float HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5];
+			const float HK25 = HK9*P[5][23];
+			const float HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5];
+			const float HK27 = HK19*HK9;
+			const float HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23];
+			const float HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5];
+			const float HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5];
+			const float HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5];
+			// const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
+
+			innovDragVar.y = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
+			if (innovDragVar.y < R_ACC) {
+				// calculation is badly conditioned
+				return;
+			}
+			const float HK32 = Kacc / innovDragVar.y;
+
+			// Observation Jacobians
+			Hfusion[0] = -HK2*HK3;
+			Hfusion[1] = -HK3*HK4;
+			Hfusion[2] = -HK3*HK5;
+			Hfusion[3] = HK3*HK6;
+			Hfusion[4] = HK8;
+			Hfusion[5] = -HK10;
+			Hfusion[6] = -HK11*HK3;
+			Hfusion[22] = -HK8;
+			Hfusion[23] = HK10;
+
+			// Kalman gains
+			// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
+			// Kfusion[0] = -HK18*HK32;
+			// Kfusion[1] = -HK30*HK32;
+			// Kfusion[2] = -HK29*HK32;
+			// Kfusion[3] = -HK31*HK32;
+			// Kfusion[4] = -HK24*HK32;
+			// Kfusion[5] = -HK26*HK32;
+			// Kfusion[6] = -HK20*HK32;
+			// Kfusion[7] = -HK32*(HK12*P[6][7] - HK13*P[4][7] + HK13*P[7][22] + HK14*P[2][7] + HK15*P[0][7] + HK16*P[1][7] - HK17*P[3][7] + HK9*P[5][7] - HK9*P[7][23]);
+			// Kfusion[8] = -HK32*(HK12*P[6][8] - HK13*P[4][8] + HK13*P[8][22] + HK14*P[2][8] + HK15*P[0][8] + HK16*P[1][8] - HK17*P[3][8] + HK9*P[5][8] - HK9*P[8][23]);
+			// Kfusion[9] = -HK32*(HK12*P[6][9] - HK13*P[4][9] + HK13*P[9][22] + HK14*P[2][9] + HK15*P[0][9] + HK16*P[1][9] - HK17*P[3][9] + HK9*P[5][9] - HK9*P[9][23]);
+			// Kfusion[10] = -HK32*(HK12*P[6][10] + HK13*P[10][22] - HK13*P[4][10] + HK14*P[2][10] + HK15*P[0][10] + HK16*P[1][10] - HK17*P[3][10] - HK9*P[10][23] + HK9*P[5][10]);
+			// Kfusion[11] = -HK32*(HK12*P[6][11] + HK13*P[11][22] - HK13*P[4][11] + HK14*P[2][11] + HK15*P[0][11] + HK16*P[1][11] - HK17*P[3][11] - HK9*P[11][23] + HK9*P[5][11]);
+			// Kfusion[12] = -HK32*(HK12*P[6][12] + HK13*P[12][22] - HK13*P[4][12] + HK14*P[2][12] + HK15*P[0][12] + HK16*P[1][12] - HK17*P[3][12] - HK9*P[12][23] + HK9*P[5][12]);
+			// Kfusion[13] = -HK32*(HK12*P[6][13] + HK13*P[13][22] - HK13*P[4][13] + HK14*P[2][13] + HK15*P[0][13] + HK16*P[1][13] - HK17*P[3][13] - HK9*P[13][23] + HK9*P[5][13]);
+			// Kfusion[14] = -HK32*(HK12*P[6][14] + HK13*P[14][22] - HK13*P[4][14] + HK14*P[2][14] + HK15*P[0][14] + HK16*P[1][14] - HK17*P[3][14] - HK9*P[14][23] + HK9*P[5][14]);
+			// Kfusion[15] = -HK32*(HK12*P[6][15] + HK13*P[15][22] - HK13*P[4][15] + HK14*P[2][15] + HK15*P[0][15] + HK16*P[1][15] - HK17*P[3][15] - HK9*P[15][23] + HK9*P[5][15]);
+			// Kfusion[16] = -HK32*(HK12*P[6][16] + HK13*P[16][22] - HK13*P[4][16] + HK14*P[2][16] + HK15*P[0][16] + HK16*P[1][16] - HK17*P[3][16] - HK9*P[16][23] + HK9*P[5][16]);
+			// Kfusion[17] = -HK32*(HK12*P[6][17] + HK13*P[17][22] - HK13*P[4][17] + HK14*P[2][17] + HK15*P[0][17] + HK16*P[1][17] - HK17*P[3][17] - HK9*P[17][23] + HK9*P[5][17]);
+			// Kfusion[18] = -HK32*(HK12*P[6][18] + HK13*P[18][22] - HK13*P[4][18] + HK14*P[2][18] + HK15*P[0][18] + HK16*P[1][18] - HK17*P[3][18] - HK9*P[18][23] + HK9*P[5][18]);
+			// Kfusion[19] = -HK32*(HK12*P[6][19] + HK13*P[19][22] - HK13*P[4][19] + HK14*P[2][19] + HK15*P[0][19] + HK16*P[1][19] - HK17*P[3][19] - HK9*P[19][23] + HK9*P[5][19]);
+			// Kfusion[20] = -HK32*(HK12*P[6][20] + HK13*P[20][22] - HK13*P[4][20] + HK14*P[2][20] + HK15*P[0][20] + HK16*P[1][20] - HK17*P[3][20] - HK9*P[20][23] + HK9*P[5][20]);
+			// Kfusion[21] = -HK32*(HK12*P[6][21] + HK13*P[21][22] - HK13*P[4][21] + HK14*P[2][21] + HK15*P[0][21] + HK16*P[1][21] - HK17*P[3][21] - HK9*P[21][23] + HK9*P[5][21]);
+			Kfusion[22] = -HK22*HK32;
+			Kfusion[23] = -HK28*HK32;
+		}
+
+		innovDrag[axis_index] = predAccel - mea_acc;
+		dragTestRatio[axis_index] = sq(innovDrag[axis_index]) / (25.0f * innovDragVar[axis_index]);
+
+		// if the innovation consistency check fails then don't fuse the sample
+		if (dragTestRatio[axis_index] > 1.0f) {
+			return;
+		}
+
+        // correct the state vector
+        for (uint8_t j= 0; j<=stateIndexLim; j++) {
+            statesArray[j] = statesArray[j] - Kfusion[j] * innovDrag[axis_index];
+        }
+        stateStruct.quat.normalize();
+
+        // correct the covariance P = (I - K*H)*P
+        // take advantage of the empty columns in KH to reduce the
+        // number of operations
+        for (unsigned i = 0; i<=stateIndexLim; i++) {
+            for (unsigned j = 0; j<=6; j++) {
+                KH[i][j] = Kfusion[i] * Hfusion[j];
+            }
+            for (unsigned j = 7; j<=21; j++) {
+                KH[i][j] = 0.0f;
+            }
+            for (unsigned j = 22; j<=23; j++) {
+                KH[i][j] = Kfusion[i] * Hfusion[j];
+            }
+        }
+        for (unsigned j = 0; j<=stateIndexLim; j++) {
+            for (unsigned i = 0; i<=stateIndexLim; i++) {
+                ftype res = 0;
+                res += KH[i][0] * P[0][j];
+                res += KH[i][1] * P[1][j];
+                res += KH[i][2] * P[2][j];
+                res += KH[i][3] * P[3][j];
+                res += KH[i][4] * P[4][j];
+                res += KH[i][5] * P[5][j];
+                res += KH[i][6] * P[6][j];
+                res += KH[i][22] * P[22][j];
+                res += KH[i][23] * P[23][j];
+                KHP[i][j] = res;
+            }
+        }
+        for (unsigned i = 0; i<=stateIndexLim; i++) {
+            for (unsigned j = 0; j<=stateIndexLim; j++) {
+                P[i][j] = P[i][j] - KHP[i][j];
+            }
+        }
+	}
+}
+
 /********************************************************
 *                   MISC FUNCTIONS                      *
 ********************************************************/

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -60,7 +60,7 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const
 void NavEKF3_core::setWindMagStateLearningMode()
 {
     // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
-    bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
+    bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip() && !(dragFusionEnabled && finalInflightYawInit)) || onGround || (PV_AidingMode == AID_NONE);
     if (!inhibitWindStates && setWindInhibit) {
         inhibitWindStates = true;
         updateStateIndexLim();
@@ -77,7 +77,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
             stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
             stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
 
-            // set the wind sate variances to the measurement uncertainty
+            // set the wind state variances to the measurement uncertainty
             for (uint8_t index=22; index<=23; index++) {
                 P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(dal.get_EAS2TAS(), 0.9f, 10.0f));
             }

libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@@ -58,6 +58,9 @@ void NavEKF3_core::Log_Write_XKF2(uint64_t time_us) const
     getWind(wind);
     getMagNED(magNED);
     getMagXYZ(magXYZ);
+    Vector2f dragInnov;
+    float betaInnov = 0;
+    getSynthAirDataInnovations(dragInnov, betaInnov);
     const struct log_XKF2 pkt2{
         LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
         time_us : time_us,
@@ -72,7 +75,10 @@ void NavEKF3_core::Log_Write_XKF2(uint64_t time_us) const
         magD    : (int16_t)(magNED.z),
         magX    : (int16_t)(magXYZ.x),
         magY    : (int16_t)(magXYZ.y),
-        magZ    : (int16_t)(magXYZ.z)
+        magZ    : (int16_t)(magXYZ.z),
+        innovDragX    : dragInnov.x,
+        innovDragY    : dragInnov.y,
+        innovSideslip : betaInnov
     };
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -482,6 +482,9 @@ void NavEKF3_core::readIMUData()
         float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
         dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
 
+        // do an addtional down sampling for data used to sample XY body frame drag specific forces
+        SampleDragData(imuDataDownSampledNew);
+
         // zero the accumulated IMU data and quaternion
         imuDataDownSampledNew.delAng.zero();
         imuDataDownSampledNew.delVel.zero();
@@ -1355,3 +1358,48 @@ void NavEKF3_core::updateMovementCheck(void)
         AP::logger().WriteBlock(&pkt, sizeof(pkt));
     }
 }
+
+void NavEKF3_core::SampleDragData(const imu_elements &imu)
+{
+    // Average and down sample to 5Hz
+    const Vector3f bcoef = frontend->_ballisticCoef.get();
+    const float mcoef = frontend->_momentumDragCoef.get();
+    const bool using_bcoef_x = bcoef.x > 1.0f;
+    const bool using_bcoef_y = bcoef.y > 1.0f;
+    const bool using_mcoef = mcoef > 0.001f;
+	if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
+        // nothing to do
+        dragFusionEnabled = false;
+		return;
+	}
+
+    dragFusionEnabled = true;
+
+	// down-sample the drag specific force data by accumulating and calculating the mean when
+	// sufficient samples have been collected
+
+    dragSampleCount ++;
+
+    // note acceleration is accumulated as a delta velocity
+    dragDownSampled.accelXY.x += imu.delVel.x;
+    dragDownSampled.accelXY.y += imu.delVel.y;
+    dragDownSampled.time_ms += imu.time_ms;
+    dragSampleTimeDelta += imu.delVelDT;
+
+    // calculate and store means from accumulated values
+    if (dragSampleTimeDelta > 0.2f - 0.5f * EKF_TARGET_DT) {
+        // note conversion from accumulated delta velocity to acceleration
+        dragDownSampled.accelXY.x /= dragSampleTimeDelta;
+        dragDownSampled.accelXY.y /= dragSampleTimeDelta;
+        dragDownSampled.time_ms /= dragSampleCount;
+
+        // write to buffer
+        storedDrag.push(dragDownSampled);
+
+        // reset accumulators
+        dragSampleCount = 0;
+        dragDownSampled.accelXY.zero();
+        dragDownSampled.time_ms = 0;
+        dragSampleTimeDelta = 0.0f;
+    }
+}

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -191,6 +191,23 @@ void NavEKF3_core::getVelNED(Vector3f &vel) const
     vel = outputDataNew.velocity + velOffsetNED;
 }
 
+// return estimate of true airspeed vector in body frame in m/s
+// returns false if estimate is unavailable
+bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const
+{
+    if (inhibitWindStates || PV_AidingMode == AID_NONE) {
+        return false;
+    }
+    vel = outputDataNew.velocity + velOffsetNED;
+    vel.x -= stateStruct.wind_vel.x;
+    vel.y -= stateStruct.wind_vel.y;
+    Matrix3f Tnb; // rotation from nav to body frame
+    outputDataNew.quat.inverse().rotation_matrix(Tnb);
+    vel = Tnb * vel;
+    return true;
+}
+
+
 // Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s
 float NavEKF3_core::getPosDownDerivative(void) const
 {
@@ -430,6 +447,14 @@ void  NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto
     yawInnov   = innovYaw;
 }
 
+// return the synthetic air data drag and sideslip innovations
+void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaInnov) const
+{
+    dragInnov.x = innovDrag[0];
+    dragInnov.y = innovDrag[1];
+    betaInnov   = innovBeta;
+}
+
 // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
 // this indicates the amount of margin available when tuning the various error traps
 // also return the delta in position due to the last position reset

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -149,6 +149,10 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     if(!storedOutput.init(imu_buffer_length)) {
         return false;
     }
+    if (!storedDrag.init(obs_buffer_length)) {
+        return false;
+    }
+
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u buffs IMU=%u OBS=%u OF=%u EN:%u dt=%.4f",
                     (unsigned)imu_index,
                     (unsigned)imu_buffer_length,
@@ -191,7 +195,7 @@ void NavEKF3_core::InitialiseVariables()
     // initialise time stamps
     imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
     prevTasStep_ms = imuSampleTime_ms;
-    prevBetaStep_ms = imuSampleTime_ms;
+    prevBetaDragStep_ms = imuSampleTime_ms;
     lastBaroReceived_ms = imuSampleTime_ms;
     lastVelPassTime_ms = 0;
     lastPosPassTime_ms = 0;
@@ -673,8 +677,8 @@ void NavEKF3_core::UpdateFilter(bool predict)
         // Update states using airspeed data
         SelectTasFusion();
 
-        // Update states using sideslip constraint assumption for fly-forward vehicles
-        SelectBetaFusion();
+        // Update states using sideslip constraint assumption for fly-forward vehicles or body drag for multicopters
+        SelectBetaDragFusion();
 
         // Update the filter status
         updateFilterStatus();

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -116,6 +116,10 @@ public:
     // return NED velocity in m/s
     void getVelNED(Vector3f &vel) const;
 
+    // return estimate of true airspeed vector in body frame in m/s
+    // returns false if estimate is unavailable
+    bool getAirSpdVec(Vector3f &vel) const;
+
     // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
     // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
     // but will always be kinematically consistent with the z component of the EKF position state
@@ -196,7 +200,10 @@ public:
     // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
     void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
 
-    // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
+    // return the synthetic air data drag and sideslip innovations
+    void getSynthAirDataInnovations(Vector2f &dragInnov, float &betaInnov) const;
+
+   // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
     void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
 
     // get a particular source's velocity innovations
@@ -581,6 +588,10 @@ private:
         bool corrected;             // true when the velocity has been corrected for sensor position
     };
 
+    struct drag_elements : EKF_obs_element_t {
+        Vector2f accelXY;       // measured specific force along the X and Y body axes (m/sec**2)
+    };
+
     // bias estimates for the IMUs that are enabled but not being used
     // by this core.
     struct {
@@ -719,8 +730,8 @@ private:
     // determine when to perform fusion of true airspeed measurements
     void SelectTasFusion();
 
-    // determine when to perform fusion of synthetic sideslp measurements
-    void SelectBetaFusion();
+    // determine when to perform fusion of drag or synthetic sideslip measurements
+    void SelectBetaDragFusion();
 
     // force alignment of the yaw angle using GPS velocity data
     void realignYawGPS();
@@ -935,6 +946,11 @@ private:
     // returns false if unsuccessful
     bool EKFGSF_resetMainFilterYaw();
 
+	// Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
+    void FuseDragForces();
+    void SelectDragFusion();
+    void SampleDragData(const imu_elements &imu);
+
     // Variables
     bool statesInitialised;         // boolean true when filter states have been initialised
     bool magHealth;                 // boolean true if magnetometer has passed innovation consistency check
@@ -981,7 +997,8 @@ private:
     bool magFusePerformed;          // boolean set to true when magnetometer fusion has been perfomred in that time step
     MagCal effectiveMagCal;         // the actual mag calibration being used as the default
     uint32_t prevTasStep_ms;        // time stamp of last TAS fusion step
-    uint32_t prevBetaStep_ms;       // time stamp of last synthetic sideslip fusion step
+    uint32_t prevBetaDragStep_ms;   // time stamp of last synthetic sideslip fusion step
+    ftype innovBeta;                // synthetic sideslip innovation (rad)
     uint32_t lastMagUpdate_us;      // last time compass was updated in usec
     uint32_t lastMagRead_ms;        // last time compass data was successfully read
     Vector3f velDotNED;             // rate of change of velocity in NED frame
@@ -1249,6 +1266,17 @@ private:
         Vector3f beaconPosNED; // beacon NED position
     } *rngBcnFusionReport;
 
+    // drag fusion for multicopter wind estimation
+    EKF_obs_buffer_t<drag_elements> storedDrag;
+    drag_elements dragSampleDelayed;
+	drag_elements dragDownSampled;	    // down sampled from filter prediction rate to observation rate
+	uint8_t dragSampleCount;	        // number of drag specific force samples accumulated at the filter prediction rate
+	float dragSampleTimeDelta;	        // time integral across all samples used to form _drag_down_sampled (sec)
+    Vector2f innovDrag;		            // multirotor drag measurement innovation (m/sec**2)
+	Vector2f innovDragVar;	            // multirotor drag measurement innovation variance ((m/sec**2)**2)
+	Vector2f dragTestRatio;		        // drag innovation consistency check ratio
+    bool dragFusionEnabled;
+
     // height source selection logic
     AP_NavEKF_Source::SourceZ activeHgtSource;  // active height source
     AP_NavEKF_Source::SourceZ prevHgtSource;    // previous height source used to detect changes in source

libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp

@@ -0,0 +1,182 @@
+// Axis 0 equations
+// Sub Expressions
+const float HK0 = vn - vwn;
+const float HK1 = ve - vwe;
+const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
+const float HK3 = 2*Kaccx;
+const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
+const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
+const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
+const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
+const float HK8 = HK7*Kaccx;
+const float HK9 = q0*q3 + q1*q2;
+const float HK10 = HK3*HK9;
+const float HK11 = q0*q2 - q1*q3;
+const float HK12 = 2*HK9;
+const float HK13 = 2*HK11;
+const float HK14 = 2*HK4;
+const float HK15 = 2*HK2;
+const float HK16 = 2*HK5;
+const float HK17 = 2*HK6;
+const float HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4];
+const float HK19 = HK12*P[5][23];
+const float HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23];
+const float HK21 = powf(Kaccx, 2);
+const float HK22 = HK12*HK21;
+const float HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22];
+const float HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22];
+const float HK25 = HK7*P[4][22];
+const float HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4];
+const float HK27 = HK21*HK7;
+const float HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22];
+const float HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4];
+const float HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4];
+const float HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4];
+const float HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
+
+
+// Observation Jacobians
+Hfusion[0] = -HK2*HK3;
+Hfusion[1] = -HK3*HK4;
+Hfusion[2] = HK3*HK5;
+Hfusion[3] = -HK3*HK6;
+Hfusion[4] = -HK8;
+Hfusion[5] = -HK10;
+Hfusion[6] = HK11*HK3;
+Hfusion[7] = 0;
+Hfusion[8] = 0;
+Hfusion[9] = 0;
+Hfusion[10] = 0;
+Hfusion[11] = 0;
+Hfusion[12] = 0;
+Hfusion[13] = 0;
+Hfusion[14] = 0;
+Hfusion[15] = 0;
+Hfusion[16] = 0;
+Hfusion[17] = 0;
+Hfusion[18] = 0;
+Hfusion[19] = 0;
+Hfusion[20] = 0;
+Hfusion[21] = 0;
+Hfusion[22] = HK8;
+Hfusion[23] = HK10;
+
+
+// Kalman gains
+Kfusion[0] = -HK18*HK32;
+Kfusion[1] = -HK29*HK32;
+Kfusion[2] = -HK30*HK32;
+Kfusion[3] = -HK31*HK32;
+Kfusion[4] = -HK26*HK32;
+Kfusion[5] = -HK23*HK32;
+Kfusion[6] = -HK24*HK32;
+Kfusion[7] = -HK32*(HK12*P[5][7] - HK12*P[7][23] - HK13*P[6][7] + HK14*P[1][7] + HK15*P[0][7] - HK16*P[2][7] + HK17*P[3][7] + HK7*P[4][7] - HK7*P[7][22]);
+Kfusion[8] = -HK32*(HK12*P[5][8] - HK12*P[8][23] - HK13*P[6][8] + HK14*P[1][8] + HK15*P[0][8] - HK16*P[2][8] + HK17*P[3][8] + HK7*P[4][8] - HK7*P[8][22]);
+Kfusion[9] = -HK32*(HK12*P[5][9] - HK12*P[9][23] - HK13*P[6][9] + HK14*P[1][9] + HK15*P[0][9] - HK16*P[2][9] + HK17*P[3][9] + HK7*P[4][9] - HK7*P[9][22]);
+Kfusion[10] = -HK32*(-HK12*P[10][23] + HK12*P[5][10] - HK13*P[6][10] + HK14*P[1][10] + HK15*P[0][10] - HK16*P[2][10] + HK17*P[3][10] - HK7*P[10][22] + HK7*P[4][10]);
+Kfusion[11] = -HK32*(-HK12*P[11][23] + HK12*P[5][11] - HK13*P[6][11] + HK14*P[1][11] + HK15*P[0][11] - HK16*P[2][11] + HK17*P[3][11] - HK7*P[11][22] + HK7*P[4][11]);
+Kfusion[12] = -HK32*(-HK12*P[12][23] + HK12*P[5][12] - HK13*P[6][12] + HK14*P[1][12] + HK15*P[0][12] - HK16*P[2][12] + HK17*P[3][12] - HK7*P[12][22] + HK7*P[4][12]);
+Kfusion[13] = -HK32*(-HK12*P[13][23] + HK12*P[5][13] - HK13*P[6][13] + HK14*P[1][13] + HK15*P[0][13] - HK16*P[2][13] + HK17*P[3][13] - HK7*P[13][22] + HK7*P[4][13]);
+Kfusion[14] = -HK32*(-HK12*P[14][23] + HK12*P[5][14] - HK13*P[6][14] + HK14*P[1][14] + HK15*P[0][14] - HK16*P[2][14] + HK17*P[3][14] - HK7*P[14][22] + HK7*P[4][14]);
+Kfusion[15] = -HK32*(-HK12*P[15][23] + HK12*P[5][15] - HK13*P[6][15] + HK14*P[1][15] + HK15*P[0][15] - HK16*P[2][15] + HK17*P[3][15] - HK7*P[15][22] + HK7*P[4][15]);
+Kfusion[16] = -HK32*(-HK12*P[16][23] + HK12*P[5][16] - HK13*P[6][16] + HK14*P[1][16] + HK15*P[0][16] - HK16*P[2][16] + HK17*P[3][16] - HK7*P[16][22] + HK7*P[4][16]);
+Kfusion[17] = -HK32*(-HK12*P[17][23] + HK12*P[5][17] - HK13*P[6][17] + HK14*P[1][17] + HK15*P[0][17] - HK16*P[2][17] + HK17*P[3][17] - HK7*P[17][22] + HK7*P[4][17]);
+Kfusion[18] = -HK32*(-HK12*P[18][23] + HK12*P[5][18] - HK13*P[6][18] + HK14*P[1][18] + HK15*P[0][18] - HK16*P[2][18] + HK17*P[3][18] - HK7*P[18][22] + HK7*P[4][18]);
+Kfusion[19] = -HK32*(-HK12*P[19][23] + HK12*P[5][19] - HK13*P[6][19] + HK14*P[1][19] + HK15*P[0][19] - HK16*P[2][19] + HK17*P[3][19] - HK7*P[19][22] + HK7*P[4][19]);
+Kfusion[20] = -HK32*(-HK12*P[20][23] + HK12*P[5][20] - HK13*P[6][20] + HK14*P[1][20] + HK15*P[0][20] - HK16*P[2][20] + HK17*P[3][20] - HK7*P[20][22] + HK7*P[4][20]);
+Kfusion[21] = -HK32*(-HK12*P[21][23] + HK12*P[5][21] - HK13*P[6][21] + HK14*P[1][21] + HK15*P[0][21] - HK16*P[2][21] + HK17*P[3][21] - HK7*P[21][22] + HK7*P[4][21]);
+Kfusion[22] = -HK28*HK32;
+Kfusion[23] = -HK20*HK32;
+
+
+// Axis 1 equations
+// Sub Expressions
+const float HK0 = ve - vwe;
+const float HK1 = vn - vwn;
+const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
+const float HK3 = 2*Kaccy;
+const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
+const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
+const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
+const float HK7 = q0*q3 - q1*q2;
+const float HK8 = HK3*HK7;
+const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
+const float HK10 = HK9*Kaccy;
+const float HK11 = q0*q1 + q2*q3;
+const float HK12 = 2*HK11;
+const float HK13 = 2*HK7;
+const float HK14 = 2*HK5;
+const float HK15 = 2*HK2;
+const float HK16 = 2*HK4;
+const float HK17 = 2*HK6;
+const float HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5];
+const float HK19 = powf(Kaccy, 2);
+const float HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23];
+const float HK21 = HK13*P[4][22];
+const float HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22];
+const float HK23 = HK13*HK19;
+const float HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5];
+const float HK25 = HK9*P[5][23];
+const float HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5];
+const float HK27 = HK19*HK9;
+const float HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23];
+const float HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5];
+const float HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5];
+const float HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5];
+const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
+
+
+// Observation Jacobians
+Hfusion[0] = -HK2*HK3;
+Hfusion[1] = -HK3*HK4;
+Hfusion[2] = -HK3*HK5;
+Hfusion[3] = HK3*HK6;
+Hfusion[4] = HK8;
+Hfusion[5] = -HK10;
+Hfusion[6] = -HK11*HK3;
+Hfusion[7] = 0;
+Hfusion[8] = 0;
+Hfusion[9] = 0;
+Hfusion[10] = 0;
+Hfusion[11] = 0;
+Hfusion[12] = 0;
+Hfusion[13] = 0;
+Hfusion[14] = 0;
+Hfusion[15] = 0;
+Hfusion[16] = 0;
+Hfusion[17] = 0;
+Hfusion[18] = 0;
+Hfusion[19] = 0;
+Hfusion[20] = 0;
+Hfusion[21] = 0;
+Hfusion[22] = -HK8;
+Hfusion[23] = HK10;
+
+
+// Kalman gains
+Kfusion[0] = -HK18*HK32;
+Kfusion[1] = -HK30*HK32;
+Kfusion[2] = -HK29*HK32;
+Kfusion[3] = -HK31*HK32;
+Kfusion[4] = -HK24*HK32;
+Kfusion[5] = -HK26*HK32;
+Kfusion[6] = -HK20*HK32;
+Kfusion[7] = -HK32*(HK12*P[6][7] - HK13*P[4][7] + HK13*P[7][22] + HK14*P[2][7] + HK15*P[0][7] + HK16*P[1][7] - HK17*P[3][7] + HK9*P[5][7] - HK9*P[7][23]);
+Kfusion[8] = -HK32*(HK12*P[6][8] - HK13*P[4][8] + HK13*P[8][22] + HK14*P[2][8] + HK15*P[0][8] + HK16*P[1][8] - HK17*P[3][8] + HK9*P[5][8] - HK9*P[8][23]);
+Kfusion[9] = -HK32*(HK12*P[6][9] - HK13*P[4][9] + HK13*P[9][22] + HK14*P[2][9] + HK15*P[0][9] + HK16*P[1][9] - HK17*P[3][9] + HK9*P[5][9] - HK9*P[9][23]);
+Kfusion[10] = -HK32*(HK12*P[6][10] + HK13*P[10][22] - HK13*P[4][10] + HK14*P[2][10] + HK15*P[0][10] + HK16*P[1][10] - HK17*P[3][10] - HK9*P[10][23] + HK9*P[5][10]);
+Kfusion[11] = -HK32*(HK12*P[6][11] + HK13*P[11][22] - HK13*P[4][11] + HK14*P[2][11] + HK15*P[0][11] + HK16*P[1][11] - HK17*P[3][11] - HK9*P[11][23] + HK9*P[5][11]);
+Kfusion[12] = -HK32*(HK12*P[6][12] + HK13*P[12][22] - HK13*P[4][12] + HK14*P[2][12] + HK15*P[0][12] + HK16*P[1][12] - HK17*P[3][12] - HK9*P[12][23] + HK9*P[5][12]);
+Kfusion[13] = -HK32*(HK12*P[6][13] + HK13*P[13][22] - HK13*P[4][13] + HK14*P[2][13] + HK15*P[0][13] + HK16*P[1][13] - HK17*P[3][13] - HK9*P[13][23] + HK9*P[5][13]);
+Kfusion[14] = -HK32*(HK12*P[6][14] + HK13*P[14][22] - HK13*P[4][14] + HK14*P[2][14] + HK15*P[0][14] + HK16*P[1][14] - HK17*P[3][14] - HK9*P[14][23] + HK9*P[5][14]);
+Kfusion[15] = -HK32*(HK12*P[6][15] + HK13*P[15][22] - HK13*P[4][15] + HK14*P[2][15] + HK15*P[0][15] + HK16*P[1][15] - HK17*P[3][15] - HK9*P[15][23] + HK9*P[5][15]);
+Kfusion[16] = -HK32*(HK12*P[6][16] + HK13*P[16][22] - HK13*P[4][16] + HK14*P[2][16] + HK15*P[0][16] + HK16*P[1][16] - HK17*P[3][16] - HK9*P[16][23] + HK9*P[5][16]);
+Kfusion[17] = -HK32*(HK12*P[6][17] + HK13*P[17][22] - HK13*P[4][17] + HK14*P[2][17] + HK15*P[0][17] + HK16*P[1][17] - HK17*P[3][17] - HK9*P[17][23] + HK9*P[5][17]);
+Kfusion[18] = -HK32*(HK12*P[6][18] + HK13*P[18][22] - HK13*P[4][18] + HK14*P[2][18] + HK15*P[0][18] + HK16*P[1][18] - HK17*P[3][18] - HK9*P[18][23] + HK9*P[5][18]);
+Kfusion[19] = -HK32*(HK12*P[6][19] + HK13*P[19][22] - HK13*P[4][19] + HK14*P[2][19] + HK15*P[0][19] + HK16*P[1][19] - HK17*P[3][19] - HK9*P[19][23] + HK9*P[5][19]);
+Kfusion[20] = -HK32*(HK12*P[6][20] + HK13*P[20][22] - HK13*P[4][20] + HK14*P[2][20] + HK15*P[0][20] + HK16*P[1][20] - HK17*P[3][20] - HK9*P[20][23] + HK9*P[5][20]);
+Kfusion[21] = -HK32*(HK12*P[6][21] + HK13*P[21][22] - HK13*P[4][21] + HK14*P[2][21] + HK15*P[0][21] + HK16*P[1][21] - HK17*P[3][21] - HK9*P[21][23] + HK9*P[5][21]);
+Kfusion[22] = -HK22*HK32;
+Kfusion[23] = -HK28*HK32;
+
+