ekf2: backend apply code style (generated code still exempt)

src/modules/ekf2/EKF/EKFGSF_yaw.cpp

@@ -13,7 +13,7 @@ EKFGSF_yaw::EKFGSF_yaw()
 	_ahrs_accel.zero();
 }
 
-void EKFGSF_yaw::update(const imuSample& imu_sample,
+void EKFGSF_yaw::update(const imuSample &imu_sample,
 			bool run_EKF,			// set to true when flying or movement is suitable for yaw estimation
 			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
 			const Vector3f &imu_gyro_bias)  // estimated rate gyro bias (rad/sec)
@@ -39,11 +39,13 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 		const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
 		const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
 		const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
+
 		if (ok_to_align) {
 			initialiseEKFGSF();
 			ahrsAlignTilt();
 			_ahrs_ekf_gsf_tilt_aligned = true;
 		}
+
 		return;
 	}
 
@@ -52,6 +54,7 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 
 	// AHRS prediction cycle for each model - this always runs
 	_ahrs_accel_fusion_gain = ahrsCalcAccelGain();
+
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 		predictEKF(model_index);
 	}
@@ -61,14 +64,18 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 		if (!_ekf_gsf_vel_fuse_started) {
 			initialiseEKFGSF();
 			ahrsAlignYaw();
+
 			// Initialise to gyro bias estimate from main filter because there could be a large
 			// uncorrected rate gyro bias error about the gravity vector
 			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 				_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
 			}
+
 			_ekf_gsf_vel_fuse_started = true;
+
 		} else {
 			bool bad_update = false;
+
 			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 				// subsequent measurements are fused as direct state observations
 				if (!updateEKF(model_index)) {
@@ -81,24 +88,29 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 				// calculate weighting for each model assuming a normal distribution
 				const float min_weight = 1e-5f;
 				uint8_t n_weight_clips = 0;
+
 				for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 					_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
+
 					if (_model_weights(model_index) < min_weight) {
 						n_weight_clips++;
 						_model_weights(model_index) = min_weight;
 					}
+
 					total_weight += _model_weights(model_index);
 				}
 
 				// normalise the weighting function
 				if (n_weight_clips < N_MODELS_EKFGSF) {
 					_model_weights /= total_weight;
+
 				} else {
 					// all weights have collapsed due to excessive innovation variances so reset filters
 					initialiseEKFGSF();
 				}
 			}
 		}
+
 	} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
 		// wait to fly again
 		_ekf_gsf_vel_fuse_started = false;
@@ -108,18 +120,21 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 	// To avoid issues with angle wrapping, the yaw state is converted to a vector with length
 	// equal to the weighting value before it is summed.
 	Vector2f yaw_vector;
+
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 		yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2));
 		yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2));
 	}
-	_gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0));
+
+	_gsf_yaw = atan2f(yaw_vector(1), yaw_vector(0));
 
 	// calculate a composite variance for the yaw state from a weighted average of the variance for each model
 	// models with larger innovations are weighted less
 	_gsf_yaw_variance = 0.0f;
+
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 		const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
-		_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
+		_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta);
 	}
 
 	// prevent the same velocity data being used more than once
@@ -138,6 +153,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
 	// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
 	// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
 	Vector3f tilt_correction;
+
 	if (_ahrs_accel_fusion_gain > 0.0f) {
 
 		Vector3f accel = _ahrs_accel;
@@ -158,13 +174,16 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
 	// Gyro bias estimation
 	constexpr float gyro_bias_limit = 0.05f;
 	const float spinRate = ang_rate.length();
+
 	if (spinRate < 0.175f) {
 		_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
-		_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit, gyro_bias_limit);
+		_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit,
+						       gyro_bias_limit);
 	}
 
 	// delta angle from previous to current frame
-	const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt;
+	const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) *
+					       _delta_ang_dt;
 
 	// Apply delta angle to rotation matrix
 	_ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);
@@ -182,7 +201,7 @@ void EKFGSF_yaw::ahrsAlignTilt()
 	const Vector3f down_in_bf = -_delta_vel.normalized();
 
 	// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
-	const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
+	const Vector3f i_vec_bf(1.0f, 0.0f, 0.0f);
 	Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf));
 	north_in_bf.normalize();
 
@@ -207,7 +226,7 @@ void EKFGSF_yaw::ahrsAlignYaw()
 {
 	// Align yaw angle for each model
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
-		Dcmf& R = _ahrs_ekf_gsf[model_index].R;
+		Dcmf &R = _ahrs_ekf_gsf[model_index].R;
 		const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
 		R = updateYawInRotMat(yaw, R);
 
@@ -226,10 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	}
 
 	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
-	const Dcmf& R = _ahrs_ekf_gsf[model_index].R;
+	const Dcmf &R = _ahrs_ekf_gsf[model_index].R;
 	_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
-					getEuler321Yaw(R) :
+				     getEuler321Yaw(R) :
-					getEuler312Yaw(R);
+				     getEuler312Yaw(R);
 
 	// calculate delta velocity in a horizontal front-right frame
 	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
@@ -284,8 +303,9 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 
 	// constrain variances
 	const float min_var = 1e-6f;
+
 	for (unsigned index = 0; index < 3; index++) {
-		_ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);
+		_ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var);
 	}
 }
 
@@ -374,8 +394,9 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
 
 	// constrain variances
 	const float min_var = 1e-6f;
+
 	for (unsigned index = 0; index < 3; index++) {
-		_ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);
+		_ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var);
 	}
 
 	// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
@@ -421,6 +442,7 @@ void EKFGSF_yaw::initialiseEKFGSF()
 
 	memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
 	const float yaw_increment = 2.0f * _m_pi / (float)N_MODELS_EKFGSF;
+
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
 		// evenly space initial yaw estimates in the region between +-Pi
 		_ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
@@ -428,11 +450,11 @@ void EKFGSF_yaw::initialiseEKFGSF()
 		// take velocity states and corresponding variance from last measurement
 		_ekf_gsf[model_index].X(0) = _vel_NE(0);
 		_ekf_gsf[model_index].X(1) = _vel_NE(1);
-		_ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy);
+		_ekf_gsf[model_index].P(0, 0) = sq(_vel_accuracy);
-		_ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0);
+		_ekf_gsf[model_index].P(1, 1) = _ekf_gsf[model_index].P(0, 0);
 
 		// use half yaw interval for yaw uncertainty
-		_ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment);
+		_ekf_gsf[model_index].P(2, 2) = sq(0.5f * yaw_increment);
 	}
 }
 
@@ -444,19 +466,23 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
 	return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
 }
 
-bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
+bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
+			    float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
 {
 	if (_ekf_gsf_vel_fuse_started) {
 		*yaw_composite = _gsf_yaw;
 		*yaw_variance = _gsf_yaw_variance;
+
 		for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
 			yaw[model_index] = _ekf_gsf[model_index].X(2);
 			innov_VN[model_index] = _ekf_gsf[model_index].innov(0);
 			innov_VE[model_index] = _ekf_gsf[model_index].innov(1);
 			weight[model_index] = _model_weights(model_index);
 		}
+
 		return true;
 	}
+
 	return false;
 }
 
