Merge pull request #321 from dagar/pr-ekf_formatting

EKF trivial code style cleanup

EKF/airspeed_fusion.cpp

@@ -150,8 +150,7 @@ void Ekf::fuseAirspeed()
 		if (_tas_test_ratio > 1.0f) {
 			_innov_check_fail_status.flags.reject_airspeed = true;
 			return;
-		}
+		} else {
-		else {
 			_innov_check_fail_status.flags.reject_airspeed = false;
 		}
 

EKF/control.cpp

@@ -968,11 +968,13 @@ void Ekf::checkForStuckRange()
 			_rng_stuck = false;
 
 		} else {
-			if (_range_sample_delayed.rng > _rng_check_max_val)
+			if (_range_sample_delayed.rng > _rng_check_max_val) {
 				_rng_check_max_val = _range_sample_delayed.rng;
+			}
 
-			if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val)
+			if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) {
 				_rng_check_min_val = _range_sample_delayed.rng;
+			}
 
 			_range_data_ready = false;
 		}
@@ -989,6 +991,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
 	bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
 	bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
+
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
 
@@ -1049,9 +1052,6 @@ void Ekf::controlBetaFusion()
 
 		fuseSideslip();
 	}
-
-
-
 }
 
 void Ekf::controlDragFusion()

EKF/covariance.cpp

@@ -733,9 +733,11 @@ void Ekf::fixCovarianceErrors()
 		const float minSafeStateVar = 1e-9f;
 		float maxStateVar = minSafeStateVar;
 		bool resetRequired = false;
+
 		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
 			if (P[stateIndex][stateIndex] > maxStateVar) {
 				maxStateVar = P[stateIndex][stateIndex];
+
 			} else if (P[stateIndex][stateIndex] < minSafeStateVar) {
 				resetRequired = true;
 			}
@@ -746,19 +748,24 @@ void Ekf::fixCovarianceErrors()
 		// Also limit variance to a maximum equivalent to a 1g uncertainty
 		const float minStateVarTarget = 1E-8f;
 		float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
+
 		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
-			P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(_gravity_mss * _dt_ekf_avg));
+			P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar,
+						    sq(_gravity_mss * _dt_ekf_avg));
 		}
 
 		// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
 		if (resetRequired) {
 			float delVelBiasVar[3];
+
 			// store all delta velocity bias variances
 			for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
 				delVelBiasVar[stateIndex - 13] = P[stateIndex][stateIndex];
 			}
+
 			// reset all delta velocity bias covariances
 			zeroCols(P, 13, 15);
+
 			// restore all delta velocity bias variances
 			for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
 				P[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13];
@@ -769,6 +776,7 @@ void Ekf::fixCovarianceErrors()
 		// calculate accel bias term aligned with the gravity vector
 		float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
 		float down_dvel_bias = 0.0f;
+
 		for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
 			down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index);
 		}
@@ -893,5 +901,4 @@ void Ekf::resetWindCovariance()
 		P[23][23] = sq(1.0f);
 
 	}
-
 }

EKF/drag_fusion.cpp

@@ -150,9 +150,11 @@ void Ekf::fuseDrag()
 			float drag_sign;
 			if (rel_wind(axis_index) >= 0.0f) {
 				drag_sign = 1.0f;
+
 			} else {
 				drag_sign = -1.0f;
 			}
+
 			float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
 			_drag_innov[axis_index] = predAccel - mea_acc;
 			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
@@ -222,9 +224,11 @@ void Ekf::fuseDrag()
 			float drag_sign;
 			if (rel_wind(axis_index) >= 0.0f) {
 				drag_sign = 1.0f;
+
 			} else {
 				drag_sign = -1.0f;
 			}
+
 			float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
 			_drag_innov[axis_index] = predAccel - mea_acc;
 			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);

EKF/ekf.cpp

@@ -43,21 +43,6 @@
 #include "ekf.h"
 #include "mathlib.h"
 
