diff --git a/CMakeLists.txt b/CMakeLists.txt
index 351fe611ce..742c50bee4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -54,6 +54,7 @@ px4_add_module(
 		EKF/sideslip_fusion.cpp
 		EKF/terrain_estimator.cpp
 		EKF/vel_pos_fusion.cpp
+		EKF/drag_fusion.cpp
 		l1/ecl_l1_pos_controller.cpp
 		validation/data_validator.cpp
 		validation/data_validator_group.cpp
diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt
index 10d92ad81a..f87fe30993 100644
--- a/EKF/CMakeLists.txt
+++ b/EKF/CMakeLists.txt
@@ -71,6 +71,7 @@ set(SRCS
 	sideslip_fusion.cpp
 	terrain_estimator.cpp
 	vel_pos_fusion.cpp
+	drag_fusion.cpp
 	)
 
 add_definitions(-DPOSIX_SHARED)
diff --git a/EKF/common.h b/EKF/common.h
index 0b05d7921d..366c287187 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -157,6 +157,7 @@ struct extVisionSample {
 #define MASK_INHIBIT_ACC_BIAS (1<<2)  // set to true to inhibit estimation of accelerometer delta velocity bias
 #define MASK_USE_EVPOS	(1<<3)  // set to true to use external vision NED position data
 #define MASK_USE_EVYAW  (1<<4)	// set to true to use exernal vision quaternion data for yaw
+#define MASK_USE_DRAG  (1<<5)	// set to true to use the multi-rotor drag model to estimate wind
 
 // Integer definitions for mag_fusion_type
 #define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
@@ -176,7 +177,7 @@ struct extVisionSample {
 
 struct parameters {
 	// measurement source control
-	int fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
+	int fusion_mode;		// bitmasked integer that selects which aiding sources will be used
 	int vdist_sensor_type;		// selects the primary source for height data
 	int sensor_interval_min_ms;	// minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
 
@@ -447,7 +448,7 @@ union innovation_fault_status_u {
 		bool reject_sideslip: 1;	// 8 - true if the synthetic sideslip observation has been rejected
 		bool reject_hagl: 1;		// 9 - true if the height above ground observation has been rejected
 		bool reject_optflow_X: 1;	// 10 - true if the X optical flow observation has been rejected
-		bool reject_optflow_Y: 1;	// 11 - true if the Y optical flow observation has been rejected
+		bool reject_optflow_Y: 1;	// 11 - true if the Y optical flow observation has been rejected		
 	} flags;
 	uint16_t value;
 
diff --git a/EKF/control.cpp b/EKF/control.cpp
index 43af6e137c..f19fabfe73 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -106,6 +106,7 @@ void Ekf::controlFusionModes()
 	controlRangeFinderFusion();
 	controlAirDataFusion();
 	controlBetaFusion();
+	controlDragFusion();
 
 	// for efficiency, fusion of direct state observations for position and velocity is performed sequentially
 	// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
@@ -793,7 +794,7 @@ void Ekf::controlAirDataFusion()
         // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
         bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
         bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
-        if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out) {
+	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
                 // if the airspeed or sideslip measurements have timed out for 10 seconds we declare the wind estimate to be invalid
 		_control_status.flags.wind = false;
 
@@ -827,7 +828,7 @@ void Ekf::controlBetaFusion()
         // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
         bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
         bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
-        if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out){
+	if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)){
                 _control_status.flags.wind = false;
 
         }
@@ -860,6 +861,22 @@ void Ekf::controlBetaFusion()
 
 }
 
+void Ekf::controlDragFusion()
+{
+	if (_params.fusion_mode & MASK_USE_DRAG && _control_status.flags.in_air) {
+		if (!_control_status.flags.wind) {
+			_control_status.flags.wind = true;
+			resetWindStates();
+			resetWindCovariance();
+
+		} else {
+			fuseDrag();
+		}
+	} else {
+		_control_status.flags.wind = false;
+	}
+}
+
 void Ekf::controlMagFusion()
 {
 	// If we are using external vision data for heading then no magnetometer fusion is used
diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index eb9b7b0b91..63a66a8670 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -864,8 +864,8 @@ void Ekf::resetWindCovariance()
 
 	} else {
 		// without airspeed, start with a small initial uncertainty to improve the initial estimate
-		P[22][22] = sq(5.0f);
-		P[23][23] = sq(5.0f);
+		P[22][22] = sq(1.0f);
+		P[23][23] = sq(1.0f);
 
 	}
 
diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp
new file mode 100644
index 0000000000..e910e85bd6
--- /dev/null
+++ b/EKF/drag_fusion.cpp
@@ -0,0 +1,315 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drag_fusion.cpp
+ * body frame drag fusion methods used for multi-rotor wind estimation.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+#include "../ecl.h"
+#include "ekf.h"
+#include "mathlib.h"
+
+void Ekf::fuseDrag()
+{ 
+	float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
+	float H_ACC[24] = {}; // Observation Jacobian
+	float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
+	float Kfusion[24] = {}; // Kalman gain vector
+	float R_ACC = 2.5f; // observation noise in specific force drag (m/sec**2) TODO add tuning parameter
+
+	float eas2tas = 1.0f; // conversion factor from EAS to TAS - TODO calculate from barometric pressure
+	float rho = 1.225f / sqrtf(eas2tas); // air density (kg/m**3)
+
+	// measured steady state EAS at specified tilt angle in X and Y directions
+	// TODO make the speeds tuning parameters
+	const float max_spd_x = 15.0f;
+	const float max_spd_y = 15.0f;
+	const float tilt_ref_angle = math::radians(25.0f);
+
+	// calculate inverse of ballistic coefficient from max speed and tilt angle
+	// used to predict the acceleration for a given predicted state vector
+	float numerator = 2.0f*_gravity_mss*tanf(tilt_ref_angle);
+	float BC_inv_x = numerator / (rho * sq(max_spd_x));
+	float BC_inv_y = numerator / (rho * sq(max_spd_y));
+
+	// get latest estimated orientation
+	float q0 = _state.quat_nominal(0);
+	float q1 = _state.quat_nominal(1);
+	float q2 = _state.quat_nominal(2);
+	float q3 = _state.quat_nominal(3);
+
+	// get latest velocity in earth frame
+	float vn = _state.vel(0);
+	float ve = _state.vel(1);
+	float vd = _state.vel(2);
+
+	// get latest wind velocity in earth frame
+	float vwn = _state.wind_vel(0);
+	float vwe = _state.wind_vel(1);
+
+	// calculate measured XY specific forces if enough data
+	if (_imu_sample_delayed.delta_vel_dt < 1e-3f) {
+		return;
+	}
+
+	// predicted specific forces
+	// calculate relative wind velocity in earth frame and rotte into body frame
+	Vector3f rel_wind;
+	rel_wind(0) = vn - vwn;
+	rel_wind(1) = ve - vwe;
+	rel_wind(2) = vd;
+	matrix::Dcm<float> earth_to_body(_state.quat_nominal);
+	earth_to_body = earth_to_body.transpose();
+	rel_wind = earth_to_body * rel_wind;
+
+	// perform sequential fusion of XY specific forces
+	for (uint8_t axis_index = 0; axis_index <2; axis_index++) {
+		// calculate observation jacobiam and Kalman gain vectors
+		if (axis_index == 0) {
+			// Estimate the airspeed from the measured drag force and ballistic coefficient
+			float mea_acc = (_imu_sample_delayed.delta_vel(axis_index)  - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt;
+			float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
+
+			// Estimate the derivative of specific force wrt airspeed along the X axis
+			// Limit lower value to prevent artithmetic exceptions
+			float Kacc = fmax(1e-1f,rho * BC_inv_x * airSpd);
+
+			SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+			SH_ACC[1] = vn - vwn;
+			SH_ACC[2] = ve - vwe;
+			SH_ACC[3] = 2.0f*q0*q3 + 2.0f*q1*q2;
+			H_ACC[0] = -Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
+			H_ACC[1] = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
+			H_ACC[2] = Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
+			H_ACC[3] = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
+			H_ACC[4] = -Kacc*SH_ACC[0];
+			H_ACC[5] = -Kacc*SH_ACC[3];
+			H_ACC[6] = Kacc*(2.0f*q0*q2 - 2.0f*q1*q3);
+			H_ACC[22] = Kacc*SH_ACC[0];
+			H_ACC[23] = Kacc*SH_ACC[3];
+			_drag_innov_var[0] = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P[4][4]*SH_ACC[0] + Kacc*P[5][4]*SH_ACC[3] - Kacc*P[22][4]*SH_ACC[0] - Kacc*P[23][4]*SH_ACC[3] - Kacc*P[6][4]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][4]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][4]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][4]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][4]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*SH_ACC[3]*(Kacc*P[4][5]*SH_ACC[0] + Kacc*P[5][5]*SH_ACC[3] - Kacc*P[22][5]*SH_ACC[0] - Kacc*P[23][5]*SH_ACC[3] - Kacc*P[6][5]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][5]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][5]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][5]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][5]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[0]*(Kacc*P[4][22]*SH_ACC[0] + Kacc*P[5][22]*SH_ACC[3] - Kacc*P[22][22]*SH_ACC[0] - Kacc*P[23][22]*SH_ACC[3] - Kacc*P[6][22]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][22]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][22]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][22]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][22]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[3]*(Kacc*P[4][23]*SH_ACC[0] + Kacc*P[5][23]*SH_ACC[3] - Kacc*P[22][23]*SH_ACC[0] - Kacc*P[23][23]*SH_ACC[3] - Kacc*P[6][23]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][23]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][23]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][23]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][23]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q0*q2 - 2.0f*q1*q3)*(Kacc*P[4][6]*SH_ACC[0] + Kacc*P[5][6]*SH_ACC[3] - Kacc*P[22][6]*SH_ACC[0] - Kacc*P[23][6]*SH_ACC[3] - Kacc*P[6][6]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][6]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][6]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][6]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][6]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P[4][0]*SH_ACC[0] + Kacc*P[5][0]*SH_ACC[3] - Kacc*P[22][0]*SH_ACC[0] - Kacc*P[23][0]*SH_ACC[3] - Kacc*P[6][0]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][0]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][0]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][0]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][0]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P[4][1]*SH_ACC[0] + Kacc*P[5][1]*SH_ACC[3] - Kacc*P[22][1]*SH_ACC[0] - Kacc*P[23][1]*SH_ACC[3] - Kacc*P[6][1]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][1]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][1]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][1]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][1]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P[4][2]*SH_ACC[0] + Kacc*P[5][2]*SH_ACC[3] - Kacc*P[22][2]*SH_ACC[0] - Kacc*P[23][2]*SH_ACC[3] - Kacc*P[6][2]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][2]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][2]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][2]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][2]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P[4][3]*SH_ACC[0] + Kacc*P[5][3]*SH_ACC[3] - Kacc*P[22][3]*SH_ACC[0] - Kacc*P[23][3]*SH_ACC[3] - Kacc*P[6][3]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][3]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][3]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][3]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][3]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)));
+			if (_drag_innov_var[0] < R_ACC) {
+				return;
+			}
+			SK_ACC[0] = 1.0f/_drag_innov_var[0];
+			SK_ACC[1] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
+			SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
+			SK_ACC[3] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
+			SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
+			SK_ACC[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
+			SK_ACC[6] = SH_ACC[3];
+// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
+//			Kfusion[0] = -SK_ACC[0]*(Kacc*P[0][4]*SH_ACC[0] - Kacc*P[0][22]*SH_ACC[0] + Kacc*P[0][0]*SK_ACC[3] - Kacc*P[0][2]*SK_ACC[2] + Kacc*P[0][3]*SK_ACC[1] + Kacc*P[0][1]*SK_ACC[4] + Kacc*P[0][5]*SK_ACC[6] - Kacc*P[0][6]*SK_ACC[5] - Kacc*P[0][23]*SK_ACC[6]);
+//			Kfusion[1] = -SK_ACC[0]*(Kacc*P[1][4]*SH_ACC[0] - Kacc*P[1][22]*SH_ACC[0] + Kacc*P[1][0]*SK_ACC[3] - Kacc*P[1][2]*SK_ACC[2] + Kacc*P[1][3]*SK_ACC[1] + Kacc*P[1][1]*SK_ACC[4] + Kacc*P[1][5]*SK_ACC[6] - Kacc*P[1][6]*SK_ACC[5] - Kacc*P[1][23]*SK_ACC[6]);
+//			Kfusion[2] = -SK_ACC[0]*(Kacc*P[2][4]*SH_ACC[0] - Kacc*P[2][22]*SH_ACC[0] + Kacc*P[2][0]*SK_ACC[3] - Kacc*P[2][2]*SK_ACC[2] + Kacc*P[2][3]*SK_ACC[1] + Kacc*P[2][1]*SK_ACC[4] + Kacc*P[2][5]*SK_ACC[6] - Kacc*P[2][6]*SK_ACC[5] - Kacc*P[2][23]*SK_ACC[6]);
+//			Kfusion[3] = -SK_ACC[0]*(Kacc*P[3][4]*SH_ACC[0] - Kacc*P[3][22]*SH_ACC[0] + Kacc*P[3][0]*SK_ACC[3] - Kacc*P[3][2]*SK_ACC[2] + Kacc*P[3][3]*SK_ACC[1] + Kacc*P[3][1]*SK_ACC[4] + Kacc*P[3][5]*SK_ACC[6] - Kacc*P[3][6]*SK_ACC[5] - Kacc*P[3][23]*SK_ACC[6]);
+//			Kfusion[4] = -SK_ACC[0]*(Kacc*P[4][4]*SH_ACC[0] - Kacc*P[4][22]*SH_ACC[0] + Kacc*P[4][0]*SK_ACC[3] - Kacc*P[4][2]*SK_ACC[2] + Kacc*P[4][3]*SK_ACC[1] + Kacc*P[4][1]*SK_ACC[4] + Kacc*P[4][5]*SK_ACC[6] - Kacc*P[4][6]*SK_ACC[5] - Kacc*P[4][23]*SK_ACC[6]);
+//			Kfusion[5] = -SK_ACC[0]*(Kacc*P[5][4]*SH_ACC[0] - Kacc*P[5][22]*SH_ACC[0] + Kacc*P[5][0]*SK_ACC[3] - Kacc*P[5][2]*SK_ACC[2] + Kacc*P[5][3]*SK_ACC[1] + Kacc*P[5][1]*SK_ACC[4] + Kacc*P[5][5]*SK_ACC[6] - Kacc*P[5][6]*SK_ACC[5] - Kacc*P[5][23]*SK_ACC[6]);
+//			Kfusion[6] = -SK_ACC[0]*(Kacc*P[6][4]*SH_ACC[0] - Kacc*P[6][22]*SH_ACC[0] + Kacc*P[6][0]*SK_ACC[3] - Kacc*P[6][2]*SK_ACC[2] + Kacc*P[6][3]*SK_ACC[1] + Kacc*P[6][1]*SK_ACC[4] + Kacc*P[6][5]*SK_ACC[6] - Kacc*P[6][6]*SK_ACC[5] - Kacc*P[6][23]*SK_ACC[6]);
+//			Kfusion[7] = -SK_ACC[0]*(Kacc*P[7][4]*SH_ACC[0] - Kacc*P[7][22]*SH_ACC[0] + Kacc*P[7][0]*SK_ACC[3] - Kacc*P[7][2]*SK_ACC[2] + Kacc*P[7][3]*SK_ACC[1] + Kacc*P[7][1]*SK_ACC[4] + Kacc*P[7][5]*SK_ACC[6] - Kacc*P[7][6]*SK_ACC[5] - Kacc*P[7][23]*SK_ACC[6]);
+//			Kfusion[8] = -SK_ACC[0]*(Kacc*P[8][4]*SH_ACC[0] - Kacc*P[8][22]*SH_ACC[0] + Kacc*P[8][0]*SK_ACC[3] - Kacc*P[8][2]*SK_ACC[2] + Kacc*P[8][3]*SK_ACC[1] + Kacc*P[8][1]*SK_ACC[4] + Kacc*P[8][5]*SK_ACC[6] - Kacc*P[8][6]*SK_ACC[5] - Kacc*P[8][23]*SK_ACC[6]);
+//			Kfusion[9] = -SK_ACC[0]*(Kacc*P[9][4]*SH_ACC[0] - Kacc*P[9][22]*SH_ACC[0] + Kacc*P[9][0]*SK_ACC[3] - Kacc*P[9][2]*SK_ACC[2] + Kacc*P[9][3]*SK_ACC[1] + Kacc*P[9][1]*SK_ACC[4] + Kacc*P[9][5]*SK_ACC[6] - Kacc*P[9][6]*SK_ACC[5] - Kacc*P[9][23]*SK_ACC[6]);
+//			Kfusion[10] = -SK_ACC[0]*(Kacc*P[10][4]*SH_ACC[0] - Kacc*P[10][22]*SH_ACC[0] + Kacc*P[10][0]*SK_ACC[3] - Kacc*P[10][2]*SK_ACC[2] + Kacc*P[10][3]*SK_ACC[1] + Kacc*P[10][1]*SK_ACC[4] + Kacc*P[10][5]*SK_ACC[6] - Kacc*P[10][6]*SK_ACC[5] - Kacc*P[10][23]*SK_ACC[6]);
+//			Kfusion[11] = -SK_ACC[0]*(Kacc*P[11][4]*SH_ACC[0] - Kacc*P[11][22]*SH_ACC[0] + Kacc*P[11][0]*SK_ACC[3] - Kacc*P[11][2]*SK_ACC[2] + Kacc*P[11][3]*SK_ACC[1] + Kacc*P[11][1]*SK_ACC[4] + Kacc*P[11][5]*SK_ACC[6] - Kacc*P[11][6]*SK_ACC[5] - Kacc*P[11][23]*SK_ACC[6]);
+//			Kfusion[12] = -SK_ACC[0]*(Kacc*P[12][4]*SH_ACC[0] - Kacc*P[12][22]*SH_ACC[0] + Kacc*P[12][0]*SK_ACC[3] - Kacc*P[12][2]*SK_ACC[2] + Kacc*P[12][3]*SK_ACC[1] + Kacc*P[12][1]*SK_ACC[4] + Kacc*P[12][5]*SK_ACC[6] - Kacc*P[12][6]*SK_ACC[5] - Kacc*P[12][23]*SK_ACC[6]);
+//			Kfusion[13] = -SK_ACC[0]*(Kacc*P[13][4]*SH_ACC[0] - Kacc*P[13][22]*SH_ACC[0] + Kacc*P[13][0]*SK_ACC[3] - Kacc*P[13][2]*SK_ACC[2] + Kacc*P[13][3]*SK_ACC[1] + Kacc*P[13][1]*SK_ACC[4] + Kacc*P[13][5]*SK_ACC[6] - Kacc*P[13][6]*SK_ACC[5] - Kacc*P[13][23]*SK_ACC[6]);
+//			Kfusion[14] = -SK_ACC[0]*(Kacc*P[14][4]*SH_ACC[0] - Kacc*P[14][22]*SH_ACC[0] + Kacc*P[14][0]*SK_ACC[3] - Kacc*P[14][2]*SK_ACC[2] + Kacc*P[14][3]*SK_ACC[1] + Kacc*P[14][1]*SK_ACC[4] + Kacc*P[14][5]*SK_ACC[6] - Kacc*P[14][6]*SK_ACC[5] - Kacc*P[14][23]*SK_ACC[6]);
+//			Kfusion[15] = -SK_ACC[0]*(Kacc*P[15][4]*SH_ACC[0] - Kacc*P[15][22]*SH_ACC[0] + Kacc*P[15][0]*SK_ACC[3] - Kacc*P[15][2]*SK_ACC[2] + Kacc*P[15][3]*SK_ACC[1] + Kacc*P[15][1]*SK_ACC[4] + Kacc*P[15][5]*SK_ACC[6] - Kacc*P[15][6]*SK_ACC[5] - Kacc*P[15][23]*SK_ACC[6]);
+//			Kfusion[16] = -SK_ACC[0]*(Kacc*P[16][4]*SH_ACC[0] - Kacc*P[16][22]*SH_ACC[0] + Kacc*P[16][0]*SK_ACC[3] - Kacc*P[16][2]*SK_ACC[2] + Kacc*P[16][3]*SK_ACC[1] + Kacc*P[16][1]*SK_ACC[4] + Kacc*P[16][5]*SK_ACC[6] - Kacc*P[16][6]*SK_ACC[5] - Kacc*P[16][23]*SK_ACC[6]);
+//			Kfusion[17] = -SK_ACC[0]*(Kacc*P[17][4]*SH_ACC[0] - Kacc*P[17][22]*SH_ACC[0] + Kacc*P[17][0]*SK_ACC[3] - Kacc*P[17][2]*SK_ACC[2] + Kacc*P[17][3]*SK_ACC[1] + Kacc*P[17][1]*SK_ACC[4] + Kacc*P[17][5]*SK_ACC[6] - Kacc*P[17][6]*SK_ACC[5] - Kacc*P[17][23]*SK_ACC[6]);
+//			Kfusion[18] = -SK_ACC[0]*(Kacc*P[18][4]*SH_ACC[0] - Kacc*P[18][22]*SH_ACC[0] + Kacc*P[18][0]*SK_ACC[3] - Kacc*P[18][2]*SK_ACC[2] + Kacc*P[18][3]*SK_ACC[1] + Kacc*P[18][1]*SK_ACC[4] + Kacc*P[18][5]*SK_ACC[6] - Kacc*P[18][6]*SK_ACC[5] - Kacc*P[18][23]*SK_ACC[6]);
+//			Kfusion[19] = -SK_ACC[0]*(Kacc*P[19][4]*SH_ACC[0] - Kacc*P[19][22]*SH_ACC[0] + Kacc*P[19][0]*SK_ACC[3] - Kacc*P[19][2]*SK_ACC[2] + Kacc*P[19][3]*SK_ACC[1] + Kacc*P[19][1]*SK_ACC[4] + Kacc*P[19][5]*SK_ACC[6] - Kacc*P[19][6]*SK_ACC[5] - Kacc*P[19][23]*SK_ACC[6]);
+//			Kfusion[20] = -SK_ACC[0]*(Kacc*P[20][4]*SH_ACC[0] - Kacc*P[20][22]*SH_ACC[0] + Kacc*P[20][0]*SK_ACC[3] - Kacc*P[20][2]*SK_ACC[2] + Kacc*P[20][3]*SK_ACC[1] + Kacc*P[20][1]*SK_ACC[4] + Kacc*P[20][5]*SK_ACC[6] - Kacc*P[20][6]*SK_ACC[5] - Kacc*P[20][23]*SK_ACC[6]);
+//			Kfusion[21] = -SK_ACC[0]*(Kacc*P[21][4]*SH_ACC[0] - Kacc*P[21][22]*SH_ACC[0] + Kacc*P[21][0]*SK_ACC[3] - Kacc*P[21][2]*SK_ACC[2] + Kacc*P[21][3]*SK_ACC[1] + Kacc*P[21][1]*SK_ACC[4] + Kacc*P[21][5]*SK_ACC[6] - Kacc*P[21][6]*SK_ACC[5] - Kacc*P[21][23]*SK_ACC[6]);
+			Kfusion[22] = -SK_ACC[0]*(Kacc*P[22][4]*SH_ACC[0] - Kacc*P[22][22]*SH_ACC[0] + Kacc*P[22][0]*SK_ACC[3] - Kacc*P[22][2]*SK_ACC[2] + Kacc*P[22][3]*SK_ACC[1] + Kacc*P[22][1]*SK_ACC[4] + Kacc*P[22][5]*SK_ACC[6] - Kacc*P[22][6]*SK_ACC[5] - Kacc*P[22][23]*SK_ACC[6]);
+			Kfusion[23] = -SK_ACC[0]*(Kacc*P[23][4]*SH_ACC[0] - Kacc*P[23][22]*SH_ACC[0] + Kacc*P[23][0]*SK_ACC[3] - Kacc*P[23][2]*SK_ACC[2] + Kacc*P[23][3]*SK_ACC[1] + Kacc*P[23][1]*SK_ACC[4] + Kacc*P[23][5]*SK_ACC[6] - Kacc*P[23][6]*SK_ACC[5] - Kacc*P[23][23]*SK_ACC[6]);
+
+			// calculate the predicted acceleration and innovation measured along the X body axis
+			float drag_sign;
+			if (rel_wind(axis_index) >= 0.0f) {
+			    drag_sign = 1.0f;
+			} else {
+			    drag_sign = -1.0f;
+			}
+			float predAccel = -BC_inv_x * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
+			_drag_innov[axis_index] = predAccel - mea_acc;
+			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
+
+		} else if (axis_index == 1) {
+			// Estimate the airspeed from the measured drag force and ballistic coefficient
+			float mea_acc = (_imu_sample_delayed.delta_vel(axis_index)  - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt;
+			float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
+
+			// Estimate the derivative of specific force wrt airspeed along the X axis
+			// Limit lower value to prevent artithmetic exceptions
+			float Kacc = fmax(1e-1f,rho * BC_inv_y * airSpd);
+
+			SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
+			SH_ACC[1] = vn - vwn;
+			SH_ACC[2] = ve - vwe;
+			H_ACC[0] = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
+			H_ACC[1] = -Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
+			H_ACC[2] = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
+			H_ACC[3] = Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
+			H_ACC[4] = Kacc*(2.0f*q0*q3 - 2.0f*q1*q2);
+			H_ACC[5] = -Kacc*SH_ACC[0];
+			H_ACC[6] = -Kacc*(2.0f*q0*q1 + 2.0f*q2*q3);
+			H_ACC[22] = -2.0f*Kacc*(q0*q3 - q1*q2);
+			H_ACC[23] = Kacc*SH_ACC[0];
+			_drag_innov_var[1] = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P[5][5]*SH_ACC[0] - Kacc*P[23][5]*SH_ACC[0] - Kacc*P[4][5]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][5]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][5]*(q0*q3 - q1*q2) + Kacc*P[0][5]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][5]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][5]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][5]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*SH_ACC[0]*(Kacc*P[5][23]*SH_ACC[0] - Kacc*P[23][23]*SH_ACC[0] - Kacc*P[4][23]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][23]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][23]*(q0*q3 - q1*q2) + Kacc*P[0][23]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][23]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][23]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][23]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*q3 - 2.0f*q1*q2)*(Kacc*P[5][4]*SH_ACC[0] - Kacc*P[23][4]*SH_ACC[0] - Kacc*P[4][4]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][4]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][4]*(q0*q3 - q1*q2) + Kacc*P[0][4]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][4]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][4]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][4]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*q1 + 2.0f*q2*q3)*(Kacc*P[5][6]*SH_ACC[0] - Kacc*P[23][6]*SH_ACC[0] - Kacc*P[4][6]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][6]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][6]*(q0*q3 - q1*q2) + Kacc*P[0][6]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][6]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][6]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][6]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + 2*Kacc*(q0*q3 - q1*q2)*(Kacc*P[5][22]*SH_ACC[0] - Kacc*P[23][22]*SH_ACC[0] - Kacc*P[4][22]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][22]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][22]*(q0*q3 - q1*q2) + Kacc*P[0][22]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][22]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][22]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][22]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P[5][0]*SH_ACC[0] - Kacc*P[23][0]*SH_ACC[0] - Kacc*P[4][0]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][0]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][0]*(q0*q3 - q1*q2) + Kacc*P[0][0]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][0]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][0]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][0]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P[5][1]*SH_ACC[0] - Kacc*P[23][1]*SH_ACC[0] - Kacc*P[4][1]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][1]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][1]*(q0*q3 - q1*q2) + Kacc*P[0][1]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][1]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][1]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][1]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P[5][2]*SH_ACC[0] - Kacc*P[23][2]*SH_ACC[0] - Kacc*P[4][2]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][2]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][2]*(q0*q3 - q1*q2) + Kacc*P[0][2]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][2]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][2]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][2]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P[5][3]*SH_ACC[0] - Kacc*P[23][3]*SH_ACC[0] - Kacc*P[4][3]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][3]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][3]*(q0*q3 - q1*q2) + Kacc*P[0][3]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][3]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][3]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][3]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)));
+			if (_drag_innov_var[1] < R_ACC) {
+				// calculation is badly conditioned
+				return;
+			}
+			SK_ACC[0] = 1.0f/_drag_innov_var[1];
+			SK_ACC[1] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
+			SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
+			SK_ACC[3] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
+			SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
+			SK_ACC[5] = 2.0f*q0*q3 - 2.0f*q1*q2;
+			SK_ACC[6] = q0*q3 - q1*q2;
+			SK_ACC[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
+			SK_ACC[8] = SH_ACC[0];
+// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
+//			Kfusion[0] = -SK_ACC[0]*(Kacc*P[0][0]*SK_ACC[3] + Kacc*P[0][1]*SK_ACC[2] - Kacc*P[0][3]*SK_ACC[1] + Kacc*P[0][2]*SK_ACC[4] - Kacc*P[0][4]*SK_ACC[5] + Kacc*P[0][5]*SK_ACC[8] + Kacc*P[0][6]*SK_ACC[7] + 2*Kacc*P[0][22]*SK_ACC[6] - Kacc*P[0][23]*SK_ACC[8]);
+//			Kfusion[1] = -SK_ACC[0]*(Kacc*P[1][0]*SK_ACC[3] + Kacc*P[1][1]*SK_ACC[2] - Kacc*P[1][3]*SK_ACC[1] + Kacc*P[1][2]*SK_ACC[4] - Kacc*P[1][4]*SK_ACC[5] + Kacc*P[1][5]*SK_ACC[8] + Kacc*P[1][6]*SK_ACC[7] + 2*Kacc*P[1][22]*SK_ACC[6] - Kacc*P[1][23]*SK_ACC[8]);
+//			Kfusion[2] = -SK_ACC[0]*(Kacc*P[2][0]*SK_ACC[3] + Kacc*P[2][1]*SK_ACC[2] - Kacc*P[2][3]*SK_ACC[1] + Kacc*P[2][2]*SK_ACC[4] - Kacc*P[2][4]*SK_ACC[5] + Kacc*P[2][5]*SK_ACC[8] + Kacc*P[2][6]*SK_ACC[7] + 2*Kacc*P[2][22]*SK_ACC[6] - Kacc*P[2][23]*SK_ACC[8]);
+//			Kfusion[3] = -SK_ACC[0]*(Kacc*P[3][0]*SK_ACC[3] + Kacc*P[3][1]*SK_ACC[2] - Kacc*P[3][3]*SK_ACC[1] + Kacc*P[3][2]*SK_ACC[4] - Kacc*P[3][4]*SK_ACC[5] + Kacc*P[3][5]*SK_ACC[8] + Kacc*P[3][6]*SK_ACC[7] + 2*Kacc*P[3][22]*SK_ACC[6] - Kacc*P[3][23]*SK_ACC[8]);
+//			Kfusion[4] = -SK_ACC[0]*(Kacc*P[4][0]*SK_ACC[3] + Kacc*P[4][1]*SK_ACC[2] - Kacc*P[4][3]*SK_ACC[1] + Kacc*P[4][2]*SK_ACC[4] - Kacc*P[4][4]*SK_ACC[5] + Kacc*P[4][5]*SK_ACC[8] + Kacc*P[4][6]*SK_ACC[7] + 2*Kacc*P[4][22]*SK_ACC[6] - Kacc*P[4][23]*SK_ACC[8]);
+//			Kfusion[5] = -SK_ACC[0]*(Kacc*P[5][0]*SK_ACC[3] + Kacc*P[5][1]*SK_ACC[2] - Kacc*P[5][3]*SK_ACC[1] + Kacc*P[5][2]*SK_ACC[4] - Kacc*P[5][4]*SK_ACC[5] + Kacc*P[5][5]*SK_ACC[8] + Kacc*P[5][6]*SK_ACC[7] + 2*Kacc*P[5][22]*SK_ACC[6] - Kacc*P[5][23]*SK_ACC[8]);
+//			Kfusion[6] = -SK_ACC[0]*(Kacc*P[6][0]*SK_ACC[3] + Kacc*P[6][1]*SK_ACC[2] - Kacc*P[6][3]*SK_ACC[1] + Kacc*P[6][2]*SK_ACC[4] - Kacc*P[6][4]*SK_ACC[5] + Kacc*P[6][5]*SK_ACC[8] + Kacc*P[6][6]*SK_ACC[7] + 2*Kacc*P[6][22]*SK_ACC[6] - Kacc*P[6][23]*SK_ACC[8]);
+//			Kfusion[7] = -SK_ACC[0]*(Kacc*P[7][0]*SK_ACC[3] + Kacc*P[7][1]*SK_ACC[2] - Kacc*P[7][3]*SK_ACC[1] + Kacc*P[7][2]*SK_ACC[4] - Kacc*P[7][4]*SK_ACC[5] + Kacc*P[7][5]*SK_ACC[8] + Kacc*P[7][6]*SK_ACC[7] + 2*Kacc*P[7][22]*SK_ACC[6] - Kacc*P[7][23]*SK_ACC[8]);
+//			Kfusion[8] = -SK_ACC[0]*(Kacc*P[8][0]*SK_ACC[3] + Kacc*P[8][1]*SK_ACC[2] - Kacc*P[8][3]*SK_ACC[1] + Kacc*P[8][2]*SK_ACC[4] - Kacc*P[8][4]*SK_ACC[5] + Kacc*P[8][5]*SK_ACC[8] + Kacc*P[8][6]*SK_ACC[7] + 2*Kacc*P[8][22]*SK_ACC[6] - Kacc*P[8][23]*SK_ACC[8]);
+//			Kfusion[9] = -SK_ACC[0]*(Kacc*P[9][0]*SK_ACC[3] + Kacc*P[9][1]*SK_ACC[2] - Kacc*P[9][3]*SK_ACC[1] + Kacc*P[9][2]*SK_ACC[4] - Kacc*P[9][4]*SK_ACC[5] + Kacc*P[9][5]*SK_ACC[8] + Kacc*P[9][6]*SK_ACC[7] + 2*Kacc*P[9][22]*SK_ACC[6] - Kacc*P[9][23]*SK_ACC[8]);
+//			Kfusion[10] = -SK_ACC[0]*(Kacc*P[10][0]*SK_ACC[3] + Kacc*P[10][1]*SK_ACC[2] - Kacc*P[10][3]*SK_ACC[1] + Kacc*P[10][2]*SK_ACC[4] - Kacc*P[10][4]*SK_ACC[5] + Kacc*P[10][5]*SK_ACC[8] + Kacc*P[10][6]*SK_ACC[7] + 2*Kacc*P[10][22]*SK_ACC[6] - Kacc*P[10][23]*SK_ACC[8]);
+//			Kfusion[11] = -SK_ACC[0]*(Kacc*P[11][0]*SK_ACC[3] + Kacc*P[11][1]*SK_ACC[2] - Kacc*P[11][3]*SK_ACC[1] + Kacc*P[11][2]*SK_ACC[4] - Kacc*P[11][4]*SK_ACC[5] + Kacc*P[11][5]*SK_ACC[8] + Kacc*P[11][6]*SK_ACC[7] + 2*Kacc*P[11][22]*SK_ACC[6] - Kacc*P[11][23]*SK_ACC[8]);
+//			Kfusion[12] = -SK_ACC[0]*(Kacc*P[12][0]*SK_ACC[3] + Kacc*P[12][1]*SK_ACC[2] - Kacc*P[12][3]*SK_ACC[1] + Kacc*P[12][2]*SK_ACC[4] - Kacc*P[12][4]*SK_ACC[5] + Kacc*P[12][5]*SK_ACC[8] + Kacc*P[12][6]*SK_ACC[7] + 2*Kacc*P[12][22]*SK_ACC[6] - Kacc*P[12][23]*SK_ACC[8]);
+//			Kfusion[13] = -SK_ACC[0]*(Kacc*P[13][0]*SK_ACC[3] + Kacc*P[13][1]*SK_ACC[2] - Kacc*P[13][3]*SK_ACC[1] + Kacc*P[13][2]*SK_ACC[4] - Kacc*P[13][4]*SK_ACC[5] + Kacc*P[13][5]*SK_ACC[8] + Kacc*P[13][6]*SK_ACC[7] + 2*Kacc*P[13][22]*SK_ACC[6] - Kacc*P[13][23]*SK_ACC[8]);
+//			Kfusion[14] = -SK_ACC[0]*(Kacc*P[14][0]*SK_ACC[3] + Kacc*P[14][1]*SK_ACC[2] - Kacc*P[14][3]*SK_ACC[1] + Kacc*P[14][2]*SK_ACC[4] - Kacc*P[14][4]*SK_ACC[5] + Kacc*P[14][5]*SK_ACC[8] + Kacc*P[14][6]*SK_ACC[7] + 2*Kacc*P[14][22]*SK_ACC[6] - Kacc*P[14][23]*SK_ACC[8]);
+//			Kfusion[15] = -SK_ACC[0]*(Kacc*P[15][0]*SK_ACC[3] + Kacc*P[15][1]*SK_ACC[2] - Kacc*P[15][3]*SK_ACC[1] + Kacc*P[15][2]*SK_ACC[4] - Kacc*P[15][4]*SK_ACC[5] + Kacc*P[15][5]*SK_ACC[8] + Kacc*P[15][6]*SK_ACC[7] + 2*Kacc*P[15][22]*SK_ACC[6] - Kacc*P[15][23]*SK_ACC[8]);
+//			Kfusion[16] = -SK_ACC[0]*(Kacc*P[16][0]*SK_ACC[3] + Kacc*P[16][1]*SK_ACC[2] - Kacc*P[16][3]*SK_ACC[1] + Kacc*P[16][2]*SK_ACC[4] - Kacc*P[16][4]*SK_ACC[5] + Kacc*P[16][5]*SK_ACC[8] + Kacc*P[16][6]*SK_ACC[7] + 2*Kacc*P[16][22]*SK_ACC[6] - Kacc*P[16][23]*SK_ACC[8]);
+//			Kfusion[17] = -SK_ACC[0]*(Kacc*P[17][0]*SK_ACC[3] + Kacc*P[17][1]*SK_ACC[2] - Kacc*P[17][3]*SK_ACC[1] + Kacc*P[17][2]*SK_ACC[4] - Kacc*P[17][4]*SK_ACC[5] + Kacc*P[17][5]*SK_ACC[8] + Kacc*P[17][6]*SK_ACC[7] + 2*Kacc*P[17][22]*SK_ACC[6] - Kacc*P[17][23]*SK_ACC[8]);
+//			Kfusion[18] = -SK_ACC[0]*(Kacc*P[18][0]*SK_ACC[3] + Kacc*P[18][1]*SK_ACC[2] - Kacc*P[18][3]*SK_ACC[1] + Kacc*P[18][2]*SK_ACC[4] - Kacc*P[18][4]*SK_ACC[5] + Kacc*P[18][5]*SK_ACC[8] + Kacc*P[18][6]*SK_ACC[7] + 2*Kacc*P[18][22]*SK_ACC[6] - Kacc*P[18][23]*SK_ACC[8]);
+//			Kfusion[19] = -SK_ACC[0]*(Kacc*P[19][0]*SK_ACC[3] + Kacc*P[19][1]*SK_ACC[2] - Kacc*P[19][3]*SK_ACC[1] + Kacc*P[19][2]*SK_ACC[4] - Kacc*P[19][4]*SK_ACC[5] + Kacc*P[19][5]*SK_ACC[8] + Kacc*P[19][6]*SK_ACC[7] + 2*Kacc*P[19][22]*SK_ACC[6] - Kacc*P[19][23]*SK_ACC[8]);
+//			Kfusion[20] = -SK_ACC[0]*(Kacc*P[20][0]*SK_ACC[3] + Kacc*P[20][1]*SK_ACC[2] - Kacc*P[20][3]*SK_ACC[1] + Kacc*P[20][2]*SK_ACC[4] - Kacc*P[20][4]*SK_ACC[5] + Kacc*P[20][5]*SK_ACC[8] + Kacc*P[20][6]*SK_ACC[7] + 2*Kacc*P[20][22]*SK_ACC[6] - Kacc*P[20][23]*SK_ACC[8]);
+//			Kfusion[21] = -SK_ACC[0]*(Kacc*P[21][0]*SK_ACC[3] + Kacc*P[21][1]*SK_ACC[2] - Kacc*P[21][3]*SK_ACC[1] + Kacc*P[21][2]*SK_ACC[4] - Kacc*P[21][4]*SK_ACC[5] + Kacc*P[21][5]*SK_ACC[8] + Kacc*P[21][6]*SK_ACC[7] + 2*Kacc*P[21][22]*SK_ACC[6] - Kacc*P[21][23]*SK_ACC[8]);
+			Kfusion[22] = -SK_ACC[0]*(Kacc*P[22][0]*SK_ACC[3] + Kacc*P[22][1]*SK_ACC[2] - Kacc*P[22][3]*SK_ACC[1] + Kacc*P[22][2]*SK_ACC[4] - Kacc*P[22][4]*SK_ACC[5] + Kacc*P[22][5]*SK_ACC[8] + Kacc*P[22][6]*SK_ACC[7] + 2*Kacc*P[22][22]*SK_ACC[6] - Kacc*P[22][23]*SK_ACC[8]);
+			Kfusion[23] = -SK_ACC[0]*(Kacc*P[23][0]*SK_ACC[3] + Kacc*P[23][1]*SK_ACC[2] - Kacc*P[23][3]*SK_ACC[1] + Kacc*P[23][2]*SK_ACC[4] - Kacc*P[23][4]*SK_ACC[5] + Kacc*P[23][5]*SK_ACC[8] + Kacc*P[23][6]*SK_ACC[7] + 2*Kacc*P[23][22]*SK_ACC[6] - Kacc*P[23][23]*SK_ACC[8]);
+
+			// calculate the predicted acceleration and innovation measured along the Y body axis
+			float drag_sign;
+			if (rel_wind(axis_index) >= 0.0f) {
+			    drag_sign = 1.0f;
+			} else {
+			    drag_sign = -1.0f;
+			}
+			float predAccel = -BC_inv_y * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
+			_drag_innov[axis_index] = predAccel - mea_acc;
+			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
+
+		}
+
+		// if the innovation consistency check fails then don't fuse the sample
+		if (_drag_test_ratio[axis_index] <= 1.0f) {
+			// apply covariance correction via P_new = (I -K*H)*P
+			// first calculate expression for KHP
+			// then calculate P - KHP
+			float KHP[_k_num_states][_k_num_states];
+			float KH[9];
+			for (unsigned row = 0; row < _k_num_states; row++) {
+
+				KH[0] = Kfusion[row] * H_ACC[0];
+				KH[1] = Kfusion[row] * H_ACC[1];
+				KH[2] = Kfusion[row] * H_ACC[2];
+				KH[3] = Kfusion[row] * H_ACC[3];
+				KH[4] = Kfusion[row] * H_ACC[4];
+				KH[5] = Kfusion[row] * H_ACC[5];
+				KH[6] = Kfusion[row] * H_ACC[6];
+				KH[7] = Kfusion[row] * H_ACC[22];
+				KH[8] = Kfusion[row] * H_ACC[23];
+
+				for (unsigned column = 0; column < _k_num_states; column++) {
+					float tmp = KH[0] * P[0][column];
+					tmp += KH[1] * P[1][column];
+					tmp += KH[2] * P[2][column];
+					tmp += KH[3] * P[3][column];
+					tmp += KH[4] * P[4][column];
+					tmp += KH[5] * P[5][column];
+					tmp += KH[6] * P[6][column];
+					tmp += KH[7] * P[22][column];
+					tmp += KH[8] * P[23][column];
+					KHP[row][column] = tmp;
+				}
+			}
+
+			// if the covariance correction will result in a negative variance, then
+			// the covariance marix is unhealthy and must be corrected
+			bool healthy = true;
+			//_fault_status.flags.bad_sideslip = false;
+			for (int i = 0; i < _k_num_states; i++) {
+				if (P[i][i] < KHP[i][i]) {
+					// zero rows and columns
+					zeroRows(P,i,i);
+					zeroCols(P,i,i);
+
+					//flag as unhealthy
+					healthy = false;
+
+					// update individual measurement health status
+					//_fault_status.flags.bad_sideslip = true;
+
+				}
+			}
+
+			// only apply covariance and state corrrections if healthy
+			if (healthy) {
+				// apply the covariance corrections
+				for (unsigned row = 0; row < _k_num_states; row++) {
+					for (unsigned column = 0; column < _k_num_states; column++) {
+						P[row][column] = P[row][column] - KHP[row][column];
+					}
+				}
+
+				// correct the covariance marix for gross errors
+				fixCovarianceErrors();
+
+				// apply the state corrections
+				fuse(Kfusion, _drag_innov[axis_index]);
+
+			}
+		}
+	}
+}
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index de965c7716..baf6ca46f1 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -150,6 +150,8 @@ Ekf::Ekf():
 	_gps_check_fail_status.value = 0;
 	_state_reset_status = {};
 	_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