@@ -496,23 +522,25 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
 	// Renormalise rows
 	for (uint8_t r = 0; r < 3; r++) {
 		const float rowLengthSq = ret.row(r).norm_squared();
+
 		if (rowLengthSq > FLT_EPSILON) {
 			// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
 			const float rowLengthInv = 1.5f - 0.5f * rowLengthSq;
 			ret.row(r) *=  rowLengthInv;
 		}
-        }
+	}
 
 	return ret;
 }
 
 bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance) const
 {
-	if(_ekf_gsf_vel_fuse_started) {
+	if (_ekf_gsf_vel_fuse_started) {
 		*yaw = _gsf_yaw;
 		*yaw_variance = _gsf_yaw_variance;
 		return true;
 	}
+
 	return false;
 }
 

src/modules/ekf2/EKF/EKFGSF_yaw.h

@@ -29,16 +29,16 @@ using namespace estimator;
 class EKFGSF_yaw
 {
 public:
-    	EKFGSF_yaw();
+	EKFGSF_yaw();
 
-    	// Update Filter States - this should be called whenever new IMU data is available
+	// Update Filter States - this should be called whenever new IMU data is available
 	void update(const imuSample &imu_sample,
-			bool run_EKF,  			// set to true when flying or movement is suitable for yaw estimation
+		    bool run_EKF,  			// set to true when flying or movement is suitable for yaw estimation
-			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
+		    float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
-			const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
+		    const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
 
 	void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
-			float accuracy);	   // 1-sigma accuracy of velocity measurement (m/s)
+			 float accuracy);	   // 1-sigma accuracy of velocity measurement (m/s)
 
 	// get solution data for logging
 	bool getLogData(float *yaw_composite,
@@ -48,9 +48,9 @@ public:
 			float innov_VE[N_MODELS_EKFGSF],
 			float weight[N_MODELS_EKFGSF]) const;
 
-    	// get yaw estimate and the corresponding variance
+	// get yaw estimate and the corresponding variance
-    	// return false if no yaw estimate available
+	// return false if no yaw estimate available
-    	bool getYawData(float *yaw, float *yaw_variance) const;
+	bool getYawData(float *yaw, float *yaw_variance) const;
 
 private:
 
@@ -68,14 +68,14 @@ private:
 	float _delta_vel_dt{};	// _delta_vel integration time interval (sec)
 	float _true_airspeed{};	// true airspeed used for centripetal accel compensation (m/s)
 
-	struct _ahrs_ekf_gsf_struct{
+	struct _ahrs_ekf_gsf_struct {
 		Dcmf R;			// matrix that rotates a vector from body to earth frame
 		Vector3f gyro_bias;	// gyro bias learned and used by the quaternion calculation
 		bool aligned;		// true when AHRS has been aligned
 		float vel_NE[2];	// NE velocity vector from last GPS measurement (m/s)
 		bool fuse_gps;		// true when GPS should be fused on that frame
 		float accel_dt;		// time step used when generating _simple_accel_FR data (sec)
-	} _ahrs_ekf_gsf[N_MODELS_EKFGSF]{};
+	} _ahrs_ekf_gsf[N_MODELS_EKFGSF] {};
 
 	bool _ahrs_ekf_gsf_tilt_aligned{};	// true the initial tilt alignment has been calculated
 	float _ahrs_accel_fusion_gain{};	// gain from accel vector tilt error to rate gyro correction used by AHRS calculation
@@ -99,13 +99,13 @@ private:
 
 	// Declarations used by a bank of N_MODELS_EKFGSF EKFs
 
-	struct _ekf_gsf_struct{
+	struct _ekf_gsf_struct {
 		matrix::Vector3f X; 				// Vel North (m/s),  Vel East (m/s), yaw (rad)s
 		matrix::SquareMatrix<float, 3> P; 		// covariance matrix
 		matrix::SquareMatrix<float, 2> S_inverse;	// inverse of the innovation covariance matrix
 		float S_det_inverse; 				// inverse of the innovation covariance matrix determinant
 		matrix::Vector2f innov; 			// Velocity N,E innovation (m/s)
-	} _ekf_gsf[N_MODELS_EKFGSF]{};
+	} _ekf_gsf[N_MODELS_EKFGSF] {};
 
 	bool _vel_data_updated{};	// true when velocity data has been updated
 	bool _run_ekf_gsf{};		// true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -55,7 +55,7 @@ void Ekf::fuseAirspeed()
 
 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
-			    math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+			       math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
 
 	// determine if we need the sideslip fusion to correct states other than wind
 	const bool update_wind_only = !_is_wind_dead_reckoning;
@@ -65,11 +65,13 @@ void Ekf::fuseAirspeed()
 	const float HK1 = ve - vwe;
 	const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
 	const float v_tas_pred = sqrtf(HK2); // predicted airspeed
+
 	//const float HK3 = powf(HK2, -1.0F/2.0F);
 	if (v_tas_pred < 1.0f) {
 		// calculation can be badly conditioned for very low airspeed values so don't fuse this time
 		return;
 	}
+
 	const float HK3 = 1.0f / v_tas_pred;
 	const float HK4 = HK0*HK3;
 	const float HK5 = HK1*HK3;
@@ -86,11 +88,13 @@ void Ekf::fuseAirspeed()
 	//const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
 
 	// innovation variance - check for badly conditioned calculation
-	_airspeed_innov_var = (-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
+	_airspeed_innov_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS);
+
 	if (_airspeed_innov_var < R_TAS) { //
 		// Reset the estimator covariance matrix
 		// if we are getting aiding from other sources, warn and reset the wind states and covariances only
-		const char* action_string = nullptr;
+		const char *action_string = nullptr;
+
 		if (update_wind_only) {
 			resetWindStates();
 			resetWindCovariance();
@@ -101,12 +105,14 @@ void Ekf::fuseAirspeed()
 			_state.wind_vel.setZero();
 			action_string = "full";
 		}
+
 		ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);
 
 		_fault_status.flags.bad_airspeed = true;
 
 		return;
 	}
+
 	const float HK16 = HK3 / _airspeed_innov_var;
 	_fault_status.flags.bad_airspeed = false;
 
@@ -119,6 +125,7 @@ void Ekf::fuseAirspeed()
 	Hfusion.at<23>() = -HK5;
 
 	Vector24f Kfusion; // Kalman gain vector
+
 	if (!update_wind_only) {
 		// we have no other source of aiding, so use airspeed measurements to correct states
 		for (unsigned row = 0; row <= 3; row++) {
@@ -164,7 +171,8 @@ void Ekf::fuseAirspeed()
 
 void Ekf::get_true_airspeed(float *tas) const
 {
-	const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
+	const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(
+					    _state.vel(2)));
 	memcpy(tas, &tempvar, sizeof(float));
 }
 
@@ -174,8 +182,8 @@ void Ekf::get_true_airspeed(float *tas) const
 void Ekf::resetWindStates()
 {
 	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
-	                        ? getEuler321Yaw(_state.quat_nominal)
+				? getEuler321Yaw(_state.quat_nominal)
-	                        : getEuler312Yaw(_state.quat_nominal);
+				: getEuler312Yaw(_state.quat_nominal);
 
 	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
 		// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available

src/modules/ekf2/EKF/common.h

@@ -99,7 +99,7 @@ struct imuSample {
 	Vector3f    delta_vel;		///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
 	float       delta_ang_dt;	///< delta angle integration period (sec)
 	float       delta_vel_dt;	///< delta velocity integration period (sec)
-	bool        delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping
+	bool        delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping
 };
 
 struct gpsSample {
@@ -300,8 +300,8 @@ struct parameters {
 	float range_cos_max_tilt{0.7071f};	///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 
 	// vision position fusion
-    float ev_vel_innov_gate{3.0f};		///< vision velocity fusion innovation consistency gate size (STD)
+	float ev_vel_innov_gate{3.0f};		///< vision velocity fusion innovation consistency gate size (STD)
-    float ev_pos_innov_gate{5.0f};		///< vision position fusion innovation consistency gate size (STD)
+	float ev_pos_innov_gate{5.0f};		///< vision position fusion innovation consistency gate size (STD)
 
 	// optical flow fusion
 	float flow_noise{0.15f};		///< observation noise for optical flow LOS rate measurements (rad/sec)
@@ -487,7 +487,7 @@ union filter_control_status_u {
 	uint32_t value;
 };
 
- // Mavlink bitmask containing state of estimator solution
+// Mavlink bitmask containing state of estimator solution
 union ekf_solution_status {
 	struct {
 		uint16_t attitude           : 1; ///< 0 - True if the attitude estimate is good

src/modules/ekf2/EKF/control.cpp

@@ -59,7 +59,8 @@ void Ekf::controlFusionModes()
 			_control_status.flags.tilt_align = true;
 
 			// send alignment status message to the console
-			const char* height_source = nullptr;
+			const char *height_source = nullptr;
+
 			if (_control_status.flags.baro_hgt) {
 				height_source = "baro";
 
@@ -76,9 +77,10 @@ void Ekf::controlFusionModes()
 				height_source = "unknown";
 
 			}
-			if (height_source){
+
+			if (height_source) {
 				ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
-					(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
+					 (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
 			}
 		}
 	}
@@ -100,7 +102,9 @@ void Ekf::controlFusionModes()
 
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
-		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))) {
+		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised
+				|| PX4_ISFINITE(_mag_declination_gps))) {
+
 			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
 			_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
 			_control_status.flags.synthetic_mag_z = true;
@@ -120,16 +124,16 @@ void Ekf::controlFusionModes()
 	}
 
 	{
-	// Get range data from buffer and check validity
+		// Get range data from buffer and check validity
-	const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+		const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
-	_range_sensor.setDataReadiness(is_rng_data_ready);
+		_range_sensor.setDataReadiness(is_rng_data_ready);
 
-	// update range sensor angle parameters in case they have changed
+		// update range sensor angle parameters in case they have changed
-	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
+		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
-	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 
-	_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
+		_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
 	}
 
 	if (_range_sensor.isDataHealthy()) {
@@ -195,7 +199,9 @@ void Ekf::controlExternalVisionFusion()
 
 		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
 		// needs to be calculated and the observations rotated into the EKF frame of reference
-		if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
+		if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS)
+				|| (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
+
 			// rotate EV measurements into the EKF Navigation frame
 			calcExtVisRotMat();
 		}
@@ -218,7 +224,9 @@ void Ekf::controlExternalVisionFusion()
 		}
 
 		// external vision yaw aiding selection logic
-		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
+		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
+		    && _control_status.flags.tilt_align) {
+
 			// don't start using EV data unless data is arriving frequently
 			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 				if (resetYawToEv()) {
@@ -242,6 +250,7 @@ void Ekf::controlExternalVisionFusion()
 			// Use an incremental position fusion method for EV position data if GPS is also used
 			if (_params.fusion_mode & MASK_USE_GPS) {
 				_fuse_hpos_as_odom = true;
+
 			} else {
 				_fuse_hpos_as_odom = false;
 			}
@@ -277,10 +286,12 @@ void Ekf::controlExternalVisionFusion()
 				// use the absolute position
 				Vector3f ev_pos_meas = _ev_sample_delayed.pos;
 				Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
+
 				if (_params.fusion_mode & MASK_ROTATE_EV) {
 					ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
 					ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
 				}
+
 				_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
 				_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
 
@@ -326,7 +337,7 @@ void Ekf::controlExternalVisionFusion()
 
 			ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));
 
-			fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,_last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
+			fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
 			fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
 		}
 
