EKF: fix formatting

EKF/common.h

@@ -50,7 +50,7 @@ struct gps_message {
 	float eph;                  // GPS horizontal position accuracy in m
 	float epv;                  // GPS vertical position accuracy in m
 	float sacc;                 // GPS speed accuracy in m/s
-        uint64_t time_usec_vel;     // Timestamp for velocity informations
+	uint64_t time_usec_vel;     // Timestamp for velocity informations
 	float vel_m_s;              // GPS ground speed (m/s)
 	float vel_ned[3];           // GPS ground speed NED
 	bool vel_ned_valid;         // GPS ground speed is valid
@@ -112,35 +112,35 @@ struct flowSample {
 };
 
 struct parameters {
-        float mag_delay_ms = 0.0f;          // magnetometer measurement delay relative to the IMU
-        float baro_delay_ms = 0.0f;         // barometer height measurement delay relative to the IMU
-        float gps_delay_ms = 200.0f;        // GPS measurement delay relative to the IMU
-        float airspeed_delay_ms = 200.0f;   // airspeed measurement delay relative to the IMU
+	float mag_delay_ms = 0.0f;          // magnetometer measurement delay relative to the IMU
+	float baro_delay_ms = 0.0f;         // barometer height measurement delay relative to the IMU
+	float gps_delay_ms = 200.0f;        // GPS measurement delay relative to the IMU
+	float airspeed_delay_ms = 200.0f;   // airspeed measurement delay relative to the IMU
 
 	// input noise
-        float gyro_noise = 1.0e-3f;         // IMU angular rate noise used for covariance prediction
-        float accel_noise = 2.5e-1f;        // IMU acceleration noise use for covariance prediction
+	float gyro_noise = 1.0e-3f;         // IMU angular rate noise used for covariance prediction
+	float accel_noise = 2.5e-1f;        // IMU acceleration noise use for covariance prediction
 
 	// process noise
-        float gyro_bias_p_noise = 7.0e-5f;  // process noise for IMU delta angle bias prediction
-        float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction
-        float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction
-        float mag_p_noise = 2.5e-2f;        // process noise for magnetic field prediction
-        float wind_vel_p_noise = 1.0e-1f;   // process noise for wind velocity prediction
+	float gyro_bias_p_noise = 7.0e-5f;  // process noise for IMU delta angle bias prediction
+	float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction
+	float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction
+	float mag_p_noise = 2.5e-2f;        // process noise for magnetic field prediction
+	float wind_vel_p_noise = 1.0e-1f;   // process noise for wind velocity prediction
 
-        float gps_vel_noise = 5.0e-1f;      // observation noise for gps velocity fusion
-        float gps_pos_noise = 1.0f;         // observation noise for gps position fusion
-        float pos_noaid_noise = 10.0f;      // observation noise for non-aiding position fusion
-        float baro_noise = 3.0f;            // observation noise for barometric height fusion
-        float baro_innov_gate = 3.0f;       // barometric height innovation consistency gate size in standard deviations
-        float posNE_innov_gate = 3.0f;      // GPS horizontal position innovation consistency gate size in standard deviations
+	float gps_vel_noise = 5.0e-1f;      // observation noise for gps velocity fusion
+	float gps_pos_noise = 1.0f;         // observation noise for gps position fusion
+	float pos_noaid_noise = 10.0f;      // observation noise for non-aiding position fusion
+	float baro_noise = 3.0f;            // observation noise for barometric height fusion
+	float baro_innov_gate = 3.0f;       // barometric height innovation consistency gate size in standard deviations
+	float posNE_innov_gate = 3.0f;      // GPS horizontal position innovation consistency gate size in standard deviations
 	float vel_innov_gate = 3.0f;        // GPS velocity innovation consistency gate size in standard deviations
 
-        float mag_heading_noise = 1.7e-1f;  // measurement noise used for simple heading fusion
-        float mag_noise = 5.0e-2f;          // measurement noise used for 3-axis magnetoemeter fusion
-        float mag_declination_deg = 0.0f;   // magnetic declination in degrees
-        float heading_innov_gate = 3.0f;    // heading fusion innovation consistency gate size in standard deviations
-        float mag_innov_gate = 3.0f;        // magnetometer fusion innovation consistency gate size in standard deviations
+	float mag_heading_noise = 1.7e-1f;  // measurement noise used for simple heading fusion
+	float mag_noise = 5.0e-2f;          // measurement noise used for 3-axis magnetoemeter fusion
+	float mag_declination_deg = 0.0f;   // magnetic declination in degrees
+	float heading_innov_gate = 3.0f;    // heading fusion innovation consistency gate size in standard deviations
+	float mag_innov_gate = 3.0f;        // magnetometer fusion innovation consistency gate size in standard deviations
 
 	// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
 	// good enough to set a local origin and commence aiding

EKF/covariance.cpp

@@ -130,12 +130,12 @@ void Ekf::predictCovariance()
 	// compute process noise
 	float process_noise[_k_num_states] = {};
 
-    float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
-    float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f);
-    float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f);
-    float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
-    float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
-    float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
+	float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
+	float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f);
+	float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f);
+	float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
+	float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
+	float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
 
 	for (unsigned i = 0; i < 9; i++) {
 		process_noise[i] = 0.0f;

EKF/ekf.cpp

@@ -330,8 +330,8 @@ bool Ekf::collect_imu(imuSample &imu)
 	_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
 	_imu_down_sampled.delta_vel += imu.delta_vel;
 
-	if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) || 
-		_dt_imu_avg * _imu_ticks >= 0.02f){
+	if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
+	    _dt_imu_avg * _imu_ticks >= 0.02f) {
 		imu = {
 			.delta_ang	= _q_down_sampled.to_axis_angle(),
 			.delta_vel	= _imu_down_sampled.delta_vel,

EKF/estimator_interface.cpp

@@ -137,7 +137,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
 
 		_gps_speed_valid = gps->vel_ned_valid;
-                _gps_speed_accuracy = gps->sacc;
+		_gps_speed_accuracy = gps->sacc;
 
 		float lpos_x = 0.0f;
 		float lpos_y = 0.0f;

EKF/estimator_interface.h

@@ -197,8 +197,8 @@ protected:
 
 	bool _NED_origin_initialised = false;
 	bool _gps_speed_valid = false;
-        float _gps_speed_accuracy = 0.0f; // GPS receiver reported speed accuracy (m/s)
-        struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
+	float _gps_speed_accuracy = 0.0f; // GPS receiver reported speed accuracy (m/s)
+	struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
 
 	bool _mag_healthy = false;		// computed by mag innovation test
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio

EKF/mag_fusion.cpp

@@ -55,8 +55,8 @@ void Ekf::fuseMag()
 	float magD = _state.mag_I(2);
 
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-    float R_MAG = fmaxf(_params.mag_noise, 1.0e-3f);
-    R_MAG = R_MAG*R_MAG;
+	float R_MAG = fmaxf(_params.mag_noise, 1.0e-3f);
+	R_MAG = R_MAG * R_MAG;
 
 	// intermediate variables from algebraic optimisation
 	float SH_MAG[9];
@@ -472,8 +472,8 @@ void Ekf::fuseHeading()
 	float magY = _mag_sample_delayed.mag(1);
 	float magZ = _mag_sample_delayed.mag(2);
 
-    float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f);
-    R_mag = R_mag*R_mag;
+	float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f);
+	R_mag = R_mag * R_mag;
 
 	float t2 = q0 * q0;
 	float t3 = q1 * q1;