ekf2: create simple estimator aid source base class and extract zero velocity update

src/modules/ekf2/CMakeLists.txt

@@ -131,7 +131,7 @@ list(APPEND EKF_SRCS
 	EKF/yaw_fusion.cpp
 	EKF/zero_innovation_heading_update.cpp
 	EKF/zero_gyro_update.cpp
-	EKF/zero_velocity_update.cpp
+	EKF/aid_sources/ZeroVelocityUpdate.cpp
 )
 
 if(CONFIG_EKF2_AIRSPEED)
@@ -220,6 +220,7 @@ px4_add_module(
 		#-O0
 	INCLUDES
 		EKF
+		EKF/aid_sources
 		${EKF_GENERATED_DERIVATION_INCLUDE_PATH}
 	PRIORITY
 		"SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads

src/modules/ekf2/EKF/CMakeLists.txt

@@ -48,7 +48,8 @@ list(APPEND EKF_SRCS
 	yaw_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_gyro_update.cpp
-	zero_velocity_update.cpp
+
+	aid_sources/ZeroVelocityUpdate.cpp
 )
 
 if(CONFIG_EKF2_AIRSPEED)

src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_ESTIMATOR_AID_SOURCE_HPP
+#define EKF_ESTIMATOR_AID_SOURCE_HPP
+
+#include <cstdint>
+
+#include <lib/matrix/matrix/math.hpp>
+
+// forward declarations
+class Ekf;
+
+namespace estimator
+{
+struct imuSample;
+};
+
+class EstimatorAidSource
+{
+public:
+	EstimatorAidSource() = default;
+	virtual ~EstimatorAidSource() = default;
+
+	virtual bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) = 0;
+
+	virtual void reset() = 0;
+
+private:
+
+
+};
+
+#endif // !EKF_ESTIMATOR_AID_SOURCE_HPP

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,37 +31,50 @@
  *
  ****************************************************************************/
 
-/**
- * @file zero_velocity_update.cpp
- * Control function for ekf zero velocity update
- */
+#include "ZeroVelocityUpdate.hpp"
 
-#include "ekf.h"
+#include "../ekf.h"
 
-void Ekf::controlZeroVelocityUpdate()
+ZeroVelocityUpdate::ZeroVelocityUpdate()
+{
+	reset();
+}
+
+void ZeroVelocityUpdate::reset()
+{
+	_time_last_zero_velocity_fuse = 0;
+}
+
+bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
 	// Fuse zero velocity at a limited rate (every 200 milliseconds)
-	const bool zero_velocity_update_data_ready = isTimedOut(_time_last_zero_velocity_fuse, (uint64_t)2e5);
+	const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us);
 
 	if (zero_velocity_update_data_ready) {
-		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
-				&& _control_status_prev.flags.vehicle_at_rest
-				&& (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift
+		const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest
+				&& ekf.control_status_prev_flags().vehicle_at_rest
+				&& (!ekf.isVerticalVelocityAidingActive()
+				    || !ekf.control_status_flags().tilt_align); // 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;
+			Vector3f vel_obs{0.f, 0.f, 0.f};
+			Vector3f innovation = ekf.state().vel - vel_obs;
 
 			// 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 = getVelocityVariance() + obs_var;
+
+			const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f);
+			Vector3f innov_var = ekf.getVelocityVariance() + obs_var;
 
 			for (unsigned i = 0; i < 3; i++) {
-				fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i);
+				ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i);
 			}
 
-			_time_last_zero_velocity_fuse = _time_delayed_us;
+			_time_last_zero_velocity_fuse = imu_delayed.time_us;
+
+			return true;
 		}
 	}
+
+	return false;
 }

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_ZERO_VELOCITY_UPDATE_HPP
+#define EKF_ZERO_VELOCITY_UPDATE_HPP
+
+#include "EstimatorAidSource.hpp"
+
+class ZeroVelocityUpdate : public EstimatorAidSource
+{
+public:
+	ZeroVelocityUpdate();
+	virtual ~ZeroVelocityUpdate() = default;
+
+	void reset() override;
+	bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;
+
+private:
+
+	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
+
+};
+
+#endif // !EKF_ZERO_VELOCITY_UPDATE_HPP

src/modules/ekf2/EKF/control.cpp

@@ -145,7 +145,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 	controlZeroInnovationHeadingUpdate();
 
-	controlZeroVelocityUpdate();
+	_zero_velocity_update.update(*this, imu_delayed);
+
 	controlZeroGyroUpdate(imu_delayed);
 
 	// Fake position measurement for constraining drift when no other velocity or position measurements

src/modules/ekf2/EKF/ekf.cpp

@@ -110,7 +110,6 @@ void Ekf::reset()
 	_time_last_hor_vel_fuse = 0;
 	_time_last_ver_vel_fuse = 0;
 	_time_last_heading_fuse = 0;
-	_time_last_zero_velocity_fuse = 0;
 
 	_last_known_pos.setZero();
 
@@ -175,6 +174,8 @@ void Ekf::reset()
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	resetEstimatorAidStatus(_aid_src_rng_hgt);
 #endif // CONFIG_EKF2_RANGE_FINDER
+
+	_zero_velocity_update.reset();
 }
 
 bool Ekf::update()

src/modules/ekf2/EKF/ekf.h

@@ -59,6 +59,8 @@
 #include <uORB/topics/estimator_aid_source2d.h>
 #include <uORB/topics/estimator_aid_source3d.h>
 
+#include "aid_sources/ZeroVelocityUpdate.hpp"
+
 enum class Likelihood { LOW, MEDIUM, HIGH };
 
 class Ekf final : public EstimatorInterface
@@ -83,6 +85,8 @@ public:
 
 	static uint8_t getNumberOfStates() { return State::size; }
 
+	const StateSample &state() const { return _state; }
+
 #if defined(CONFIG_EKF2_BAROMETER)
 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
 	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
@@ -317,6 +321,9 @@ public:
 #endif
 	}
 
+	// fuse single velocity and position measurement
+	bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
+
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
 	Vector3f getGyroBiasVariance() const { return getStateVariance<State::gyro_bias>(); } // get the gyroscope bias variance in rad/s
@@ -514,7 +521,6 @@ private:
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
 	uint64_t _time_last_heading_fuse{0};
-	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
@@ -774,9 +780,6 @@ private:
 	void fuseDrag(const dragSample &drag_sample);
 #endif // CONFIG_EKF2_DRAG_FUSION
 
-	// fuse single velocity and position measurement
-	bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
-
 	void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var);
 
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var);
@@ -1070,7 +1073,6 @@ private:
 	void resetHeightToLastKnown();
 	void stopFakeHgtFusion();
 
-	void controlZeroVelocityUpdate();
 	void controlZeroGyroUpdate(const imuSample &imu_delayed);
 	void fuseDeltaAngBias(float innov, float innov_var, int obs_index);
 
@@ -1239,6 +1241,8 @@ private:
 		// if any of the innovations are rejected, then the overall innovation is rejected
 		status.innovation_rejected = innovation_rejected;
 	}
+
+	ZeroVelocityUpdate _zero_velocity_update{};
 };
 
 #endif // !EKF_EKF_H