@@ -366,8 +377,7 @@ void Ekf::controlOpticalFlowFusion()
 
 		const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f);
 		const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
-		const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min
+		const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
-		                                && _flow_sample_delayed.dt <= delta_time_max;
 		const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
 
 		if (is_quality_good
@@ -405,19 +415,19 @@ void Ekf::controlOpticalFlowFusion()
 
 		// Check if we are in-air and require optical flow to control position drift
 		const bool is_flow_required = _control_status.flags.in_air
-		                              && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
+					      && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
-		                                  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
+						  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
-		                                  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
+						  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
 
 
 		// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
 		const bool preflight_motion_not_ok = !_control_status.flags.in_air
-		                                     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
+						     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
-		                                         || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
+								     || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
 		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
 
 		_inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
-		                    || !_control_status.flags.tilt_align;
+				    || !_control_status.flags.tilt_align;
 
 		// Handle cases where we are using optical flow but we should not use it anymore
 		if (_control_status.flags.opt_flow) {
@@ -431,9 +441,8 @@ void Ekf::controlOpticalFlowFusion()
 
 		// optical flow fusion mode selection logic
 		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
-			&& !_control_status.flags.opt_flow // we are not yet using flow data
+		    && !_control_status.flags.opt_flow // we are not yet using flow data
-			&& !_inhibit_flow_use)
+		    && !_inhibit_flow_use) {
-		{
 			// If the heading is valid and use is not inhibited , start using optical flow aiding
 			if (_control_status.flags.yaw_align) {
 				// set the flag and reset the fusion timeout
@@ -472,7 +481,9 @@ void Ekf::controlOpticalFlowFusion()
 			}
 		}
 
-	} else if (_control_status.flags.opt_flow && (_imu_sample_delayed.time_us >  _flow_sample_delayed.time_us + (uint64_t)10e6)) {
+	} else if (_control_status.flags.opt_flow
+		   && (_imu_sample_delayed.time_us >  _flow_sample_delayed.time_us + (uint64_t)10e6)) {
+
 		stopFlowFusion();
 	}
 }
@@ -483,9 +494,9 @@ void Ekf::updateOnGroundMotionForOpticalFlowChecks()
 	const float accel_norm = _accel_vec_filt.norm();
 
 	const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
-				    || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
+					  || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
-				    || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
+					  || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
-				    || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
+					  || (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively
 
 	if (motion_is_excessive) {
 		_time_bad_motion_us = _imu_sample_delayed.time_us;
@@ -536,24 +547,29 @@ void Ekf::controlGpsFusion()
 			}
 
 		}  else if (_control_status.flags.gps
-		            && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
+			    && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
+
 			stopGpsFusion();
 		}
 
 		// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
 		if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 			stopGpsFusion();
+
 			// Reset position state to external vision if we are going to use absolute values
 			if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
 				resetHorizontalPosition();
 			}
+
 			_warning_events.flags.gps_quality_poor = true;
 			ECL_WARN("GPS quality poor - stopping use");
 		}
 
 		// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
 		// we can start using GPS
-		const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
+		const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset
+						 && isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
+
 		if (align_yaw_using_gsf) {
 			if (resetYawToEKFGSF()) {
 				_ekfgsf_yaw_reset_time = _time_last_imu;
@@ -566,9 +582,9 @@ void Ekf::controlGpsFusion()
 			// We are relying on aiding to constrain drift so after a specified time
 			// with no aiding we need to do something
 			bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-					&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
+						&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
-					&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+						&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-					&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
+						&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
 
 			// We haven't had an absolute position fix for a longer time so need to do something
 			do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
@@ -604,6 +620,7 @@ void Ekf::controlGpsFusion()
 							  (_time_last_hor_pos_fuse > _time_last_on_ground_us);
 
 			bool is_yaw_failure = false;
+
 			if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) {
 				// Do sanity check to see if the innovation failures is likely caused by a yaw angle error
 				// by measuring the angle between the velocity estimate and the last velocity observation
@@ -720,12 +737,14 @@ void Ekf::controlGpsFusion()
 		stopGpsFusion();
 		_warning_events.flags.gps_data_stopped = true;
 		ECL_WARN("GPS data stopped");
-	}  else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
+
+	}  else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6)
+		    && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 		// Handle the case where we are fusing another position source along GPS,
 		// stop waiting for GPS after 1 s of lost signal
 		stopGpsFusion();
 		_warning_events.flags.gps_data_stopped_using_alternate = true;
-		ECL_WARN("GPS data stopped, using only EV, OF or air data" );
+		ECL_WARN("GPS data stopped, using only EV, OF or air data");
 	}
 }
 
@@ -747,10 +766,10 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 		const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
-		                                         && _control_status.flags.tilt_align
+				&& _control_status.flags.tilt_align
-		                                         && gps_checks_passing
+				&& gps_checks_passing
-							 && !is_gps_yaw_data_intermittent
+				&& !is_gps_yaw_data_intermittent
-		                                         && !_gps_hgt_intermittent;
+				&& !_gps_hgt_intermittent;
 
 		_time_last_gps_yaw_data = _time_last_imu;
 
@@ -779,6 +798,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
 						stopGpsYawFusion();
 					}
+
 					// TODO: should we give a new reset credit when the fusion does not fail for some time?
 				}
 
@@ -788,7 +808,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 			}
 
 		} else {
-		        if (starting_conditions_passing) {
+			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
 				if (resetYawToGps()) {
 					_control_status.flags.yaw_align = true;
@@ -798,8 +818,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 			}
 		}
 
-	} else if (_control_status.flags.gps_yaw
+	} else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) {
-	           && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) {
 		// No yaw data in the message anymore. Stop until it comes back.
 		stopGpsYawFusion();
 	}
@@ -834,8 +853,8 @@ void Ekf::controlHeightSensorTimeouts()
 	if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
 
 		bool request_height_reset = false;
-		const char* failing_height_source = nullptr;
+		const char *failing_height_source = nullptr;
-		const char* new_height_source = nullptr;
+		const char *new_height_source = nullptr;
 
 		if (_control_status.flags.baro_hgt) {
 			// check if GPS height is available
@@ -847,7 +866,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 			// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
 			const bool reset_to_gps = !_gps_hgt_intermittent &&
-					    ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);
+						  ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);
 
 			if (reset_to_gps) {
 				// set height sensor health
@@ -873,12 +892,11 @@ void Ekf::controlHeightSensorTimeouts()
 			// check the baro height source for consistency and freshness
 			const baroSample &baro_init = _baro_buffer.get_newest();
 			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
-			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate);
+			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
 
 			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
 			const bool reset_to_baro = !_baro_hgt_faulty &&
