Move variable declaration to better place &

remove airspeed_innov_var_temp variable. setting to global variable even when fusion is aborted is okay

EKF/airspeed_fusion.cpp

@@ -46,11 +46,6 @@
 
 void Ekf::fuseAirspeed()
 {
-	float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian
-	float H_TAS[24] = {}; // Observation Jacobian
-	float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector
-	Vector24f Kfusion; // Kalman gain vector
-
 	const float vn = _state.vel(0); // Velocity in north direction
 	const float ve = _state.vel(1); // Velocity in east direction
 	const float vd = _state.vel(2); // Velocity in downwards direction
@@ -69,25 +64,23 @@ void Ekf::fuseAirspeed()
 		// determine if we need the sideslip fusion to correct states other than wind
 		const bool update_wind_only = !_is_wind_dead_reckoning;
 
-		// Calculate the observation jacobian
 		// intermediate variable from algebraic optimisation
+		float SH_TAS[3];
 		SH_TAS[0] = 1.0f/v_tas_pred;
 		SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
 		SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
 
-		for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; }
+		// Observation Jacobian
-
+		float H_TAS[24] = {};
 		H_TAS[4] = SH_TAS[2];
 		H_TAS[5] = SH_TAS[1];
 		H_TAS[6] = vd*SH_TAS[0];
 		H_TAS[22] = -SH_TAS[2];
 		H_TAS[23] = -SH_TAS[1];
 
-		// We don't want to update the innovation variance if the calculation is ill conditioned
+		_airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
-		const float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
 
-		if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
+		if (_airspeed_innov_var >= R_TAS) { // Check for badly conditioned calculation
-			SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
 			_fault_status.flags.bad_airspeed = false;
 
 		} else { // Reset the estimator covariance matrix
@@ -110,8 +103,12 @@ void Ekf::fuseAirspeed()
 			return;
 		}
 
+		// Variable used to optimise calculations of the Kalman gain
+		float SK_TAS[2];
+		SK_TAS[0] = 1.0f / _airspeed_innov_var;
 		SK_TAS[1] = SH_TAS[1];
 
+		Vector24f Kfusion; // Kalman gain
 		if (!update_wind_only) {
 			// we have no other source of aiding, so use airspeed measurements to correct states
 			Kfusion(0) = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]);
@@ -144,9 +141,6 @@ void Ekf::fuseAirspeed()
 		_airspeed_innov = v_tas_pred -
 				  _airspeed_sample_delayed.true_airspeed;
 
-		// Calculate the innovation variance
-		_airspeed_innov_var = 1.0f / SK_TAS[0];
-
 		// Compute the ratio of innovation to gate size
 		_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);
 

EKF/control.cpp

@@ -1279,8 +1279,6 @@ void Ekf::controlFakePosFusion()
 		// Fuse synthetic position observations every 200msec
 		if (isTimedOut(_time_last_fake_pos, (uint64_t)2e5)) {
 
-			Vector3f fake_pos_obs_var;
-
 			// Reset position and velocity states if we re-commence this aiding method
 			if (isTimedOut(_time_last_fake_pos, (uint64_t)4e5)) {
 				resetHorizontalPosition();
@@ -1294,6 +1292,8 @@ void Ekf::controlFakePosFusion()
 			}
 			_time_last_fake_pos = _time_last_imu;
 
+			Vector3f fake_pos_obs_var;
+
 			if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
 				fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
 

EKF/ekf.cpp

@@ -251,7 +251,6 @@ void Ekf::predictState()
 {
 	// apply imu bias corrections
 	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
-	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
 
 	// subtract component of angular rate due to earth rotation
 	corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
@@ -259,17 +258,12 @@ void Ekf::predictState()
 	const Quatf dq(AxisAnglef{corrected_delta_ang});
 
 	// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
-	_state.quat_nominal = _state.quat_nominal * dq;
+	_state.quat_nominal = (_state.quat_nominal * dq).normalized();
-
-	// quaternions must be normalised whenever they are modified
-	_state.quat_nominal.normalize();
-
-	// save the previous value of velocity so we can use trapzoidal integration
-	const Vector3f vel_last = _state.vel;
 
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// Calculate an earth frame delta velocity
+	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
 	const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
 
 	// calculate a filtered horizontal acceleration with a 1 sec time constant
@@ -277,6 +271,8 @@ void Ekf::predictState()
 	float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
 	_accel_lpf_NE(0) = _accel_lpf_NE(0) * alpha + corrected_delta_vel_ef(0);
 	_accel_lpf_NE(1) = _accel_lpf_NE(1) * alpha + corrected_delta_vel_ef(1);
+	// save the previous value of velocity so we can use trapzoidal integration
+	const Vector3f vel_last = _state.vel;
 
 	// calculate the increment in velocity using the current orientation
 	_state.vel += corrected_delta_vel_ef;