diff --git a/CMakeLists.txt b/CMakeLists.txt
index 62f2a90f43..60f49dc144 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,6 +55,7 @@ px4_add_module(
 		EKF/vel_pos_fusion.cpp
 		EKF/mag_fusion.cpp
 		EKF/gps_checks.cpp
+		EKF/airspeed_fusion.cpp
 		EKF/optflow_fusion.cpp
 		EKF/control.cpp
 		EKF/terrain_estimator.cpp
diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt
index eee1af9037..6400204045 100644
--- a/EKF/CMakeLists.txt
+++ b/EKF/CMakeLists.txt
@@ -64,6 +64,7 @@ set(SRCS
 	vel_pos_fusion.cpp
 	mag_fusion.cpp
 	gps_checks.cpp
+	airspeed_fusion.cpp
 	control.cpp
 	geo.cpp
 	mathlib.cpp
diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp
new file mode 100644
index 0000000000..d630fe6546
--- /dev/null
+++ b/EKF/airspeed_fusion.cpp
@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   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 airspeed_fusion.cpp
+ * airspeed fusion methods.
+ *
+ * @author Carl Olsson <carlolsson.co@gmail.com>
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+#include "ekf.h"
+#include "mathlib.h"
+
+void Ekf::fuseAirspeed()
+{
+	// Initialize variables
+	float vn; // Velocity in north direction
+	float ve; // Velocity in east direction
+	float vd; // Velocity in downwards direction
+	float vwn; // Wind speed in north direction
+	float vwe; // Wind speed in east direction
+	float v_tas_pred; // Predicted measurement
+	float EAS2TAS = 1.0f; // Where can I get this from?
+	float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(EAS2TAS, 0.9f, 10.0f)); // Variance for true airspeed measurement - (m/sec)^2
+	float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian
+	float H_TAS[24] = {}; // Observation Jacobian
+	float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector
+	float Kfusion[24] = {}; // Kalman gain vector
+
+	// Copy required states to local variable names
+	vn = _state.vel(0);
+	ve = _state.vel(1);
+	vd = _state.vel(2);
+	vwn = _state.wind_vel(0);
+	vwe = _state.wind_vel(1);
+
+	// Calculate the predicted airspeed
+	v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
+
+	// Perform fusion of True Airspeed measurement
+	if (v_tas_pred > 3.0f) {
+		// Calculate the observation jacobian
+		// intermediate variable from algebraic optimisation
+		SH_TAS[0] = 1 / (sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+		SH_TAS[1] = (SH_TAS[0] * (2 * ve - 2 * vwe)) / 2.0f;
+		SH_TAS[2] = (SH_TAS[0] * (2 * vn - 2 * vwn)) / 2.0f;
+
+		for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; }
+
+		H_TAS[3] = SH_TAS[2];
+		H_TAS[4] = SH_TAS[1];
+		H_TAS[5] = vd * SH_TAS[0];
+		H_TAS[22] = -SH_TAS[2];
+		H_TAS[23] = -SH_TAS[1];
+	
+		// We don't want to update the innovation variance if the calculation is ill conditioned
+		float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2] * (P[3][3] * SH_TAS[2] + P[4][3] * SH_TAS[1] - P[22][3] * SH_TAS[2] - P[23][3] * SH_TAS[1] + P[5][3] * vd *SH_TAS[0]) + SH_TAS[1] * (P[3][4] * SH_TAS[2] + P[4][4] * SH_TAS[1] - P[22][4] * SH_TAS[2] - P[23][4] * SH_TAS[1] + P[5][4] * vd *SH_TAS[0]) - SH_TAS[2] * (P[3][22] * SH_TAS[2] + P[4][22] * SH_TAS[1] - P[22][22] * SH_TAS[2] - P[23][22] * SH_TAS[1] + P[5][22] * vd *SH_TAS[0]) - SH_TAS[1] * (P[3][23] * SH_TAS[2] + P[4][23] * SH_TAS[1] - P[22][23] * SH_TAS[2] - P[23][23] * SH_TAS[1] + P[5][23] * vd *SH_TAS[0]) + vd *SH_TAS[0] * (P[3][5] * SH_TAS[2] + P[4][5] * SH_TAS[1] - P[22][5] * SH_TAS[2] - P[23][5] * SH_TAS[1] + P[5][5] * vd *SH_TAS[0]));
+		if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
+            SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
+        } else { // Reset the estimator
+            initialiseCovariance();
+            return;
+        }
+		SK_TAS[1] = SH_TAS[1];
+
+		Kfusion[0] = SK_TAS[0]*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]);
+		Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]);
+		Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]);
+		Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]);
+		Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]);
+		Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]);
+		Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]);
+		Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]);
+		Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]);
+		Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]);
+		Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]);
+		Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]);
+		Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]);
+		Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]);
+		Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]);
+		Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]);
+		Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]);
+		Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]);
+
+		// Only update the magnetometer states if we are airborne and using 3D mag fusion
+		if(_control_status.flags.mag_3D && _control_status.flags.in_air){
+			Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]);
+			Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]);
+			Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]);
+			Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]);
+			Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]);
+			Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]);
+		}
+		else{
+			for (int i = 16; i <= 21; i++)
+			{
+				Kfusion[i] = 0.0f;
+			}
+		}
+
+
+		// calculate measurement innovation
+		_airspeed_innov = v_tas_pred - _airspeed_sample_delayed.airspeed; // This is TAS, maybe we should indicate that in some way
+
+		// Calculate the innovation variance
+		_airspeed_innov_var = 1.0f / SK_TAS[0];
+
+		// Compute the ratio of innovation to gate size
+		_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);
+
+		if (_tas_test_ratio < 1.0f)
+		{
+			// by definition the angle error state is zero at the fusion time
+			_state.ang_error.setZero();
+
+			// Fuse airspeed measurement
+			fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero?
+
+			// correct the nominal quaternion
+			Quaternion dq;
+			dq.from_axis_angle(_state.ang_error);
+			_state.quat_nominal = dq * _state.quat_nominal;
+			_state.quat_nominal.normalize();
+
+			// update covariance matrix via Pnew = (I - KH)P = P - KHP
+			float KH[_k_num_states][_k_num_states] = {};
+			float KHP[_k_num_states][_k_num_states] = {};
+
+			for (unsigned row = 0; row < _k_num_states; row++) {
+				for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that...
+					KH[row][column] = Kfusion[row] * H_TAS[column];
+				}
+			}
+
+			for (unsigned row = 0; row < _k_num_states; row++) {
+				for (unsigned column = 0; column < _k_num_states; column++) {
+					for (unsigned i = 0; i < _k_num_states; i++) { // Check if this is correct matrix multiplication!
+						KHP[row][column] += KH[row][i] * P[i][column];
+					}
+				}
+			}
+
+			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];
+				}
+			}
+		}
+
+		makeSymmetrical(); 
+		limitCov();
+	}
+	// Do we want to force and limit the covariance matrx even if v_tas_pred < X ?
+}
\ No newline at end of file
diff --git a/EKF/common.h b/EKF/common.h
index 8fd9d2652b..95ce9fb7a4 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -112,7 +112,7 @@ struct rangeSample {
 };
 
 struct airspeedSample {
-	float       airspeed;	// airspeed measurement in m/s
+	float       airspeed;	// true airspeed measurement in m/s
 	uint64_t    time_us;	// timestamp in microseconds
 };
 