-						   ((baro_data_consistent && !gps_hgt_accurate) ||
+						   ((baro_data_consistent && !gps_hgt_accurate) || _gps_hgt_intermittent);
-						     _gps_hgt_intermittent);
 
 			if (reset_to_baro) {
 				startBaroHgtFusion();
@@ -918,8 +936,8 @@ void Ekf::controlHeightSensorTimeouts()
 				failing_height_source = "ev";
 				new_height_source = "ev";
 
-			// Fallback to rangefinder data if available
 			} else if (_range_sensor.isHealthy()) {
+				// Fallback to rangefinder data if available
 				setControlRangeHeight();
 				request_height_reset = true;
 				failing_height_source = "ev";
@@ -957,6 +975,7 @@ void Ekf::checkVerticalAccelerationHealth()
 	// Don't use stale innovation data.
 	bool is_inertial_nav_falling = false;
 	bool are_vertical_pos_and_vel_independant = false;
+
 	if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
 		if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
 			// If vertical position and velocity come from independent sensors then we can
@@ -966,6 +985,7 @@ void Ekf::checkVerticalAccelerationHealth()
 			are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both);
 			is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > 0.0f;
 			is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > 0.0f;
+
 		} else {
 			// only height sensing available
 			is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim;
@@ -977,17 +997,19 @@ void Ekf::checkVerticalAccelerationHealth()
 	const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
 				 _imu_sample_delayed.delta_vel_clipping[1] ||
 				 _imu_sample_delayed.delta_vel_clipping[2];
-	if (is_clipping &&_clip_counter < clip_count_limit) {
+
+	if (is_clipping && _clip_counter < clip_count_limit) {
 		_clip_counter++;
+
 	} else if (_clip_counter > 0) {
 		_clip_counter--;
 	}
+
 	const bool is_clipping_frequently = _clip_counter > 0;
 
 	// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
 	// innovations are large
-	const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) &&
+	const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling;
-				is_inertial_nav_falling;
 
 	if (bad_vert_accel) {
 		_time_bad_vert_accel =  _time_last_imu;
@@ -1091,21 +1113,27 @@ void Ekf::controlHeightFusion()
 			if (!_gps_hgt_intermittent && _gps_checks_passed) {
 				// GPS quality OK
 				startGpsHgtFusion();
+
 			} else if (!_baro_hgt_faulty) {
 				// Use baro as a fallback
 				startBaroHgtFusion();
 			}
+
 		} else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
 			// In baro fallback mode and GPS has recovered so start using it
 			startGpsHgtFusion();
 		}
+
 		if (_control_status.flags.gps_hgt && _gps_data_ready) {
 			fuse_height = true;
+
 		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
 			fuse_height = true;
+
 		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
 			fuse_height = true;
 		}
+
 		break;
 
 	case VDIST_SENSOR_EV:
@@ -1114,18 +1142,21 @@ void Ekf::controlHeightFusion()
 		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 			fuse_height = true;
 			setControlEVHeight();
+
 			if (!_control_status_prev.flags.rng_hgt) {
-				 resetHeight();
+				resetHeight();
 			}
 		}
 
 		if (_control_status.flags.ev_hgt && _ev_data_ready) {
-		     fuse_height = true;
+			fuse_height = true;
+
 		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-		      fuse_height = true;
+			fuse_height = true;
+
 		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
-		      fuse_height = true;
+			fuse_height = true;
-	    }
+		}
 
 		break;
 	}
@@ -1174,9 +1205,10 @@ void Ekf::controlHeightFusion()
 					}
 				}
 			}
+
 			// fuse height information
-			fuseVerticalPosition(_baro_hgt_innov,baro_hgt_innov_gate,
+			fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
-				baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio);
+					     baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
 
 		} else if (_control_status.flags.gps_hgt) {
 			Vector2f gps_hgt_innov_gate;
@@ -1187,23 +1219,23 @@ void Ekf::controlHeightFusion()
 			// innovation gate size
 			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
 			// fuse height information
-			fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate,
+			fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
-				gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
+					     gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
 
 		} else if (_control_status.flags.rng_hgt) {
 			Vector2f rng_hgt_innov_gate;
 			Vector3f rng_hgt_obs_var;
 			// use range finder with tilt correction
 			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-							 _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+							     _params.rng_gnd_clearance)) - _hgt_sensor_offset;
 			// observation variance - user parameter defined
 			rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
 						   + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
 			// innovation gate size
 			rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
 			// fuse height information
-			fuseVerticalPosition(_rng_hgt_innov,rng_hgt_innov_gate,
+			fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
-				rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio);
+					     rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
 
 		} else if (_control_status.flags.ev_hgt) {
 			Vector2f ev_hgt_innov_gate;
@@ -1215,8 +1247,8 @@ void Ekf::controlHeightFusion()
 			// innovation gate size
 			ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
 			// fuse height information
-			fuseVerticalPosition(_ev_pos_innov,ev_hgt_innov_gate,
+			fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
-				ev_hgt_obs_var, _ev_pos_innov_var,_ev_pos_test_ratio);
+					     ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
 		}
 	}
 }
@@ -1257,6 +1289,7 @@ void Ekf::controlAirDataFusion()
 	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
 	const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
+
 	if (_control_status.flags.wind &&
 	    (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) {
 		_control_status.flags.wind = false;
@@ -1291,6 +1324,7 @@ void Ekf::controlBetaFusion()
 
 	// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
 	const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
+
 	if (beta_fusion_time_triggered &&
 	    _control_status.flags.fuse_beta &&
 	    _control_status.flags.in_air) {
@@ -1315,15 +1349,16 @@ void Ekf::controlDragFusion()
 	    !_using_synthetic_position &&
 	    _control_status.flags.in_air &&
 	    !_mag_inhibit_yaw_reset_req) {
-			if (!_control_status.flags.wind) {
-				// reset the wind states and covariances when starting drag accel fusion
-				_control_status.flags.wind = true;
-				resetWindStates();
-				resetWindCovariance();
 
-			} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
+		if (!_control_status.flags.wind) {
-				fuseDrag();
+			// reset the wind states and covariances when starting drag accel fusion
-			}
+			_control_status.flags.wind = true;
+			resetWindStates();
+			resetWindCovariance();
+
+		} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
+			fuseDrag();
+		}
 
 	}
 }
@@ -1355,6 +1390,7 @@ void Ekf::controlFakePosFusion()
 				}
 
 			}
+
 			_time_last_fake_pos = _time_last_imu;
 
 			Vector3f fake_pos_obs_var;
@@ -1376,7 +1412,7 @@ void Ekf::controlFakePosFusion()
 			const Vector2f fake_pos_innov_gate(3.0f, 3.0f);
 
 			fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
-						_gps_pos_innov_var, _gps_pos_test_ratio, true);
+					       _gps_pos_innov_var, _gps_pos_test_ratio, true);
 		}
 
 	} else {
@@ -1398,7 +1434,7 @@ void Ekf::controlAuxVelFusion()
 		_last_vel_obs_var = _aux_vel_innov_var;
 
 		fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
-				_aux_vel_innov_var, _aux_vel_test_ratio);
+				       _aux_vel_innov_var, _aux_vel_test_ratio);
 
 		// Can be enabled after bit for this is added to EKF_AID_MASK
 		// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,

src/modules/ekf2/EKF/covariance.cpp

@@ -140,7 +140,7 @@ void Ekf::predictCovariance()
 	_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;
 
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
-					    || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
+					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
 
 	const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)
 					 || is_manoeuvre_level_high
@@ -172,7 +172,7 @@ void Ekf::predictCovariance()
 	// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
 	float mag_I_sig;
 
