ekf2: improve attitude estimation without horizontal aiding

- fake_pos only if at rest or tilt variances becomes large
 - fake pos: don't run when grav fusion is enabled
 - gravity fusion enabled by default
 - gravity: only fuse when accel norm and lpf norm are consistent

Co-authored-by: bresch <brescianimathieu@gmail.com>

src/modules/ekf2/EKF/control.cpp

@@ -68,11 +68,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	// monitor the tilt alignment
 	if (!_control_status.flags.tilt_align) {
 		// whilst we are aligning the tilt, monitor the variances
-		const Vector3f angle_err_var_vec = getQuaternionVariance();
-
-		// Once the tilt variances have reduced to equivalent of 3deg uncertainty
+		// Once the tilt variances have reduced to equivalent of 3 deg uncertainty
 		// and declare the tilt alignment complete
-		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
+		if (getTiltVariance() < sq(math::radians(3.f))) {
 			_control_status.flags.tilt_align = true;
 
 			// send alignment status message to the console

src/modules/ekf2/EKF/ekf.h

@@ -255,7 +255,10 @@ public:
 	matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
 
 	matrix::Vector<float, State::quat_nominal.dof> getQuaternionVariance() const { return getStateVariance<State::quat_nominal>(); }
+	float getTiltVariance() const { return getStateVariance<State::quat_nominal>()(0) + getStateVariance<State::quat_nominal>()(1); };
+
 	Vector3f getVelocityVariance() const { return getStateVariance<State::vel>(); };
+
 	Vector3f getPositionVariance() const { return getStateVariance<State::pos>(); }
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -43,7 +43,7 @@ void Ekf::controlFakePosFusion()
 	auto &aid_src = _aid_src_fake_pos;
 
 	// If we aren't doing any aiding, fake position measurements at the last known position to constrain drift
-	// During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF
+	// During initial tilt alignment, fake position is used to perform a "quasi-stationary" leveling of the EKF
 	const bool fake_pos_data_ready = !isHorizontalAidingActive()
 					 && isTimedOut(aid_src.time_last_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
 
@@ -68,7 +68,10 @@ void Ekf::controlFakePosFusion()
 		updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src);
 
 
-		const bool continuing_conditions_passing = !isHorizontalAidingActive();
+		const bool continuing_conditions_passing = !isHorizontalAidingActive()
+							   && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
+							   && (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest);
+
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _horizontal_deadreckon_time_exceeded;
 
@@ -77,8 +80,8 @@ void Ekf::controlFakePosFusion()
 
 				// always protect against extreme values that could result in a NaN
 				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
-				&& (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
-				) {
+				    && (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
+				   ) {
 					fuseHorizontalPosition(aid_src);
 				}
 

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -46,16 +46,21 @@
 
 void Ekf::controlGravityFusion(const imuSample &imu)
 {
-	// fuse gravity observation if our overall acceleration isn't too big
-	const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G;
-
-	_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
-					       && (((gravity_scale >= 0.9f && gravity_scale <= 1.1f)) || _control_status.flags.vehicle_at_rest)
-					       && !isHorizontalAidingActive();
-
 	// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
-	const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias();
-	const float measurement_var = sq(_params.gravity_noise);
+	const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - _state.accel_bias;
+	const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f));
+
+	const float accel_lpf_norm_sq = _accel_vec_filt.norm_squared();
+	const float accel_norm_sq = measurement.norm_squared();
+	const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
+	const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
+	const bool accel_lpf_norm_good = (accel_lpf_norm_sq > sq(lower_accel_limit)) && (accel_lpf_norm_sq < sq(upper_accel_limit));
+	const bool accel_norm_good = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
+
+	// fuse gravity observation if our overall acceleration isn't too big
+	_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
+					       && ((accel_lpf_norm_good && accel_norm_good) || _control_status.flags.vehicle_at_rest)
+					       && !isHorizontalAidingActive();
 
 	// calculate kalman gains and innovation variances
 	Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2)