diff --git a/EKF/common.h b/EKF/common.h
index 861e6294a5..c96bb67482 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -155,8 +155,8 @@ struct parameters {
 	float req_gdop;         // maximum acceptable geometric dilution of precision
 	float req_hdrift;       // maximum acceptable horizontal drift speed
 	float req_vdrift;       // maximum acceptable vertical drift speed
-	
-    // Initialize parameter values.  Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
+
+	// Initialize parameter values.  Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
 	parameters()
 	{
 		mag_delay_ms = 0.0f;
diff --git a/EKF/control.cpp b/EKF/control.cpp
index d010b77552..0b482fa325 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -57,8 +57,10 @@ void Ekf::controlFusionModes()
 	if (!_control_status.flags.gps) {
 		if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
 		    && (_time_last_imu - _last_gps_fail_us > 5e6)) {
-			// Reset the yaw and magnetic field states
-			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+			// If the heading is not aligned, reset the yaw and magnetic field states
+			if (!_control_status.flags.yaw_align) {
+				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+			}
 
 			// If the heading is valid, reset the positon and velocity and start using gps aiding
 			if (_control_status.flags.yaw_align) {
@@ -110,6 +112,11 @@ void Ekf::controlFusionModes()
 
 		} else {
 			if (_control_status.flags.in_air) {
+				// if transitioning from a non-3D fusion mode, we need to initialise the yaw angle and field states
+				if (!_control_status.flags.mag_3D) {
+					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+				}
+
 				// always use 3D mag fusion when airborne
 				_control_status.flags.mag_hdg = false;
 				_control_status.flags.mag_2D = false;
@@ -143,6 +150,11 @@ void Ekf::controlFusionModes()
 		_control_status.flags.mag_3D = false;
 
 	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
+		// if transitioning from a non-3D fusion mode, we need to initialise the yaw angle and field states
+		if (!_control_status.flags.mag_3D) {
+			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+		}
+
 		// always use 3-axis mag fusion
 		_control_status.flags.mag_hdg = false;
 		_control_status.flags.mag_2D = false;
diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index c5dbf51512..59a87db520 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -52,34 +52,37 @@ void Ekf::initialiseCovariance()
 		}
 	}
 
+	// calculate average prediction time step in sec
+	float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS;
+
 	// XXX use initial guess for the diagonal elements for the covariance matrix
 	// angle error
-	P[0][0] = 0.001f;
-	P[1][1] = 0.001f;
-	P[2][2] = 0.001f;
+	P[0][0] = 0.1f;
+	P[1][1] = 0.1f;
+	P[2][2] = 0.1f;
 
 	// velocity
-	P[3][3] = 0.1f;
-	P[4][4] = 0.1f;
-	P[5][5] = 0.1f;
+	P[3][3] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
+	P[4][4] = P[3][3];
+	P[5][5] = sq(1.5f) * P[3][3];
 
 	// position
-	P[6][6] = 0.1f;
-	P[7][7] = 0.1f;
-	P[8][8] = 0.1f;
+	P[6][6] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
+	P[7][7] = P[6][6];
+	P[8][8] = sq(fmaxf(_params.baro_noise, 0.01f));
 
 	// gyro bias
-	P[9][9] = 0.00001f;
-	P[10][10] = 0.00001f;
-	P[11][11] = 0.00001f;
+	P[9][9] = sq(0.035f * dt);
+	P[10][10] = P[9][9];
+	P[11][11] = P[9][9];
 
 	// gyro scale
-	P[12][12] = 0.0001f;
-	P[13][13] = 0.0001f;
-	P[14][14] = 0.0001f;
+	P[12][12] = sq(0.001f);
+	P[13][13] = P[12][12];
+	P[14][14] = P[12][12];
 
 	// accel z bias
-	P[15][15] = 0.0001f;
+	P[15][15] = sq(0.2f * dt);
 
 	// variances for optional states
 	// these state variances are set to zero until the states are required, then they must be initialised
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index e8eb696a0e..578dd20669 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -272,7 +272,7 @@ bool Ekf::initialiseFilter(void)
 		if (_delVel_sum.norm() > 0.001f) {
 			_delVel_sum.normalize();
 			pitch = asinf(_delVel_sum(0));
-			roll = -asinf(_delVel_sum(1) / cosf(pitch));
+			roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
 
 		} else {
 			return false;
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index e055021dff..af75b87b4a 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -117,6 +117,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	_state.quat_nominal = Quaternion(euler_init);
 	_output_new.quat_nominal = _state.quat_nominal;
 
+	// reset the angle error variances because the yaw angle could have changed by a significant amount
+	// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
+	// will grow them again.
+	zeroRows(P, 0, 2);
+	zeroCols(P, 0, 2);
+
 	// calculate initial earth magnetic field states
 	matrix::Dcm<float> R_to_earth(euler_init);
 	_state.mag_I = R_to_earth * mag_init;
diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 32dbc5c5e6..d73869d00e 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -68,9 +68,9 @@ EstimatorInterface::EstimatorInterface():
 	_mag_declination_gps(0.0f),
 	_mag_declination_to_save_deg(0.0f)
 {
-    _pos_ref = {};
-    memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
-    memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
+	_pos_ref = {};
+	memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
+	memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
 }
 
 EstimatorInterface::~EstimatorInterface()
@@ -202,7 +202,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data)
 	if (time_usec > _time_last_airspeed) {
 		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 = time_usec - _params.airspeed_delay_ms * 1000;
 		airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
 		_time_last_airspeed = time_usec;
 
diff --git a/EKF/geo.cpp b/EKF/geo.cpp
index d422d20817..55cd5ca47f 100644
--- a/EKF/geo.cpp
+++ b/EKF/geo.cpp
@@ -211,13 +211,13 @@ uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
 }
 
 int map_projection_global_init(double lat_0, double lon_0,
-					uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+			       uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
 	return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
 }
 
 int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
-		uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+				    uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
 
 	ref->lat_rad = lat_0 * M_DEG_TO_RAD;
@@ -237,7 +237,7 @@ int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
 }
 
 int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
-				      double *ref_lon_rad)
+			     double *ref_lon_rad)
 {
 	if (!map_projection_initialized(ref)) {
 		return -1;
@@ -256,7 +256,7 @@ int map_projection_global_project(double lat, double lon, float *x, float *y)
 }
 
 int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
-				    float *y)
+			   float *y)
 {
 	if (!map_projection_initialized(ref)) {
 		return -1;
@@ -293,7 +293,7 @@ int map_projection_global_reproject(float x, float y, double *lat, double *lon)
 }
 
 int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
-				      double *lon)
+			     double *lon)
 {
 	if (!map_projection_initialized(ref)) {
 		return -1;
@@ -419,7 +419,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n
 }
 
 void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
-		double *lat_target, double *lon_target)
+					double *lat_target, double *lon_target)
 {
 	if (fabsf(dist) < FLT_EPSILON) {
 		*lat_target = lat_A;
@@ -437,7 +437,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
 }
 
 void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
-		double *lat_target, double *lon_target)
+					double *lat_target, double *lon_target)
 {
 	bearing = _wrap_2pi(bearing);
 	double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
@@ -472,7 +472,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
 }
 
 void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