-	if (_control_status.flags.mag_3D && (P(16,16) + P(17,17) + P(18,18)) < 0.1f) {
+	if (_control_status.flags.mag_3D && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
 		mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
 
 	} else {
@@ -182,7 +182,7 @@ void Ekf::predictCovariance()
 	// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
 	float mag_B_sig;
 
-	if (_control_status.flags.mag_3D && (P(19,19) + P(20,20) + P(21,21)) < 0.1f) {
+	if (_control_status.flags.mag_3D && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
 		mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
 
 	} else {
@@ -210,15 +210,15 @@ void Ekf::predictCovariance()
 	// These are kinematic states and their error growth is controlled separately by the IMU noise variances
 
 	// delta angle bias states
-	process_noise.slice<3,1>(10,0) = sq(d_ang_bias_sig);
+	process_noise.slice<3, 1>(10, 0) = sq(d_ang_bias_sig);
 	// delta_velocity bias states
-	process_noise.slice<3,1>(13,0) = sq(d_vel_bias_sig);
+	process_noise.slice<3, 1>(13, 0) = sq(d_vel_bias_sig);
 	// earth frame magnetic field states
-	process_noise.slice<3,1>(16,0) = sq(mag_I_sig);
+	process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig);
 	// body frame magnetic field states
-	process_noise.slice<3,1>(19,0) = sq(mag_B_sig);
+	process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
 	// wind velocity states
-	process_noise.slice<2,1>(22,0) = sq(wind_vel_sig);
+	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig);
 
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
@@ -246,11 +246,13 @@ void Ekf::predictCovariance()
 		dvxVar = sq(dt * BADACC_BIAS_PNOISE);
 		_fault_status.flags.bad_acc_clipping = true;
 	}
+
 	// delta velocity Y: increase process noise if sample contained any Y axis clipping
 	if (_imu_sample_delayed.delta_vel_clipping[1]) {
 		dvyVar = sq(dt * BADACC_BIAS_PNOISE);
 		_fault_status.flags.bad_acc_clipping = true;
 	}
+
 	// delta velocity Z: increase process noise if sample contained any Z axis clipping
 	if (_imu_sample_delayed.delta_vel_clipping[2]) {
 		dvzVar = sq(dt * BADACC_BIAS_PNOISE);
@@ -585,9 +587,9 @@ void Ekf::predictCovariance()
 
 	// process noise contribution for delta angle states can be very small compared to
 	// the variances, therefore use algorithm to minimise numerical error
-	for (unsigned i = 10; i <=12; i++) {
+	for (unsigned i = 10; i <= 12; i++) {
-		const int index = i-10;
+		const int index = i - 10;
-		nextP(i,i) = kahanSummation(nextP(i,i), process_noise(i), _delta_angle_bias_var_accum(index));
+		nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
 	}
 
 	if (!_accel_bias_inhibit[0]) {
@@ -610,7 +612,7 @@ void Ekf::predictCovariance()
 		// add process noise that is not from the IMU
 		// process noise contribution for delta velocity states can be very small compared to
 		// the variances, therefore use algorithm to minimise numerical error
-		nextP(13,13) = kahanSummation(nextP(13,13), process_noise(13), _delta_vel_bias_var_accum(0));
+		nextP(13, 13) = kahanSummation(nextP(13, 13), process_noise(13), _delta_vel_bias_var_accum(0));
 
 	} else {
 		nextP.uncorrelateCovarianceSetVariance<1>(13, _prev_dvel_bias_var(0));
@@ -640,7 +642,7 @@ void Ekf::predictCovariance()
 		// add process noise that is not from the IMU
 		// process noise contribution for delta velocity states can be very small compared to
 		// the variances, therefore use algorithm to minimise numerical error
-		nextP(14,14) = kahanSummation(nextP(14,14), process_noise(14), _delta_vel_bias_var_accum(1));
+		nextP(14, 14) = kahanSummation(nextP(14, 14), process_noise(14), _delta_vel_bias_var_accum(1));
 
 	} else {
 		nextP.uncorrelateCovarianceSetVariance<1>(14, _prev_dvel_bias_var(1));
@@ -670,7 +672,7 @@ void Ekf::predictCovariance()
 		// add process noise that is not from the IMU
 		// process noise contribution for delta velocity states can be very small compared to
 		// the variances, therefore use algorithm to minimise numerical error
-		nextP(15,15) = kahanSummation(nextP(15,15), process_noise(15), _delta_vel_bias_var_accum(2));
+		nextP(15, 15) = kahanSummation(nextP(15, 15), process_noise(15), _delta_vel_bias_var_accum(2));
 
 	} else {
 		nextP.uncorrelateCovarianceSetVariance<1>(15, _prev_dvel_bias_var(2));
@@ -802,7 +804,7 @@ void Ekf::predictCovariance()
 
 		// add process noise that is not from the IMU
 		for (unsigned i = 16; i <= 21; i++) {
-			nextP(i,i) += process_noise(i);
+			nextP(i, i) += process_noise(i);
 		}
 
 	}
@@ -862,18 +864,18 @@ void Ekf::predictCovariance()
 
 		// add process noise that is not from the IMU
 		for (unsigned i = 22; i <= 23; i++) {
-			nextP(i,i) += process_noise(i);
+			nextP(i, i) += process_noise(i);
 		}
 
 	}
 
 	// stop position covariance growth if our total position variance reaches 100m
 	// this can happen if we lose gps for some time
-	if ((P(7,7) + P(8,8)) > 1e4f) {
+	if ((P(7, 7) + P(8, 8)) > 1e4f) {
 		for (uint8_t i = 7; i <= 8; i++) {
 			for (uint8_t j = 0; j < _k_num_states; j++) {
-				nextP(i,j) = P(i,j);
+				nextP(i, j) = P(i, j);
-				nextP(j,i) = P(j,i);
+				nextP(j, i) = P(j, i);
 			}
 		}
 	}
@@ -881,13 +883,13 @@ void Ekf::predictCovariance()
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	for (unsigned row = 1; row < _k_num_states; row++) {
 		for (unsigned column = 0 ; column < row; column++) {
-			P(row,column) = P(column,row) = nextP(column,row);
+			P(row, column) = P(column, row) = nextP(column, row);
 		}
 	}
 
 	// copy variances (diagonals)
 	for (unsigned i = 0; i < _k_num_states; i++) {
-		P(i,i) = nextP(i,i);
+		P(i, i) = nextP(i, i);
 	}
 
 	// fix gross errors in the covariance matrix and ensure rows and
@@ -915,22 +917,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 	for (int i = 0; i <= 3; i++) {
 		// quaternion states
-		P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[0]);
+		P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]);
 	}
 
 	for (int i = 4; i <= 6; i++) {
 		// NED velocity states
-		P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[1]);
+		P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]);
 	}
 
 	for (int i = 7; i <= 9; i++) {
 		// NED position states
-		P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[2]);
+		P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]);
 	}
 
 	for (int i = 10; i <= 12; i++) {
 		// gyro bias states
-		P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[3]);
+		P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]);
 	}
 
 	// force symmetry on the quaternion, velocity and position state covariances
@@ -953,10 +955,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 				// Skip the check for the inhibited axis
 				continue;
 			}
-			if (P(stateIndex,stateIndex) > maxStateVar) {
-				maxStateVar = P(stateIndex,stateIndex);
 
-			} else if (P(stateIndex,stateIndex) < minSafeStateVar) {
+			if (P(stateIndex, stateIndex) > maxStateVar) {
+				maxStateVar = P(stateIndex, stateIndex);
+
+			} else if (P(stateIndex, stateIndex) < minSafeStateVar) {
 				resetRequired = true;
 			}
 		}
@@ -972,7 +975,9 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 				// Skip the check for the inhibited axis
 				continue;
 			}
-			P(stateIndex,stateIndex) = math::constrain(P(stateIndex,stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg));
+
+			P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar,
+						    sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg));
 		}
 
 		// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
@@ -987,12 +992,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 		// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
 		bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
-				     && ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
+				     && ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
-				     ||   (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) )
+					 || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel))
-				     && ( (down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
+				     && ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
-				     ||   (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
+					 || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
-				     ||   (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
+					 || (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
-				     ||   (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt) ) );
+					 || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt)));
 
 		// record the pass/fail
 		if (!bad_acc_bias) {
@@ -1028,11 +1033,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	} else {
 		// constrain variances
 		for (int i = 16; i <= 18; i++) {
-			P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[5]);
+			P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]);
 		}
 
 		for (int i = 19; i <= 21; i++) {
-			P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[6]);
+			P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]);
 		}
 
 		// force symmetry
@@ -1050,7 +1055,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	} else {
 		// constrain variances
 		for (int i = 22; i <= 23; i++) {
-			P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[7]);
+			P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]);
 		}
 
 		// force symmetry
@@ -1062,14 +1067,17 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 // if the covariance correction will result in a negative variance, then
 // the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) {
+bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
+{
 	bool healthy = true;
+
 	for (int i = 0; i < _k_num_states; i++) {
 		if (P(i, i) < KHP(i, i)) {
 			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
 			healthy = false;
 		}
 	}
+
 	return healthy;
 }
 
@@ -1079,7 +1087,8 @@ void Ekf::resetMagRelatedCovariances()
 	resetMagCov();
 }
 