+	memset(_drag_innov, 0, sizeof(_drag_innov));
+	memset(_drag_innov_var, 0, sizeof(_drag_innov_var));
 }
 
 bool Ekf::init(uint64_t timestamp)
diff --git a/EKF/ekf.h b/EKF/ekf.h
index f14a34ef45..5cb0a4332b 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -94,6 +94,12 @@ public:
 	// gets the innovation of the flow measurement
 	void get_flow_innov(float flow_innov[2]);
 
+	// gets the innovation variance of the drag specific force measurement
+	void get_drag_innov_var(float drag_innov_var[2]);
+
+	// gets the innovation of the drag specific force measurement
+	void get_drag_innov(float drag_innov[2]);
+
 	// gets the innovation variance of the HAGL measurement
 	void get_hagl_innov_var(float *hagl_innov_var);
 
@@ -272,6 +278,9 @@ private:
 	float _beta_innov;		// synthetic sideslip measurement innovation
  	float _beta_innov_var;	// synthetic sideslip measurement innovation variance
 
+	float _drag_innov[2];		// multirotor drag measurement innovation
+	float _drag_innov_var[2];	// multirotor drag measurement innovation variance
+
 	float _heading_innov;		// heading measurement innovation
 	float _heading_innov_var;	// heading measurement innovation variance
 