-		float *v_e)
+				 float *v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -488,7 +488,7 @@ void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next
 }
 
 void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next,
-		float *v_n, float *v_e)
+				      float *v_n, float *v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -504,7 +504,7 @@ void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat
 }
 
 void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
-		double *lon_res)
+				   double *lon_res)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -516,7 +516,7 @@ void add_vector_to_global_position(double lat_now, double lon_now, float v_n, fl
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
 
 int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
-				  double lat_start, double lon_start, double lat_end, double lon_end)
+			 double lat_start, double lon_start, double lat_end, double lon_end)
 {
 // This function returns the distance to the nearest point on the track line.  Distance is positive if current
 // position is right of the track and negative if left of the track as seen from a point on the track line
@@ -568,8 +568,8 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
 
 
 int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
-				 double lat_center, double lon_center,
-				 float radius, float arc_start_bearing, float arc_sweep)
+			double lat_center, double lon_center,
+			float radius, float arc_start_bearing, float arc_sweep)
 {
 	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
 	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
diff --git a/EKF/geo.h b/EKF/geo.h
index e20c440583..bc33d23b94 100644
--- a/EKF/geo.h
+++ b/EKF/geo.h
@@ -57,7 +57,7 @@
 #define M_PI_2_F  1.57079632679489661923f
 #define M_RAD_TO_DEG 0.01745329251994329576f
 #define M_DEG_TO_RAD 57.29577951308232087679f
-#define OK 0 
+#define OK 0
 #define ERROR -1
 // XXX remove
 struct crosstrack_error_s {
@@ -116,7 +116,7 @@ int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
  * @return 0 if map_projection_init was called before, -1 else
  */
 int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
-				      double *ref_lon_rad);
+			     double *ref_lon_rad);
 
 
 /**
@@ -128,7 +128,7 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
 int map_projection_init_timestamped(struct map_projection_reference_s *ref,
-		double lat_0, double lon_0, uint64_t timestamp);
+				    double lat_0, double lon_0, uint64_t timestamp);
 
 /**
  * Initializes the map transformation given by the argument and sets the timestamp to now.
@@ -161,7 +161,7 @@ int map_projection_global_project(double lat, double lon, float *x, float *y);
 * @return 0 if map_projection_init was called before, -1 else
 */
 int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