-void Ekf::resetQuatCov(){
+void Ekf::resetQuatCov()
+{
 	zeroQuatCov();
 
 	// define the initial angle uncertainty as variances for a rotation vector
@@ -1152,14 +1161,15 @@ void Ekf::resetWindCovariance()
 		// it is safer to remove all existing correlations to other states at this time
 		P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
 
-		P(22,22) = R_TAS*sq(cos_yaw) + R_yaw*sq(-Wx*sin_yaw - Wy*cos_yaw) + initial_wind_var_body_y*sq(sin_yaw);
+		P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
-		P(22,23) = R_TAS*sin_yaw*cos_yaw + R_yaw*(-Wx*sin_yaw - Wy*cos_yaw)*(Wx*cos_yaw - Wy*sin_yaw) - initial_wind_var_body_y*sin_yaw*cos_yaw;
+		P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
-		P(23,22) = P(22,23);
+			    initial_wind_var_body_y * sin_yaw * cos_yaw;
-		P(23,23) = R_TAS*sq(sin_yaw) + R_yaw*sq(Wx*cos_yaw - Wy*sin_yaw) + initial_wind_var_body_y*sq(cos_yaw);
+		P(23, 22) = P(22, 23);
+		P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
 
 		// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
-		P(22,22) += P(4,4);
+		P(22, 22) += P(4, 4);
-		P(23,23) += P(5,5);
+		P(23, 23) += P(5, 5);
 
 	} else {
 		// without airspeed, start with a small initial uncertainty to improve the initial estimate

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -98,8 +98,10 @@ void Ekf::fuseDrag()
 		// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
 		// parallel to the rotor disc and mass flow through the rotor disc.
 		float pred_acc = 0.0f; // predicted drag acceleration
+
 		if (axis_index == 0) {
 			float Kacc; // Derivative of specific force wrt airspeed
+
 			if (using_mcoef && using_bcoef_x) {
 				// Use a combination of bluff body and propeller momentum drag
 				const float bcoef_inv = 1.0f / _params.bcoef_x;
@@ -108,10 +110,12 @@ void Ekf::fuseDrag()
 				const float airspeed = (_params.bcoef_x  / rho) * (- mcoef_corrrected  + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
 				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
 				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * mcoef_corrrected;
+
 			} else if (using_mcoef) {
 				// Use propeller momentum drag only
 				Kacc = fmaxf(1e-1f, mcoef_corrrected);
 				pred_acc = - rel_wind_body(0) * mcoef_corrrected;
+
 			} else if (using_bcoef_x) {
 				// Use bluff body drag only
 				// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
@@ -120,6 +124,7 @@ void Ekf::fuseDrag()
 				const float bcoef_inv = 1.0f / _params.bcoef_x;
 				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
 				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign;
+
 			} else {
 				// skip this axis
 				continue;
@@ -165,6 +170,7 @@ void Ekf::fuseDrag()
 			if (_drag_innov_var(0) < R_ACC) {
 				return;
 			}
+
 			const float HK32 = Kacc / _drag_innov_var(0);
 
 			// Observation Jacobians
@@ -208,6 +214,7 @@ void Ekf::fuseDrag()
 
 		} else if (axis_index == 1) {
 			float Kacc; // Derivative of specific force wrt airspeed
+
 			if (using_mcoef && using_bcoef_y) {
 				// Use a combination of bluff body and propeller momentum drag
 				const float bcoef_inv = 1.0f / _params.bcoef_y;
@@ -216,10 +223,12 @@ void Ekf::fuseDrag()
 				const float airspeed = (_params.bcoef_y  / rho) * (- mcoef_corrrected  + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
 				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
 				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * mcoef_corrrected;
+
 			} else if (using_mcoef) {
 				// Use propeller momentum drag only
 				Kacc = fmaxf(1e-1f, mcoef_corrrected);
 				pred_acc = - rel_wind_body(1) * mcoef_corrrected;
+
 			} else if (using_bcoef_y) {
 				// Use bluff body drag only
 				// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
@@ -228,6 +237,7 @@ void Ekf::fuseDrag()
 				const float bcoef_inv = 1.0f / _params.bcoef_y;
 				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
 				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign;
+
 			} else {
 				// nothing more to do
 				return;
@@ -273,6 +283,7 @@ void Ekf::fuseDrag()
 				// calculation is badly conditioned
 				return;
 			}
+
 			const float HK32 = Kacc / _drag_innov_var(1);
 
 			// Observation Jacobians

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -262,12 +262,14 @@ void Ekf::resetHeight()
 	if (_control_status.flags.rng_hgt) {
 
 		// a fallback from any other height source to rangefinder happened
-		if(!_control_status_prev.flags.rng_hgt) {
+		if (!_control_status_prev.flags.rng_hgt) {
 
 			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-			    _hgt_sensor_offset = _terrain_vpos;
+				_hgt_sensor_offset = _terrain_vpos;
+
 			} else if (_control_status.flags.in_air) {
-				 _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+				_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+
 			} else {
 				_hgt_sensor_offset = _params.rng_gnd_clearance;
 			}
@@ -729,6 +731,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			// reset altitude
 			_gps_alt_ref = altitude;
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
+
 		} else {
 			// reset altitude
 			_gps_alt_ref = altitude;
@@ -1042,9 +1045,9 @@ void Ekf::uncorrelateQuatFromOtherStates()
 void Ekf::update_deadreckoning_status()
 {
 	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
-				&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
+				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-				|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
+				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
-				|| isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
+				      || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
 	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
 	const bool airDataAiding = _control_status.flags.wind &&
 				   isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
@@ -1316,8 +1319,7 @@ void Ekf::updateBaroHgtOffset()
 		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
 
 		// apply a 10 second first order low pass filter to baro offset
-		const float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt + _state.pos(2) -
+		const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset);
-								_baro_hgt_offset);
 		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
 	}
 }
@@ -1340,19 +1342,22 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
 	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
 
 	// rotate measurement into correct earth frame if required
-	switch(_ev_sample_delayed.vel_frame) {
+	switch (_ev_sample_delayed.vel_frame) {
-		case velocity_frame_t::BODY_FRAME_FRD:
+	case velocity_frame_t::BODY_FRAME_FRD:
-			vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
+		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
-			break;
+		break;
-		case velocity_frame_t::LOCAL_FRAME_FRD:
+
-			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+	case velocity_frame_t::LOCAL_FRAME_FRD:
-			if (_params.fusion_mode & MASK_ROTATE_EV)
+		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-			{
+
-				vel = _R_ev_to_ekf *_ev_sample_delayed.vel - vel_offset_earth;
+		if (_params.fusion_mode & MASK_ROTATE_EV) {
-			} else {
+			vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
-				vel = _ev_sample_delayed.vel - vel_offset_earth;
+
-			}
+		} else {
-			break;
+			vel = _ev_sample_delayed.vel - vel_offset_earth;
+		}
+
+		break;
 	}
 
 	return vel;
@@ -1363,17 +1368,17 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
 	Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
 
 	// rotate measurement into correct earth frame if required
-	switch(_ev_sample_delayed.vel_frame) {
+	switch (_ev_sample_delayed.vel_frame) {
-		case velocity_frame_t::BODY_FRAME_FRD:
+	case velocity_frame_t::BODY_FRAME_FRD:
-			ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
+		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
-			break;
+		break;
 
-		case velocity_frame_t::LOCAL_FRAME_FRD:
+	case velocity_frame_t::LOCAL_FRAME_FRD:
-			if(_params.fusion_mode & MASK_ROTATE_EV)
+		if (_params.fusion_mode & MASK_ROTATE_EV) {
-			{
+			ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
-				ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
+		}
-			}
+
-			break;
+		break;
 	}
 
 	return ev_vel_cov.diag();

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -520,7 +520,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
 	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
 
-	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) {
+	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length)
+	    || !_output_vert_buffer.allocate(_imu_buffer_length)) {
+
 		printBufferAllocationFailed("IMU and output");
 		return false;
 	}

src/modules/ekf2/EKF/estimator_interface.h

@@ -113,6 +113,7 @@ public:
 		} else {
 			_time_last_in_air = _time_last_imu;
 		}
+
 		_control_status.flags.in_air = in_air;
 	}
 
@@ -248,8 +249,8 @@ public:
 	// Getter for the baro sample on the delayed time horizon
 	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
 
-	const bool& global_origin_valid() const { return _NED_origin_initialised; }
+	const bool &global_origin_valid() const { return _NED_origin_initialised; }
-	const map_projection_reference_s& global_origin() const { return _pos_ref; }
+	const map_projection_reference_s &global_origin() const { return _pos_ref; }
 
 	void print_status();
 

src/modules/ekf2/EKF/gps_checks.cpp

@@ -215,8 +215,8 @@ bool Ekf::gps_is_good(const gps_message &gps)
 
 		// Check the magnitude of the filtered horizontal GPS velocity
 		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
-							     -10.0f * _params.req_hdrift,
+					   -10.0f * _params.req_hdrift,
-							      10.0f * _params.req_hdrift);
+					   10.0f * _params.req_hdrift);
 		_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
 		_gps_drift_metrics[2] = _gps_velNE_filt.norm();
 		_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -66,7 +66,7 @@ void Ekf::fuseGpsYaw()
 	}
 
 	// calculate predicted antenna yaw angle
