ekf2: move predict covariance IMU inhibit check to function

src/modules/ekf2/EKF/covariance.cpp

@@ -102,91 +102,12 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;
 
-	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
+	// delta angle noise variance
-	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
+	float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
-	const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
-	const float beta = 1.0f - alpha;
-	_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
-	_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
-	_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + 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;
-
-	// gyro bias inhibit
-	const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
-
-	for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
-		const unsigned stateIndex = State::gyro_bias.idx + index;
-
-		bool is_bias_observable = true;
-
-		// TODO: gyro bias conditions
-
-		const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
-
-		if (do_inhibit_axis) {
-			// store the bias state variances to be reinstated later
-			if (!_gyro_bias_inhibit[index]) {
-				_prev_gyro_bias_var(index) = P(stateIndex, stateIndex);
-				_gyro_bias_inhibit[index] = true;
-			}
-
-		} else {
-			if (_gyro_bias_inhibit[index]) {
-				// reinstate the bias state variances
-				P(stateIndex, stateIndex) = _prev_gyro_bias_var(index);
-				_gyro_bias_inhibit[index] = false;
-			}
-		}
-	}
-
-	// accel bias inhibit
-	const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
-					 || is_manoeuvre_level_high
-					 || _fault_status.flags.bad_acc_vertical;
-
-	for (unsigned index = 0; index < State::accel_bias.dof; index++) {
-		const unsigned stateIndex = State::accel_bias.idx + index;
-
-		bool is_bias_observable = true;
-
-		if (_control_status.flags.vehicle_at_rest) {
-			is_bias_observable = true;
-
-		} else if (_control_status.flags.fake_hgt) {
-			is_bias_observable = false;
-
-		} else if (_control_status.flags.fake_pos) {
-			// when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector
-			is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
-		}
-
-		const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
-
-		if (do_inhibit_axis) {
-			// store the bias state variances to be reinstated later
-			if (!_accel_bias_inhibit[index]) {
-				_prev_accel_bias_var(index) = P(stateIndex, stateIndex);
-				_accel_bias_inhibit[index] = true;
-			}
-
-		} else {
-			if (_accel_bias_inhibit[index]) {
-				// reinstate the bias state variances
-				P(stateIndex, stateIndex) = _prev_accel_bias_var(index);
-				_accel_bias_inhibit[index] = false;
-			}
-		}
-	}
-
-	// assign IMU noise variances
-	// inputs to the system are 3 delta angles and 3 delta velocities
-	float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
 	const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise);
 
-	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
+	// delta velocity noise variance
-
+	float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
 	Vector3f d_vel_var;
 
 	for (unsigned i = 0; i < 3; i++) {

src/modules/ekf2/EKF/ekf.cpp

@@ -195,6 +195,14 @@ bool Ekf::update()
 		// TODO: explicitly pop at desired time horizon
 		const imuSample imu_sample_delayed = _imu_buffer.get_oldest();
 
+		// calculate an average filter update time
+		//  filter and limit input between -50% and +100% of nominal value
+		float input = 0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt);
+		float filter_update_s = 1e-6f * _params.filter_update_interval_us;
+		_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
+
+		updateIMUBiasInhibit(imu_sample_delayed);
+
 		// perform state and covariance prediction for the main filter
 		predictCovariance(imu_sample_delayed);
 		predictState(imu_sample_delayed);
@@ -305,13 +313,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 
 	constrainStates();
 
-	// calculate an average filter update time
-	float input = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
-
-	// filter and limit input between -50% and +100% of nominal value
-	const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
-	input = math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
-	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
 
 	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
 	// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate

src/modules/ekf2/EKF/ekf.h

@@ -822,6 +822,8 @@ private:
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
 
+	void updateIMUBiasInhibit(const imuSample &imu_delayed);
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// ekf sequential fusion of magnetometer measurements
 	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1107,3 +1107,94 @@ void Ekf::resetWindCov()
 	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
 }
 #endif // CONFIG_EKF2_WIND
+
+void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
+{
+	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
+	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
+	{
+		const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
+		const float beta = 1.f - alpha;
+		_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
+	}
+
+	{
+		const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
+		const float beta = 1.f - alpha;
+
+		_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
+		_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + 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);
+
+	// gyro bias inhibit
+	const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
+
+	for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
+		const unsigned stateIndex = State::gyro_bias.idx + index;
+
+		bool is_bias_observable = true;
+
+		// TODO: gyro bias conditions
+
+		const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
+
+		if (do_inhibit_axis) {
+			// store the bias state variances to be reinstated later
+			if (!_gyro_bias_inhibit[index]) {
+				_prev_gyro_bias_var(index) = P(stateIndex, stateIndex);
+				_gyro_bias_inhibit[index] = true;
+			}
+
+		} else {
+			if (_gyro_bias_inhibit[index]) {
+				// reinstate the bias state variances
+				P(stateIndex, stateIndex) = _prev_gyro_bias_var(index);
+				_gyro_bias_inhibit[index] = false;
+			}
+		}
+	}
+
+	// accel bias inhibit
+	const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
+					 || is_manoeuvre_level_high
+					 || _fault_status.flags.bad_acc_vertical;
+
+	for (unsigned index = 0; index < State::accel_bias.dof; index++) {
+		const unsigned stateIndex = State::accel_bias.idx + index;
+
+		bool is_bias_observable = true;
+
+		if (_control_status.flags.vehicle_at_rest) {
+			is_bias_observable = true;
+
+		} else if (_control_status.flags.fake_hgt) {
+			is_bias_observable = false;
+
+		} else if (_control_status.flags.fake_pos) {
+			// when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector
+			is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
+		}
+
+		const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
+
+		if (do_inhibit_axis) {
+			// store the bias state variances to be reinstated later
+			if (!_accel_bias_inhibit[index]) {
+				_prev_accel_bias_var(index) = P(stateIndex, stateIndex);
+				_accel_bias_inhibit[index] = true;
+			}
+
+		} else {
+			if (_accel_bias_inhibit[index]) {
+				// reinstate the bias state variances
+				P(stateIndex, stateIndex) = _prev_accel_bias_var(index);
+				_accel_bias_inhibit[index] = false;
+			}
+		}
+	}
+
+}