-				    float *y);
+			   float *y);
 
 /**
  * Transforms a point in the local azimuthal equidistant plane to the
@@ -186,7 +186,7 @@ int map_projection_global_reproject(float x, float y, double *lat, double *lon);
  * @return 0 if map_projection_init was called before, -1 else
  */
 int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
-				      double *lon);
+			     double *lon);
 
 /**
  * Get reference position of the global map projection
@@ -243,7 +243,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n
  * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
  */
 void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
-		double *lat_target, double *lon_target);
+					double *lat_target, double *lon_target);
 
 /**
  * Creates a waypoint from given waypoint, distance and bearing
@@ -257,7 +257,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
  * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
  */
 void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
-		double *lat_target, double *lon_target);
+					double *lat_target, double *lon_target);
 
 /**
  * Returns the bearing to the next waypoint in radians.
@@ -270,20 +270,20 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa
 float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
-		float *v_e);
+				 float *v_e);
 
 void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next,
-		float *v_n, float *v_e);
+				      float *v_n, float *v_e);
 
 void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
-		double *lon_res);
+				   double *lon_res);
 
 int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
-				  double lat_start, double lon_start, double lat_end, double lon_end);
+			 double lat_start, double lon_start, double lat_end, double lon_end);
 
 int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
-				 double lat_center, double lon_center,
-				 float radius, float arc_start_bearing, float arc_sweep);
+			double lat_center, double lon_center,
+			float radius, float arc_start_bearing, float arc_sweep);
 
 /*
  * Calculate distance in global frame
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index c31ad0cf58..857d1c019c 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -610,9 +610,11 @@ void Ekf::fuseHeading()
 
 	// TODO - enable use of an off-board heading measurement
 
-	// rotate the magnetometer measurement into earth frame using an assumed zero yaw angle
+	// rotate the magnetometer measurement into earth frame
 	matrix::Euler<float> euler(_state.quat_nominal);
 	float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation
+
+	// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
 	euler(2) = 0.0f;
 	matrix::Dcm<float> R_to_earth(euler);
 	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
diff --git a/EKF/mathlib.h b/EKF/mathlib.h
index f20ce65111..d4e002dd58 100644
--- a/EKF/mathlib.h
+++ b/EKF/mathlib.h
@@ -45,13 +45,14 @@
 #include <algorithm>
 #define M_PI_F 3.14159265358979323846f
 
-namespace math {
-	using namespace Eigen;
-	using namespace std;
+namespace math
+{
+using namespace Eigen;
+using namespace std;
 
-	float constrain(float &val, float min, float max);
-	float radians(float degrees);
-	float degrees(float radians);
+float constrain(float &val, float min, float max);
+float radians(float degrees);
+float degrees(float radians);
 }
 #else
 #include <mathlib/mathlib.h>
diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp
index 6ec7300a89..fd54177083 100644
--- a/EKF/vel_pos_fusion.cpp
+++ b/EKF/vel_pos_fusion.cpp
@@ -43,139 +43,148 @@
 
 void Ekf::fuseVelPosHeight()
 {
-    bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available
-    bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
-    float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
-    float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
-    float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
+	bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available
+	bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
+	float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
+	float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
+	float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
 
-    // calculate innovations, innovations gate sizes and observation variances
+	// calculate innovations, innovations gate sizes and observation variances
 	if (_fuse_hor_vel) {
 		fuse_map[0] = fuse_map[1] = true;
-        // horizontal velocity innovations
+		// horizontal velocity innovations
 		_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
 		_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
-        // observation variance - use receiver reported accuracy with parameter setting the minimum value
-        R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
-        R[0] = fmaxf(R[0], _gps_speed_accuracy);
-        R[0] = R[0] * R[0];
-        R[1] = R[0];
-        // innovation gate sizes
-        gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
-        gate_size[1] = gate_size[0];
-    }
+		// observation variance - use receiver reported accuracy with parameter setting the minimum value
+		R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
+		R[0] = fmaxf(R[0], _gps_speed_accuracy);
+		R[0] = R[0] * R[0];
+		R[1] = R[0];
+		// innovation gate sizes
+		gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
+		gate_size[1] = gate_size[0];
+	}
 
 	if (_fuse_vert_vel) {
 		fuse_map[2] = true;
-        // vertical velocity innovation
-        _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
-        // observation variance - use receiver reported accuracy with parameter setting the minimum value
-        R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
-        // use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
-        R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy);
-        R[2] = R[2] * R[2];
-        // innovation gate size
-        gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
-    }
+		// vertical velocity innovation
+		_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
+		// observation variance - use receiver reported accuracy with parameter setting the minimum value
+		R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
+		// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
+		R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy);
+		R[2] = R[2] * R[2];
+		// innovation gate size
+		gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
+	}
 
 	if (_fuse_pos) {
 		fuse_map[3] = fuse_map[4] = true;
-        // horizontal position innovations
+		// horizontal position innovations
 		_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
 		_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
-        // observation variance - user parameter defined
-        // if we are in flight and not using GPS, then use a specific parameter
-        if (!_control_status.flags.gps && _control_status.flags.in_air) {
-            R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
-        } else {
-            R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
-        }
-        R[3] = R[3] * R[3];
-        R[4] = R[3];
-        // innovation gate sizes
-        gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
-        gate_size[4] = gate_size[3];
+
+		// observation variance - user parameter defined
+		// if we are in flight and not using GPS, then use a specific parameter
+		if (!_control_status.flags.gps && _control_status.flags.in_air) {
+			R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
+
+		} else {
+			R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
+		}
+
+		R[3] = R[3] * R[3];
+		R[4] = R[3];
+		// innovation gate sizes
+		gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
+		gate_size[4] = gate_size[3];
 	}
 
 	if (_fuse_height) {
 		fuse_map[5] = true;
-        // vertical position innovation - baro measurement has opposite sign to earth z axis
-        _vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment -_baro_sample_delayed.hgt);
-        // observation variance - user parameter defined
-        R[5] = fmaxf(_params.baro_noise, 0.01f);
-        R[5] = R[5] * R[5];
-        // innovation gate size
-        gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
-    }
+		// vertical position innovation - baro measurement has opposite sign to earth z axis
+		_vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment - _baro_sample_delayed.hgt);
+		// observation variance - user parameter defined
+		R[5] = fmaxf(_params.baro_noise, 0.01f);
+		R[5] = R[5] * R[5];
+		// innovation gate size
+		gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
+	}
 
-    // calculate innovation test ratios
-    for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
-        if (fuse_map[obs_index]) {
-            // compute the innovation variance SK = HPH + R
-            unsigned state_index = obs_index + 3;	// we start with vx and this is the 4. state
-            _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
-            // Compute the ratio of innovation to gate size
-            _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov[obs_index]);
-        }
-    }
+	// calculate innovation test ratios
+	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
+		if (fuse_map[obs_index]) {
+			// compute the innovation variance SK = HPH + R
+			unsigned state_index = obs_index + 3;	// we start with vx and this is the 4. state
+			_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
+			// Compute the ratio of innovation to gate size
+			_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov[obs_index]);
+		}
+	}
 
-    // check position, velocity and height innovations
-    // treat 3D velocity, 2D position and height as separate sensors
-    // always pass position checks if using synthetic position measurements
-    bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f);
-    innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
-    bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
-    bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || using_synthetic_measurements;
-    innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
-    innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
+	// check position, velocity and height innovations
+	// treat 3D velocity, 2D position and height as separate sensors
+	// always pass position checks if using synthetic position measurements
+	bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
+			      && (_vel_pos_test_ratio[2] <= 1.0f);
+	innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
+	bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
+	bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f))
+			      || using_synthetic_measurements;
+	innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
+	innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
 
-    // record the successful velocity fusion time
-    if (vel_check_pass && _fuse_hor_vel) {
-        _time_last_vel_fuse = _time_last_imu;
-    }
+	// record the successful velocity fusion time
+	if (vel_check_pass && _fuse_hor_vel) {
+		_time_last_vel_fuse = _time_last_imu;
+	}
 
-    // record the successful position fusion time
-    if (pos_check_pass && _fuse_pos) {
-        _time_last_pos_fuse = _time_last_imu;
-    }
+	// record the successful position fusion time
+	if (pos_check_pass && _fuse_pos) {
+		_time_last_pos_fuse = _time_last_imu;
+	}
 
-    // record the successful height fusion time
-    if (innov_check_pass_map[5] && _fuse_height) {
-        _time_last_hgt_fuse = _time_last_imu;
-    }
+	// record the successful height fusion time
+	if (innov_check_pass_map[5] && _fuse_height) {
+		_time_last_hgt_fuse = _time_last_imu;
+	}
 
-    for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
-        // skip fusion if not requested or checks have failed
-        if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
+	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
+		// skip fusion if not requested or checks have failed
+		if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
 			continue;
 		}
 
 		unsigned state_index = obs_index + 3;	// we start with vx and this is the 4. state
 
-        // calculate kalman gain K = PHS, where S = 1/innovation variance
-                for (int row = 0; row <= 15; row++) {
-                    Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+		// calculate kalman gain K = PHS, where S = 1/innovation variance
+		for (int row = 0; row <= 15; row++) {
+			Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+		}
+
+		// only update magnetic field states if we are fusing 3-axis observations
+		if (_control_status.flags.mag_3D) {
+			for (int row = 16; row <= 21; row++) {
+				Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+			}
+
+		} else {
+			for (int row = 16; row <= 21; row++) {
+				Kfusion[row] = 0.0f;
+			}
+		}
+
+		// only update wind states if we are doing wind estimation
+		if (_control_status.flags.wind) {
+			for (int row = 22; row <= 23; row++) {
+				Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+			}
+
+		} else {
+			for (int row = 22; row <= 23; row++) {
+				Kfusion[row] = 0.0f;
+			}
 		}
-                // only update magnetic field states if we are fusing 3-axis observations
-                if (_control_status.flags.mag_3D) {
-                    for (int row = 16; row <= 21; row++) {
-                        Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
-                    }
-                } else {
-                    for (int row = 16; row <= 21; row++) {
-                        Kfusion[row] = 0.0f;
-                    }
-                }
-                // only update wind states if we are doing wind estimation
-                if (_control_status.flags.wind) {
-                    for (int row = 22; row <= 23; row++) {
-                        Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
-                    }
-                } else {
-                    for (int row = 22; row <= 23; row++) {
-                        Kfusion[row] = 0.0f;
-                    }
-                }
 
 		// by definition the angle error state is zero at the fusion time
 		_state.ang_error.setZero();
diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp
index 8c36cf6d2c..983ec2a439 100644
--- a/validation/data_validator.cpp
+++ b/validation/data_validator.cpp
@@ -55,6 +55,7 @@ DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_M2{0.0f},
 	_rms{0.0f},
 	_value{0.0f},
+	_vibe{0.0f},
 	_value_equal_count(0),
 	_sibling(prev_sibling)
 {
@@ -112,8 +113,10 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
 			}
 		}
 
+		_vibe[i] = _vibe[i] * 0.99f + 0.01f * fabsf(val[i] - _lp[i]);
+
 		// XXX replace with better filter, make it auto-tune to update rate
-		_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
+		_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
 
 		_value[i] = val[i];
 	}
diff --git a/validation/data_validator.h b/validation/data_validator.h
index 28914454c5..59e4472983 100644
--- a/validation/data_validator.h
+++ b/validation/data_validator.h
@@ -117,6 +117,12 @@ public:
 	 */
 	float*			rms() { return _rms; }
 