-	const float predicted_hdg =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
+	const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
 
 	// using magnetic heading process noise
 	// TODO extend interface to use yaw uncertainty provided by GPS if available
@@ -83,6 +83,7 @@ void Ekf::fuseGpsYaw()
 	const float HK7 = ecl::powf(q0, 2) - ecl::powf(q3, 2);
 	const float HK8 = HK4*(HK5 - HK6 + HK7);
 	const float HK9 = HK3 - HK8;
+
 	if (fabsf(HK9) < 1e-3f) {
 		return;
 	}
@@ -100,6 +101,7 @@ void Ekf::fuseGpsYaw()
 	const float HK18 = 2/HK17;
 	// const float HK19 = 1.0F/(-HK3 + HK8);
 	const float HK19_inverse = -HK3 + HK8;
+
 	if (fabsf(HK19_inverse) < 1e-6f) {
 		return;
 	}
@@ -122,7 +124,7 @@ void Ekf::fuseGpsYaw()
 	_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
 
 	if (_heading_innov_var < R_YAW) {
-	// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_hdg = true;
 
 		// we reinitialise the covariance matrix and abort this fusion step
@@ -132,7 +134,7 @@ void Ekf::fuseGpsYaw()
 	}
 
 	_fault_status.flags.bad_hdg = false;
-	const float HK32 = HK18/_heading_innov_var;
+	const float HK32 = HK18 / _heading_innov_var;
 
 	// calculate the innovation and define the innovation gate
 	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);

src/modules/ekf2/EKF/mag_control.cpp

@@ -50,11 +50,11 @@ void Ekf::controlMagFusion()
 	// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
 	// ground and to prevent uncontrolled yaw variance growth
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
-		if (noOtherYawAidingThanMag())
+		if (noOtherYawAidingThanMag()) {
-		{
 			_is_yaw_fusion_inhibited = true;
 			fuseHeading();
 		}
+
 		return;
 	}
 
@@ -81,12 +81,14 @@ void Ekf::controlMagFusion()
 		// there are large external disturbances or the more accurate 3-axis fusion
 		switch (_params.mag_fusion_type) {
 		default:
+
 		/* fallthrough */
 		case MAG_FUSE_TYPE_AUTO:
 			selectMagAuto();
 			break;
 
 		case MAG_FUSE_TYPE_INDOOR:
+
 		/* fallthrough */
 		case MAG_FUSE_TYPE_HEADING:
 			startMagHdgFusion();
@@ -169,8 +171,12 @@ void Ekf::runInAirYawReset()
 	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
 		bool has_realigned_yaw = false;
 
-		if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
+		if (canRealignYawUsingGps()) {
-		else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
+			has_realigned_yaw = realignYawGPS();
+
+		} else if (canResetMagHeading()) {
+			has_realigned_yaw = resetMagHeading(_mag_lpf.getState());
+		}
 
 		if (has_realigned_yaw) {
 			_mag_yaw_reset_req = false;
@@ -266,6 +272,7 @@ void Ekf::checkMagDeclRequired()
 void Ekf::checkMagInhibition()
 {
 	_is_yaw_fusion_inhibited = shouldInhibitMag();
+
 	if (!_is_yaw_fusion_inhibited) {
 		_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
 	}
@@ -286,8 +293,8 @@ bool Ekf::shouldInhibitMag() const
 	const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
 
 	const bool heading_not_required_for_navigation = !_control_status.flags.gps
-							 && !_control_status.flags.ev_pos
+			&& !_control_status.flags.ev_pos
-							 && !_control_status.flags.ev_vel;
+			&& !_control_status.flags.ev_vel;
 
 	return (user_selected && heading_not_required_for_navigation)
 	       || isStrongMagneticDisturbance();
@@ -297,8 +304,8 @@ void Ekf::checkMagFieldStrength()
 {
 	if (_params.check_mag_strength) {
 		_control_status.flags.mag_field_disturbed = _NED_origin_initialised
-							    ? !isMeasuredMatchingGpsMagStrength()
+				? !isMeasuredMatchingGpsMagStrength()
-							    : !isMeasuredMatchingAverageMagStrength();
+				: !isMeasuredMatchingAverageMagStrength();
 
 	} else {
 		_control_status.flags.mag_field_disturbed = false;
@@ -323,13 +330,14 @@ bool Ekf::isMeasuredMatchingAverageMagStrength() const
 bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
 {
 	return (measured >= expected - gate)
-		&& (measured <= expected + gate);
+	       && (measured <= expected + gate);
 }
 
 void Ekf::runMagAndMagDeclFusions()
 {
 	if (_control_status.flags.mag_3D) {
 		run3DMagAndDeclFusions();
+
 	} else if (_control_status.flags.mag_hdg) {
 		fuseHeading();
 	}
@@ -351,6 +359,7 @@ void Ekf::run3DMagAndDeclFusions()
 		// declination angle at a higher uncertainty to allow some learning of
 		// declination angle over time.
 		fuseMag();
+
 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);
 		}
@@ -359,10 +368,10 @@ void Ekf::run3DMagAndDeclFusions()
 
 bool Ekf::otherHeadingSourcesHaveStopped()
 {
-    // detect rising edge of noOtherYawAidingThanMag()
+	// detect rising edge of noOtherYawAidingThanMag()
-    bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
+	bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
 
-    _non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
+	_non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
 
-    return  result;
+	return  result;
 }

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -436,6 +436,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 	const float SA2 = SA0*q0 + SA1*q1;
 	const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
 	float SA4, SA5_inv;
+
 	if (sq(SA3) > 1e-6f) {
 		SA4 = 1.0F/sq(SA3);
 		SA5_inv = sq(SA2)*SA4 + 1;
@@ -448,6 +449,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 	const float SB2 = SB0*q3 + SB1*q2;
 	const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
 	float SB3, SB5_inv;
+
 	if (sq(SB2) > 1e-6f) {
 		SB3 = 1.0F/sq(SB2);
 		SB5_inv = SB3*sq(SB4) + 1;
@@ -484,8 +486,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 
 	// calculate the yaw innovation and wrap to the interval between +-pi
 	float innovation;
+
 	if (zero_innovation) {
 		innovation = 0.0f;
+
 	} else {
 		innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement);
 	}
@@ -516,6 +520,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 	const float SA2 = SA0*q0 - SA1*q1;
 	const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
 	float SA4, SA5_inv;
+
 	if (sq(SA3) > 1e-6f) {
 		SA4 = 1.0F/sq(SA3);
 		SA5_inv = sq(SA2)*SA4 + 1;
@@ -528,6 +533,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 	const float SB2 = -SB0*q3 + SB1*q2;
 	const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
 	float SB3, SB5_inv;
+
 	if (sq(SB2) > 1e-6f) {
 		SB3 = 1.0F/sq(SB2);
 		SB5_inv = SB3*sq(SB4) + 1;
@@ -563,8 +569,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 	}
 
 	float innovation;
+
 	if (zero_innovation) {
 		innovation = 0.0f;
+
 	} else {
 		// calculate the the innovation and wrap to the interval between +-pi
 		innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement);
@@ -578,16 +586,18 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 }
 
 // update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
-void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f& yaw_jacobian)
+void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma,
+			   const Vector4f &yaw_jacobian)
 {
 	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
 	// calculate the innovation variance
 	_heading_innov_var = variance;
+
 	for (unsigned row = 0; row <= 3; row++) {
 		float tmp = 0.0f;
 
 		for (uint8_t col = 0; col <= 3; col++) {
-			tmp += P(row,col) * yaw_jacobian(col);
+			tmp += P(row, col) * yaw_jacobian(col);
 		}
 
 		_heading_innov_var += yaw_jacobian(row) * tmp;
@@ -617,7 +627,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 
 	for (uint8_t row = 0; row <= 15; row++) {
 		for (uint8_t col = 0; col <= 3; col++) {
-			Kfusion(row) += P(row,col) * yaw_jacobian(col);
+			Kfusion(row) += P(row, col) * yaw_jacobian(col);
 		}
 
 		Kfusion(row) *= heading_innov_var_inv;
@@ -626,7 +636,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 	if (_control_status.flags.wind) {
 		for (uint8_t row = 22; row <= 23; row++) {
 			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row,col) * yaw_jacobian(col);
+				Kfusion(row) += P(row, col) * yaw_jacobian(col);
 			}
 
 			Kfusion(row) *= heading_innov_var_inv;
@@ -680,11 +690,11 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 		KH[3] = Kfusion(row) * yaw_jacobian(3);
 
 		for (unsigned column = 0; column < _k_num_states; column++) {
-			float tmp = KH[0] * P(0,column);
+			float tmp = KH[0] * P(0, column);
-			tmp += KH[1] * P(1,column);
+			tmp += KH[1] * P(1, column);
-			tmp += KH[2] * P(2,column);
+			tmp += KH[2] * P(2, column);
-			tmp += KH[3] * P(3,column);
+			tmp += KH[3] * P(3, column);
-			KHP(row,column) = tmp;
+			KHP(row, column) = tmp;
 		}
 	}
 
@@ -711,6 +721,7 @@ void Ekf::fuseHeading()
 
 	// Calculate the observation variance
 	float R_YAW;
+
 	if (_control_status.flags.mag_hdg) {
 		// using magnetic heading tuning parameter
 		R_YAW = sq(_params.mag_heading_noise);
@@ -747,6 +758,7 @@ void Ekf::fuseHeading()
 
 		// handle special case where yaw measurement is unavailable
 		bool fuse_zero_innov = false;
+
 		if (_is_yaw_fusion_inhibited) {
 			// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
 			// conditioned covariance matrix developing over time.
@@ -755,12 +767,14 @@ void Ekf::fuseHeading()
 				// unconstrained quaternion variance growth and record the predicted heading
 				// to use as an observation when movement ceases.
 				// TODO a better way of determining when this is necessary
-				const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
+				const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
+
 				if (sumQuatVar > _params.quat_max_variance) {
 					fuse_zero_innov = true;
 					R_YAW = 0.25f;
 
 				}
+
 				_last_static_yaw = predicted_hdg;
 
 			} else {
@@ -770,6 +784,7 @@ void Ekf::fuseHeading()
 				measured_hdg = _last_static_yaw;
 
 			}
+
 		} else {
 			_last_static_yaw = predicted_hdg;
 
@@ -798,6 +813,7 @@ void Ekf::fuseHeading()
 
 		// handle special case where yaw measurement is unavailable
 		bool fuse_zero_innov = false;
+
 		if (_is_yaw_fusion_inhibited) {
 			// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
 			// conditioned covariance matrix developing over time.
@@ -806,12 +822,14 @@ void Ekf::fuseHeading()
 				// unconstrained quaterniion variance growth and record the predicted heading
 				// to use as an observation when movement ceases.
 				// TODO a better way of determining when this is necessary
-				const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
+				const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
+
 				if (sumQuatVar > _params.quat_max_variance) {
 					fuse_zero_innov = true;
 					R_YAW = 0.25f;
 
 				}
+
 				_last_static_yaw = predicted_hdg;
 
 			} else {
@@ -821,6 +839,7 @@ void Ekf::fuseHeading()
 				measured_hdg = _last_static_yaw;
 
 			}
+
 		} else {
 			_last_static_yaw = predicted_hdg;
 
@@ -848,6 +867,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 		// calculation is badly conditioned close to +-90 deg declination
 		return;
 	}
+
 	const float HK0 = ecl::powf(magN, -2);
 	const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F;
 	const float HK2 = 1.0F/HK1;
@@ -859,6 +879,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 	const float HK8 = HK5*P(16,16) - P(16,17);
 	const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL;
 	float HK9;
+
 	if (innovation_variance > R_DECL) {
 		HK9 = HK4/innovation_variance;
 	} else {
@@ -875,6 +896,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 
 	// Calculate the Kalman gains
 	Vector24f Kfusion;
+
 	for (unsigned row = 0; row <= 15; row++) {
 		Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
 	}
@@ -903,15 +925,17 @@ void Ekf::limitDeclination()
 	// set to 50% of the horizontal strength from geo tables if location is known
 	float decl_reference;
 	float h_field_min = 0.001f;
+
 	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
 		if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
 			decl_reference = _mag_declination_gps;
-			h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
+			h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
 
 		} else {
 			decl_reference = math::radians(_params.mag_declination_deg);
 		}
+
 	} else {
 		// always use the parameter value
 		decl_reference = math::radians(_params.mag_declination_deg);
@@ -920,18 +944,21 @@ void Ekf::limitDeclination()
 	// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
 	// and can result in a reversal of the NE field states which the filter cannot recover from
 	// apply a circular limit
-	float h_field = sqrtf(_state.mag_I(0)*_state.mag_I(0) + _state.mag_I(1)*_state.mag_I(1));
+	float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1));
+
 	if (h_field < h_field_min) {
 		if (h_field > 0.001f * h_field_min) {
 			const float h_scaler = h_field_min / h_field;
 			_state.mag_I(0) *= h_scaler;
 			_state.mag_I(1) *= h_scaler;
+
 		} else {
 			// too small to scale radially so set to expected value
 			const float mag_declination = getMagDeclination();
 			_state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination);
 			_state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination);
 		}
+
 		h_field = h_field_min;
 	}
 
@@ -939,17 +966,19 @@ void Ekf::limitDeclination()
 	constexpr float decl_tolerance = 0.5f;
 	const float decl_max = decl_reference + decl_tolerance;
 	const float decl_min = decl_reference - decl_tolerance;
-	const float decl_estimate = atan2f(_state.mag_I(1) , _state.mag_I(0));
+	const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0));
+
 	if (decl_estimate > decl_max)  {
 		_state.mag_I(0) = h_field * cosf(decl_max);
 		_state.mag_I(1) = h_field * sinf(decl_max);
+
 	} else if (decl_estimate < decl_min)  {
 		_state.mag_I(0) = h_field * cosf(decl_min);
 		_state.mag_I(1) = h_field * sinf(decl_min);
 	}
 }
 