-#ifndef __PX4_QURT
-#if defined(__cplusplus) && !defined(__PX4_NUTTX)
-#include <cmath>
-#define ISFINITE(x) std::isfinite(x)
-#else
-#define ISFINITE(x) isfinite(x)
-#endif
-#endif
-
-#if defined(__PX4_QURT)
-// Missing math.h defines
-#define ISFINITE(x) __builtin_isfinite(x)
-#endif
-
-
 bool Ekf::init(uint64_t timestamp)
 {
 	bool ret = initialise_interface(timestamp);
@@ -617,6 +602,7 @@ void Ekf::calculateOutputStates()
 			unsigned index = _output_vert_buffer.get_oldest_index();
 			unsigned index_next;
 			unsigned size = _output_vert_buffer.get_length();
+
 			for (unsigned counter = 0; counter < (size - 1); counter++) {
 				index_next = (index + 1) % size;
 				current_state = _output_vert_buffer.get_from_index(index);

EKF/geo.cpp

@@ -42,23 +42,15 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 #ifdef POSIX_SHARED
-//#include <unistd.h>
+
-//#include <pthread.h>
+#include "ecl.h"
+
 #include <stdio.h>
 #include <math.h>
 #include <stdbool.h>
 #include <string.h>
 #include <float.h>
 
-#ifndef __PX4_QURT
-#if defined(__cplusplus) && !defined(__PX4_NUTTX)
-#include <cmath>
-#define ISFINITE(x) std::isfinite(x)
-#else
-#define ISFINITE(x) isfinite(x)
-#endif
-#endif
-
 /****************************************************************************
  *
  *   Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.

EKF/sideslip_fusion.cpp

@@ -84,9 +84,11 @@ void Ekf::fuseSideslip()
 
 		// intermediate variable from algebraic optimisation
 		SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + (ve - vwe)*(2.0f*q0*q3 + 2.0f*q1*q2);
+
 		if (fabsf(SH_BETA[0]) <= 1e-9f) {
 			return;
 		}
+
 		SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - (vn - vwn)*(2.0f*q0*q3 - 2.0f*q1*q2);
 		SH_BETA[2] = vn - vwn;
 		SH_BETA[3] = ve - vwe;
@@ -122,8 +124,10 @@ void Ekf::fuseSideslip()
 		if (_beta_innov_var >= R_BETA) {
 			SK_BETA[0] = 1.0f / _beta_innov_var;
 			_fault_status.flags.bad_sideslip = false;
+
 		} else { // Reset the estimator
 			_fault_status.flags.bad_sideslip = true;
+
 			// if we are getting aiding from other sources, warn and reset the wind states and covariances only
 			if (update_wind_only) {
 				resetWindStates();
@@ -134,10 +138,11 @@ void Ekf::fuseSideslip()
 				initialiseCovariance();
 				_state.wind_vel.setZero();
 				ECL_ERR("EKF synthetic sideslip fusion badly conditioned - full covariance reset");
-
 			}
+
 			return;
 		}
+
 		SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
 		SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2);
 		SK_BETA[3] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3);
@@ -188,6 +193,7 @@ void Ekf::fuseSideslip()
 				}
 			}
 		}
+
 		Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]);
 		Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]);
 
@@ -204,7 +210,6 @@ void Ekf::fuseSideslip()
 
 		} else {
 			_innov_check_fail_status.flags.reject_sideslip = false;
-
 		}
 
 		// synthetic sideslip measurement sample has passed check so record it
@@ -215,8 +220,8 @@ void Ekf::fuseSideslip()
 		// then calculate P - KHP
 		float KHP[_k_num_states][_k_num_states];
 		float KH[9];
-	for (unsigned row = 0; row < _k_num_states; row++) {
 
+		for (unsigned row = 0; row < _k_num_states; row++) {
 			KH[0] = Kfusion[row] * H_BETA[0];
 			KH[1] = Kfusion[row] * H_BETA[1];
 			KH[2] = Kfusion[row] * H_BETA[2];
@@ -245,6 +250,7 @@ void Ekf::fuseSideslip()
 		// the covariance marix is unhealthy and must be corrected
 		bool healthy = true;
 		_fault_status.flags.bad_sideslip = false;
+
 		for (int i = 0; i < _k_num_states; i++) {
 			if (P[i][i] < KHP[i][i]) {
 				// zero rows and columns
@@ -256,7 +262,6 @@ void Ekf::fuseSideslip()
 
 				// update individual measurement health status
 				_fault_status.flags.bad_sideslip = true;
-
 			}
 		}
 
@@ -274,7 +279,6 @@ void Ekf::fuseSideslip()
 
 			// apply the state corrections
 			fuse(Kfusion, _beta_innov);
-
 		}
 	}
 }

ecl.h

@@ -56,3 +56,17 @@
 #define ECL_ERR printf
 
 #endif
+
+#ifndef __PX4_QURT
+#if defined(__cplusplus) && !defined(__PX4_NUTTX)
+#include <cmath>
+#define ISFINITE(x) std::isfinite(x)
+#else
+#define ISFINITE(x) isfinite(x)
+#endif
+#endif
+
+#if defined(__PX4_QURT)
+// Missing math.h defines
+#define ISFINITE(x) __builtin_isfinite(x)
+#endif