+	/**
+	 * Get the vibration offset
+	 * @return		the stored vibration offset
+	 */
+	float*			vibration_offset() { return _vibe; }
+
 	/**
 	 * Print the validator value
 	 *
@@ -154,6 +160,7 @@ private:
 	float _M2[3];				/**< RMS component value */
 	float _rms[3];				/**< root mean square error */
 	float _value[3];			/**< last value */
+	float _vibe[3];				/**< vibration level, in sensor unit */
 	float _value_equal_count;		/**< equal values in a row */
 	DataValidator *_sibling;		/**< sibling in the group */
 	const unsigned NORETURN_ERRCOUNT = 10000;	/**< if the error count reaches this value, return sensor as invalid */
diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp
index 8bf060cf65..3f04180a77 100644
--- a/validation/data_validator_group.cpp
+++ b/validation/data_validator_group.cpp
@@ -197,6 +197,30 @@ DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
 	return vibe;
 }
 
+float
+DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis)
+{
+	DataValidator *next = _first;
+
+	float vibe = -1.0f;
+
+	/* find the best vibration value of a non-timed out sensor */
+	while (next != nullptr) {
+
+		if (next->confidence(timestamp) > 0.5f) {
+			float* vibration_offset = next->vibration_offset();
+
+			if (vibe < 0.0f || vibration_offset[axis] < vibe) {
+				vibe = vibration_offset[axis];
+			}
+		}
+
+		next = next->sibling();
+	}
+
+	return vibe;
+}
+
 void
 DataValidatorGroup::print()
 {
diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h
index 855c3ed4ae..dbc40f9ad9 100644
--- a/validation/data_validator_group.h
+++ b/validation/data_validator_group.h
@@ -74,6 +74,13 @@ public:
 	 */
 	float			get_vibration_factor(uint64_t timestamp);
 
+	/**
+	 * Get the vibration offset in the sensor unit
+	 *
+	 * @return		float value representing the vibration offset
+	 */
+	float			get_vibration_offset(uint64_t timestamp, int axis);
+
 	/**
 	 * Get the number of failover events
 	 *