-float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f& mag_meas, const Vector3f& mag_earth_predicted)
+float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
 {
 	// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
 	// of the earth magnetic field vector at the current location

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -240,7 +240,7 @@ void Ekf::fuseOptFlow()
 	float test_ratio[2];
 	test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
 	test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
-	_optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]);
+	_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);
 
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
 		if (test_ratio[obs_index] > 1.0f) {
@@ -262,6 +262,7 @@ void Ekf::fuseOptFlow()
 	// fuse observation axes sequentially
 	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
 	Vector24f Kfusion; // Optical flow Kalman gains
+
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
 
 		// calculate observation Jocobians and Kalman gains
@@ -339,7 +340,7 @@ bool Ekf::calcOptFlowBodyRateComp()
 		return false;
 	}
 
-	const bool use_flow_sensor_gyro =  PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
+	const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
 
 	if (use_flow_sensor_gyro) {
 

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -137,7 +137,8 @@ void Ekf::fuseSideslip()
 			_fault_status.flags.bad_sideslip = true;
 
 			// if we are getting aiding from other sources, warn and reset the wind states and covariances only
-			const char* action_string = nullptr;
+			const char *action_string = nullptr;
+
 			if (update_wind_only) {
 				resetWindStates();
 				resetWindCovariance();
@@ -148,12 +149,14 @@ void Ekf::fuseSideslip()
 				_state.wind_vel.setZero();
 				action_string = "full";
 			}
+
 			ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
 
 			return;
 		}
+
 		_fault_status.flags.bad_sideslip = false;
-		const float HK52 = HK16/_beta_innov_var;
+		const float HK52 = HK16 / _beta_innov_var;
 
 		// Calculate predicted sideslip angle and innovation using small angle approximation
 		_beta_innov = rel_wind_body(1) / rel_wind_body(0);
@@ -184,6 +187,7 @@ void Ekf::fuseSideslip()
 
 		// Calculate Kalman gains
 		Vector24f Kfusion;
+
 		if (!update_wind_only) {
 
 			Kfusion(0) = HK38*HK52;

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -142,9 +142,9 @@ void Ekf::fuseHagl()
 	_hagl_innov = pred_hagl - meas_hagl;
 
 	// calculate the observation variance adding the variance of the vehicles own height uncertainty
-	const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
+	const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
-			     + sq(_params.range_noise)
+				   + sq(_params.range_noise)
-			     + sq(_params.range_noise_scaler * _range_sensor.getRange());
+				   + sq(_params.range_noise_scaler * _range_sensor.getRange());
 
 	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
 	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
@@ -167,6 +167,7 @@ void Ekf::fuseHagl()
 	} else {
 		// If we have been rejecting range data for too long, reset to measurement
 		const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
+
 		if (isTimedOut(_time_last_hagl_fuse, timeout)) {
 			_terrain_vpos = _state.pos(2) + meas_hagl;
 			_terrain_var = obs_variance;
@@ -290,8 +291,7 @@ void Ekf::updateTerrainValidity()
 	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
 
 	_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
-						 && recent_range_fusion
+			&& recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
-						 && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
 
 	_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion;
 }