@@ -195,6 +195,10 @@ struct parameters {
 	int mag_declination_source;	// bitmask used to control the handling of declination data
 	int mag_fusion_type;		// integer used to specify the type of magnetometer fusion used
 
+	// airspeed fusion
+	float tas_innov_gate;		// True Airspeed Innovation consistency gate size in standard deciation [WHAT SHALL THIS VALUE BE?]
+  	float eas_noise;			// EAS measurement noise standard deviation used for airspeed fusion [m/s]
+
 	// range finder fusion
 	float range_noise;		// observation noise for range finder measurements (m)
 	float range_innov_gate;		// range finder fusion innovation consistency gate size (STD)
@@ -265,6 +269,10 @@ struct parameters {
 		mag_declination_source = 3;
 		mag_fusion_type = 0;
 
+		// airspeed fusion
+		tas_innov_gate = 3.0f; // [CHECK THIS VALUE]		
+  		eas_noise = 1.4f;			
+
 		// range finder fusion
 		range_noise = 0.1f;
 		range_innov_gate = 5.0f;
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index cdfe4cc01b..07ccb422c2 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -57,6 +57,8 @@ Ekf::Ekf():
 	_time_last_hgt_fuse(0),
 	_time_last_of_fuse(0),
 	_last_disarmed_posD(0.0f),
+	_airspeed_innov(0.0f),
+	_airspeed_innov_var(0.0f),
 	_heading_innov(0.0f),
 	_heading_innov_var(0.0f),
 	_delta_time_of(0.0f),
@@ -303,6 +305,11 @@ bool Ekf::update()
 			_last_known_posNE(1) = _state.pos(1);
 			_fuse_flow = false;
 		}
+
+		// TODO This is just to get the logic inside but we will only start fusion once we tested this again
+		if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed) && false) {
+			fuseAirspeed();
+		}
 	}
 
 	// the output observer always runs
