ekf2: do not fuse ZVU if other velocity source is active

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -528,6 +528,17 @@ bool EstimatorInterface::isHorizontalAidingActive() const
 	return getNumberOfActiveHorizontalAidingSources() > 0;
 }
 
+bool EstimatorInterface::isVerticalVelocityAidingActive() const
+{
+	return getNumberOfActiveVerticalVelocityAidingSources() > 0;
+}
+
+int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
+{
+	return int(_control_status.flags.gps)
+	       + int(_control_status.flags.ev_vel);
+}
+
 void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 {
 	if (buffer_name) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -181,6 +181,9 @@ public:
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 
+	bool isVerticalVelocityAidingActive() const;
+	int getNumberOfActiveVerticalVelocityAidingSources() const;
+
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
 	// get the velocity of the body frame origin in local NED earth frame

src/modules/ekf2/EKF/zero_velocity_update.cpp

@@ -45,13 +45,16 @@ void Ekf::controlZeroVelocityUpdate()
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
-				&& _control_status_prev.flags.vehicle_at_rest;
+				&& _control_status_prev.flags.vehicle_at_rest
+				&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
 
 		if (continuing_conditions_passing) {
 			Vector3f vel_obs{0, 0, 0};
 			Vector3f innovation = _state.vel - vel_obs;
 
-			const float obs_var = sq(0.001f);
+			// Set a low variance initially for faster leveling and higher
+			// later to let the states follow the measurements
+			const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
 			Vector3f innov_var{
 				P(4, 4) + obs_var,
 				P(5, 5) + obs_var,