@@ -383,6 +392,9 @@ private:
 	// fuse synthetic zero sideslip measurement
  	void fuseSideslip();
 
+	// fuse body frame drag specific forces for multi-rotor wind estimation
+	void fuseDrag();
+
 	// fuse velocity and position measurements (also barometer height)
 	void fuseVelPosHeight();
 
@@ -464,6 +476,9 @@ private:
 	// control fusion of synthetic sideslip observations
 	void controlBetaFusion();
 
+	// control fusion of multi-rotor drag specific force observations
+	void controlDragFusion();
+
 	// control fusion of pressure altitude observations
 	void controlBaroFusion();
 
diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index da1a370135..1eb37e809f 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -75,6 +75,7 @@ EstimatorInterface::EstimatorInterface():
 	_tas_test_ratio(0.0f),
 	_terr_test_ratio(0.0f),
 	_beta_test_ratio(0.0f),
+	_drag_test_ratio{},
 	_is_dead_reckoning(false),
 	_vibe_metrics{},
 	_time_last_imu(0),
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index ee4cdc848e..9f373a07ab 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -107,6 +107,12 @@ public:
 	// gets the innovation of the flow measurement
 	virtual void get_flow_innov(float flow_innov[2]) = 0;
 
+	// gets the innovation variance of the drag specific force measurement
+	virtual void get_drag_innov_var(float drag_innov_var[2]) = 0;
+
+	// gets the innovation of the drag specific force measurement
+	virtual void get_drag_innov(float drag_innov[2]) = 0;
+
 	// gets the innovation variance of the HAGL measurement
 	virtual void get_hagl_innov_var(float *flow_innov_var) = 0;
 
@@ -339,6 +345,7 @@ protected:
 	float _tas_test_ratio;		// tas innovation consistency check ratio
 	float _terr_test_ratio;		// height above terrain measurement innovation consistency check ratio
 	float _beta_test_ratio;		// sideslip innovation consistency check ratio
+	float _drag_test_ratio[2];	// drag innovation cinsistency check ratio
 	innovation_fault_status_u _innov_check_fail_status{};
 
 	bool _is_dead_reckoning;	// true if we are no longer fusing measurements that constrain horizontal velocity drift
diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp
index d8d599262d..16b03d2e94 100644
--- a/EKF/optflow_fusion.cpp
+++ b/EKF/optflow_fusion.cpp
@@ -510,6 +510,17 @@ void Ekf::get_flow_innov_var(float flow_innov_var[2])
 	memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
 }
 
+void Ekf::get_drag_innov(float drag_innov[2])
+{
+	memcpy(drag_innov, _drag_innov, sizeof(_drag_innov));
+}
+
+
+void Ekf::get_drag_innov_var(float drag_innov_var[2])
+{
+	memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var));
+}
+
 // calculate optical flow gyro bias errors
 void Ekf::calcOptFlowBias()
 {