@@ -587,9 +594,4 @@ void Ekf::calculateOutputStates()
 	_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;
 
 	_vel_corr = (_state.pos - _output_sample_delayed.pos);
-}
-
-void Ekf::fuseAirspeed()
-{
-
-}
+}
\ No newline at end of file
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 51a01a9be2..b98f9769d3 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -73,6 +73,12 @@ public:
 	// gets the innovation variances of the earth magnetic field measurements
 	void get_mag_innov_var(float mag_innov_var[3]);
 
+	// gets the innovations of airspeed measurement
+	void get_airspeed_innov(float *airspeed_innov);
+
+	// gets the innovation variance of the airspeed measurement
+	void get_airspeed_innov_var(float *airspeed_innov_var);
+
 	// gets the innovation variance of the heading measurement
 	void get_heading_innov_var(float *heading_innov_var);
 
@@ -155,6 +161,9 @@ private:
 	float _mag_innov[3];		// earth magnetic field innovations
 	float _mag_innov_var[3];	// earth magnetic field innovation variance
 
+	float _airspeed_innov;		// airspeed measurement innovation
+	float _airspeed_innov_var;  // airspeed measurement innovation variance
+
 	float _heading_innov;		// heading measurement innovation
 	float _heading_innov_var;	// heading measurement innovation variance
 
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index f74fb7392d..371366a731 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -267,6 +267,12 @@ void Ekf::get_mag_innov(float mag_innov[3])
 	memcpy(mag_innov, _mag_innov, 3 * sizeof(float));
 }
 
+// gets the innovations of the airspeed measnurement
+void Ekf::get_airspeed_innov(float *airspeed_innov)
+{
+	memcpy(airspeed_innov,&_airspeed_innov, sizeof(float));
+}
+
 // gets the innovations of the heading measurement
 void Ekf::get_heading_innov(float *heading_innov)
 {
@@ -286,6 +292,12 @@ void Ekf::get_mag_innov_var(float mag_innov_var[3])
 	memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3);
 }
 
+// gest the innovation variance of the airspeed measurement
+void Ekf::get_airspeed_innov_var(float *airspeed_innov_var)
+{
+	memcpy(airspeed_innov_var, &_airspeed_innov_var, sizeof(float));
+}
+
 // gets the innovation variance of the heading measurement
 void Ekf::get_heading_innov_var(float *heading_innov_var)
 {
diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 373a703f80..c744b9b208 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -206,11 +206,11 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data)
 		return;
 	}
 
-	if (time_usec > _time_last_airspeed) {
+	if (time_usec - _time_last_airspeed > 80000) {
 		airspeedSample airspeed_sample_new;
 		airspeed_sample_new.airspeed = *data;
 		airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
-		airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
+		airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; //typo PeRRiod
 		_time_last_airspeed = time_usec;
 
 		_airspeed_buffer.push(airspeed_sample_new);
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index 2e5a3aca1a..17b453fb75 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -63,6 +63,9 @@ public:
 	// gets the innovations of the earth magnetic field measurements
 	virtual void get_mag_innov(float mag_innov[3]) = 0;
 
+	// gets the innovation of airspeed measurement
+ 	virtual void get_airspeed_innov(float *airspeed_innov) = 0;
+
 	// gets the innovations of the heading measurement
 	virtual void get_heading_innov(float *heading_innov) = 0;
 
@@ -73,6 +76,9 @@ public:
 	// gets the innovation variances of the earth magnetic field measurements
 	virtual void get_mag_innov_var(float mag_innov_var[3]) = 0;
 
+	// gets the innovation variance of the airspeed measurement
+ 	virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0;
+
 	// gets the innovation variance of the heading measurement
 	virtual void get_heading_innov_var(float *heading_innov_var) = 0;
 
@@ -229,8 +235,8 @@ protected:
 	bool _mag_healthy;              // computed by mag innovation test
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio
 	float _mag_test_ratio[3];       // magnetometer XYZ innovation consistency check ratios
-
 	float _vel_pos_test_ratio[6];   // velocity and position innovation consistency check ratios
+	float _tas_test_ratio;			// tas innovation consistency check ratio
 
 	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer;