move hover_thrust_estimator to new module (mc_hover_thrust_estimator)

* MC_HTE: unitialize with hover_thrust parameter
* MC_HTE: constrain hover thrust setter between 0.1 and 0.9
* MC_HTE: integrate with land detector and velocity controller
* MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.

* MC_HTE: use altitude agl threshold to start the estimator
local_position.z is relative to the origin of the EKF while dist_bottom
is above ground

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

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -76,6 +76,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.landSpeed      = param_find("MPC_LAND_SPEED");
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.minThrottle    = param_find("MPC_THR_MIN");
+	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle  = param_find("MPC_THR_HOVER");
 
 	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
@@ -93,6 +94,14 @@ void MulticopterLandDetector::_update_topics()
 	_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
 	_vehicle_control_mode_sub.update(&_vehicle_control_mode);
 	_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
+
+	if (_params.useHoverThrustEstimate) {
+		hover_thrust_estimate_s hte;
+
+		if (_hover_thrust_estimate_sub.update(&hte)) {
+			_params.hoverThrottle = hte.hover_thrust;
+		}
+	}
 }
 
 void MulticopterLandDetector::_update_params()
@@ -102,9 +111,16 @@ void MulticopterLandDetector::_update_params()
 	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get()));
 
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
-	param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 	param_get(_paramHandle.landSpeed, &_params.landSpeed);
+
+	int32_t use_hover_thrust_estimate = 0;
+	param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
+	_params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1);
+
+	if (!_params.useHoverThrustEstimate) {
+		param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
+	}
 }
 
 bool MulticopterLandDetector::_get_freefall_state()

src/modules/land_detector/MulticopterLandDetector.h

@@ -48,6 +48,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/hover_thrust_estimate.h>
 
 #include "LandDetector.h"
 
@@ -102,6 +103,7 @@ private:
 		param_t hoverThrottle;
 		param_t minManThrottle;
 		param_t landSpeed;
+		param_t useHoverThrustEstimate;
 	} _paramHandle{};
 
 	struct {
@@ -109,6 +111,7 @@ private:
 		float hoverThrottle;
 		float minManThrottle;
 		float landSpeed;
+		bool useHoverThrustEstimate;
 	} _params{};
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
@@ -118,6 +121,7 @@ private:
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 
 	actuator_controls_s               _actuator_controls {};
 	battery_status_s                  _battery_status {};

src/modules/mc_hover_thrust_estimator/CMakeLists.txt

@@ -0,0 +1,51 @@
+############################################################################
+#
+#   Copyright (c) 2020 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.
+#
+############################################################################
+
+px4_add_library(zero_order_hover_thrust_ekf
+	zero_order_hover_thrust_ekf.cpp
+	zero_order_hover_thrust_ekf.hpp
+)
+
+px4_add_module(
+	MODULE modules__mc_hover_thrust_estimator
+	MAIN mc_hover_thrust_estimator
+	SRCS
+		MulticopterHoverThrustEstimator.cpp
+		MulticopterHoverThrustEstimator.hpp
+	DEPENDS
+		mathlib
+		px4_work_queue
+		zero_order_hover_thrust_ekf
+)
+
+px4_add_unit_gtest(SRC zero_order_hover_thrust_ekf_test.cpp LINKLIBS zero_order_hover_thrust_ekf)

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hover_thrust_estimator.cpp
+ *
+ * @author Mathieu Bresciani 	<brescianimathieu@gmail.com>
+ */
+
+#include "MulticopterHoverThrustEstimator.hpp"
+
+#include <mathlib/mathlib.h>
+
+MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	updateParams();
+	reset();
+}
+
+MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator()
+{
+	perf_free(_cycle_perf);
+}
+
+bool MulticopterHoverThrustEstimator::init()
+{
+	if (!_vehicle_local_position_setpoint_sub.registerCallback()) {
+		PX4_ERR("vehicle_local_position_setpoint callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void MulticopterHoverThrustEstimator::reset()
+{
+	_hover_thrust_ekf.setHoverThrust(_param_mpc_thr_hover.get());
+	_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
+	_hover_thrust_ekf.resetAccelNoise();
+}
+
+void MulticopterHoverThrustEstimator::updateParams()
+{
+	const float ht_err_init_prev = _param_hte_ht_err_init.get();
+	ModuleParams::updateParams();
+
+	_hover_thrust_ekf.setProcessNoiseStdDev(_param_hte_ht_noise.get());
+
+	if (fabsf(_param_hte_ht_err_init.get() - ht_err_init_prev) > FLT_EPSILON) {
+		_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
+	}
+
+	_hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get());
+}
+
+void MulticopterHoverThrustEstimator::Run()
+{
+	if (should_exit()) {
+		_vehicle_local_position_setpoint_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+
+	perf_begin(_cycle_perf);
+
+	vehicle_land_detected_s vehicle_land_detected;
+	vehicle_status_s vehicle_status;
+	vehicle_local_position_s local_pos;
+
+	if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
+		_landed = vehicle_land_detected.landed;
+	}
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	}
+
+	if (_vehicle_local_pos_sub.update(&local_pos)) {
+		// This is only necessary because the landed
+		// flag of the land detector does not guarantee that
+		// the vehicle does not touch the ground anymore
+		// TODO: improve the landed flag
+		_in_air = local_pos.dist_bottom > 1.f;
+	}
+
+	ZeroOrderHoverThrustEkf::status status{};
+
+	if (_armed && !_landed && _in_air) {
+		vehicle_local_position_setpoint_s local_pos_sp;
+
+		if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
+
+			const hrt_abstime now = hrt_absolute_time();
+			const float dt = math::constrain((now - _timestamp_last) / 1e6f, 0.002f, 0.2f);
+
+			_hover_thrust_ekf.predict(dt);
+
+			if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) {
+				// Inform the hover thrust estimator about the measured vertical
+				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
+				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
+			}
+		}
+
+	} else {
+		if (!_armed) {
+			reset();
+		}
+
+		status.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate();
+		status.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar();
+		status.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar();
+		status.innov = NAN;
+		status.innov_var = NAN;
+		status.innov_test_ratio = NAN;
+	}
+
+	publishStatus(status);
+
+	perf_end(_cycle_perf);
+}
+
+void MulticopterHoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status)
+{
+	hover_thrust_estimate_s status_msg{};
+	status_msg.hover_thrust = status.hover_thrust;
+	status_msg.hover_thrust_var = status.hover_thrust_var;
+	status_msg.accel_innov = status.innov;
+	status_msg.accel_innov_var = status.innov_var;
+	status_msg.accel_innov_test_ratio = status.innov_test_ratio;
+	status_msg.accel_noise_var = status.accel_noise_var;
+	status_msg.timestamp = hrt_absolute_time();
+	_hover_thrust_ekf_pub.publish(status_msg);
+}
+
+int MulticopterHoverThrustEstimator::task_spawn(int argc, char *argv[])
+{
+	MulticopterHoverThrustEstimator *instance = new MulticopterHoverThrustEstimator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int MulticopterHoverThrustEstimator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int MulticopterHoverThrustEstimator::print_status()
+{
+	perf_print_counter(_cycle_perf);
+
+	return 0;
+}
+
+int MulticopterHoverThrustEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("mc_hover_thrust_estimator", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int mc_hover_thrust_estimator_main(int argc, char *argv[])
+{
+	return MulticopterHoverThrustEstimator::main(argc, argv);
+}

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hover_thrust_estimator.hpp
+ * @brief Interface class for a hover thrust estimator
+ * Convention is positive thrust, hover thrust and acceleration UP
+ *
+ * @author Mathieu Bresciani 	<brescianimathieu@gmail.com>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/hover_thrust_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include "zero_order_hover_thrust_ekf.hpp"
+
+class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrustEstimator>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	MulticopterHoverThrustEstimator();
+	~MulticopterHoverThrustEstimator() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	void Run() override;
+	void updateParams() override;
+
+	void reset();
+	void publishStatus(ZeroOrderHoverThrustEkf::status &status);
+
+	ZeroOrderHoverThrustEkf _hover_thrust_ekf{};
+
+	uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_setpoint_sub{this, ORB_ID(vehicle_local_position_setpoint)};
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_local_pos_sub{ORB_ID(vehicle_local_position)};
+
+	hrt_abstime _timestamp_last{0};
+
+	bool _armed{false};
+	bool _landed{false};
+	bool _in_air{false};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::HTE_HT_NOISE>) _param_hte_ht_noise,
+		(ParamFloat<px4::params::HTE_ACC_GATE>) _param_hte_acc_gate,
+		(ParamFloat<px4::params::HTE_HT_ERR_INIT>) _param_hte_ht_err_init,
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover
+	)
+};

src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c

@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hover_thrust_estimator_params.c
+ *
+ * Parameters used by the hover thrust estimator
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ */
+
+/**
+ * Hover thrust process noise
+ *
+ * Reduce to make the hover thrust estimate
+ * more stable, increase if the real hover thrust
+ * is expected to change quickly over time.
+ *
+ * @decimal 4
+ * @min 0.0001
+ * @max 1.0
+ * @unit normalized_thrust/s
+ * @group Hover Thrust Estimator
+ */
+PARAM_DEFINE_FLOAT(HTE_HT_NOISE, 0.0005);
+
+/**
+ * Gate size for acceleration fusion
+ *
+ * Sets the number of standard deviations used
+ * by the innovation consistency test.
+ *
+ * @decimal 1
+ * @min 1.0
+ * @max 10.0
+ * @unit SD
+ * @group Hover Thrust Estimator
+ */
+PARAM_DEFINE_FLOAT(HTE_ACC_GATE, 3.0);
+
+/**
+ * 1-sigma initial hover thrust uncertainty
+ *
+ * Sets the number of standard deviations used
+ * by the innovation consistency test.
+ *
+ * @decimal 3
+ * @min 0.0
+ * @max 1.0
+ * @unit normalized_thrust
+ * @group Hover Thrust Estimator
+ */
+PARAM_DEFINE_FLOAT(HTE_HT_ERR_INIT, 0.1);

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp

@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file zero_order_thrust_ekf.cpp
+ *
+ * @author Mathieu Bresciani 	<brescianimathieu@gmail.com>
+ */
+
+#include "zero_order_hover_thrust_ekf.hpp"
+
+void ZeroOrderHoverThrustEkf::predict(const float dt)
+{
+	// State is constant
+	// Predict state covariance only
+	_state_var += _process_var * dt;
+	_dt = dt;
+}
+
+void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, status &status_return)
+{
+	const float H = computeH(thrust);
+	const float innov_var = computeInnovVar(H);
+	const float innov = computeInnov(acc_z, thrust);
+	const float K = computeKalmanGain(H, innov_var);
+	const float innov_test_ratio = computeInnovTestRatio(innov, innov_var);
+
+	float residual = innov;
+
+	if (isTestRatioPassing(innov_test_ratio)) {
+		updateState(K, innov);
+		updateStateCovariance(K, H);
+		residual = computeInnov(acc_z, thrust); // residual != innovation since the hover thrust changed
+		updateMeasurementNoise(residual, H);
+
+	} else if (!isTestRatioPassing(_innov_test_ratio_lpf)) {
+		// Rejecting all the measurements for some time,
+		// it means that the hover thrust suddenly changed or that the EKF
+		// is diverging. To recover, we bump the state variance and reset the accel noise.
+		bumpStateVariance();
+		resetAccelNoise();
+	}
+
+	updateLpf(residual, innov_test_ratio);
+
+	status_return = packStatus(innov, innov_var, innov_test_ratio);
+}
+
+inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const
+{
+	return -CONSTANTS_ONE_G * thrust / (_hover_thr * _hover_thr);
+}
+
+inline float ZeroOrderHoverThrustEkf::computeInnovVar(const float H) const
+{
+	const float R = _acc_var;
+	const float P = _state_var;
+	return math::max(H * P * H + R, R);
+}
+
+float ZeroOrderHoverThrustEkf::computeInnov(const float acc_z, const float thrust) const
+{
+	const float predicted_acc_z = computePredictedAccZ(thrust);
+	return acc_z - predicted_acc_z;
+}
+
+float ZeroOrderHoverThrustEkf::computePredictedAccZ(const float thrust) const
+{
+	return CONSTANTS_ONE_G * thrust / _hover_thr - CONSTANTS_ONE_G;
+}
+
+inline float ZeroOrderHoverThrustEkf::computeKalmanGain(const float H, const float innov_var) const
+{
+	return _state_var * H / innov_var;
+}
+
+inline float ZeroOrderHoverThrustEkf::computeInnovTestRatio(const float innov, const float innov_var) const
+{
+	return innov * innov / (_gate_size * _gate_size * innov_var);
+}
+
+inline bool ZeroOrderHoverThrustEkf::isTestRatioPassing(const float innov_test_ratio) const
+{
+	return innov_test_ratio < 1.f;
+}
+
+inline void ZeroOrderHoverThrustEkf::updateState(const float K, const float innov)
+{
+	_hover_thr = math::constrain(_hover_thr + K * innov, 0.1f, 0.9f);
+}
+
+inline void ZeroOrderHoverThrustEkf::updateStateCovariance(const float K, const float H)
+{
+	_state_var = math::constrain((1.f - K * H) * _state_var, 1e-10f, 1.f);
+}
+
+inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual, const float H)
+{
+	const float alpha = _dt / (noise_learning_time_constant + _dt);
+	const float res_no_bias = residual - _residual_lpf;
+	const float P = _state_var;
+	_acc_var = math::constrain((1.f - alpha) * _acc_var  + alpha * (res_no_bias * res_no_bias + H * P * H), 1e-4f, 60.f);
+}
+
+inline void ZeroOrderHoverThrustEkf::bumpStateVariance()
+{
+	_state_var += 10000.f * _process_var * _dt;
+}
+
+inline void ZeroOrderHoverThrustEkf::updateLpf(const float residual, const float innov_test_ratio)
+{
+	const float alpha = _dt / (5.f * noise_learning_time_constant + _dt);
+	_residual_lpf = (1.f - alpha) * _residual_lpf + alpha * residual;
+	_innov_test_ratio_lpf = (1.f - alpha) * _innov_test_ratio_lpf + alpha * math::min(innov_test_ratio, 5.f);
+}
+
+inline ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkf::packStatus(const float innov, const float innov_var,
+		const float innov_test_ratio) const
+{
+	// Send back status for logging
+	status ret{};
+	ret.hover_thrust = _hover_thr;
+	ret.hover_thrust_var = _state_var;
+	ret.innov = innov;
+	ret.innov_var = innov_var;
+	ret.innov_test_ratio = innov_test_ratio;
+	ret.accel_noise_var = _acc_var;
+
+	return ret;
+}

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file zero_order_thrust_ekf.hpp
+ * @brief Implementation of a single-state hover thrust estimator
+ *
+ * state: hover thrust (Th)
+ * Vertical acceleration is used as a measurement and the current
+ * thrust (T[k]) is used in the measurement model.
+ *
+ * The state is noise driven: Transition matrix A = 1
+ * x[k+1] = Ax[k] + v with v ~ N(0, Q)
+ * y[k] = h(u, x) + w with w ~ N(0, R)
+ *
+ * Where the measurement model and corresponding partial derivative (w.r.t. Th) are:
+ * h(u, x)[k] = g * T[k] / Th[k] - g
+ * H[k] = -g * T[k] / Th[k]**2
+ *
+ * The measurmement noise variance R is continuously estimated using the residual after each
+ * measurement update with the following equation:
+ * R = (1 - alpha) * R + alpha * (z^2 + P * H^2)
+ * Where z is the residual and alpha a forgetting factor between 0 and 1
+ * (see S.Akhlaghi et al., Adaptive adjustment of noise covariance in Kalman filter for dynamic state estimation, 2017)
+ *
+ * During the measurment update step, the Normalized Innovation Squared (NIS) is checked
+ * and the measurement is rejected if larger than the gate size.
+ * A recovery logic is triggered when too many measurements are continuously rejected and
+ * consists of a measurement variance reset and a state variance boost.
+ *
+ * @author Mathieu Bresciani 	<brescianimathieu@gmail.com>
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <ecl/geo/geo.h>
+#include <mathlib/mathlib.h>
+
+class ZeroOrderHoverThrustEkf
+{
+public:
+	struct status {
+		float hover_thrust;
+		float hover_thrust_var;
+		float innov;
+		float innov_var;
+		float innov_test_ratio;
+		float accel_noise_var;
+	};
+
+	ZeroOrderHoverThrustEkf() = default;
+	~ZeroOrderHoverThrustEkf() = default;
+
+	void resetAccelNoise() { _acc_var = 5.f; };
+
+	void predict(float _dt);
+	void fuseAccZ(float acc_z, float thrust, status &status_return);
+
+	void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
+	void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
+	void setMeasurementNoiseStdDev(float measurement_noise) { _acc_var = measurement_noise * measurement_noise; }
+	void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; }
+	void setAccelInnovGate(float gate_size) { _gate_size = gate_size; }
+
+	float getHoverThrustEstimate() const { return _hover_thr; }
+	float getHoverThrustEstimateVar() const { return _state_var; }
+	float getAccelNoiseVar() const { return _acc_var; }
+
+private:
+	float _hover_thr{0.5f};
+
+	float _gate_size{3.f};
+	float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2)
+	float _process_var{0.25e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2)
+	float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
+	float _dt{0.02f};
+
+	float _residual_lpf{}; ///< used to remove the constant bias of the residual
+	float _innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic
+
+	float computeH(float thrust) const;
+	float computeInnovVar(float H) const;
+	float computePredictedAccZ(float thrust) const;
+	float computeInnov(float acc_z, float thrust) const;
+	float computeKalmanGain(float H, float innov_var) const;
+
+	/*
+	 * Compute the ratio between the Normalized Innovation Squared (NIS)
+	 * and its maximum gate size. Use isTestRatioPassing to know if the
+	 * measurement should be fused or not.
+	 */
+	float computeInnovTestRatio(float innov, float innov_var) const;
+	bool isTestRatioPassing(float innov_test_ratio) const;
+
+	void updateState(float K, float innov);
+	void updateStateCovariance(float K, float H);
+	void updateMeasurementNoise(float residual, float H);
+
+	void bumpStateVariance();
+	void updateLpf(float residual, float innov_test_ratio);
+
+	status packStatus(float innov, float innov_var, float innov_test_ratio) const;
+
+	static constexpr float noise_learning_time_constant = 0.5f; ///< in seconds
+};

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp

@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Zero Order Hover Thrust Estimator
+ * Run this test only using make tests TESTFILTER=zero_order_hover_thrust_ekf
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include <random>
+
+#include "zero_order_hover_thrust_ekf.hpp"
+
+using namespace matrix;
+
+class ZeroOrderHoverThrustEkfTest : public ::testing::Test
+{
+public:
+	ZeroOrderHoverThrustEkfTest()
+	{
+		_random_generator.seed(42);
+	}
+	float computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust);
+	ZeroOrderHoverThrustEkf::status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
+					       float thr_noise = 0.f);
+
+private:
+	ZeroOrderHoverThrustEkf _ekf{};
+	static constexpr float _dt = 0.02f;
+
+	std::normal_distribution<float> _standard_normal_distribution;
+	std::default_random_engine _random_generator; // Pseudo-random generator with constant seed
+};
+
+float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust)
+{
+	return CONSTANTS_ONE_G * thrust / hover_thrust - CONSTANTS_ONE_G;
+}
+
+ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust, float time,
+		float accel_noise, float thr_noise)
+{
+	ZeroOrderHoverThrustEkf::status status{};
+
+	for (float t = 0.f; t <= time; t += _dt) {
+		_ekf.predict(_dt);
+		float noisy_thrust =  thrust + thr_noise * _standard_normal_distribution(_random_generator);
+		float accel_theory = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true);
+		float noisy_accel =  accel_theory + accel_noise * _standard_normal_distribution(_random_generator);
+		_ekf.fuseAccZ(noisy_accel, noisy_thrust, status);
+	}
+
+	return status;
+}
+
+TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase)
+{
+	// GIVEN: a vehicle at hover, (the estimator starting at the true value)
+	const float thrust = 0.5f;
+	const float hover_thrust_true = 0.5f;
+
+	// WHEN: we input noiseless data and run the filter
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 1.f);
+
+	// THEN: The estimate should not move and its variance decrease quickly
+	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f);
+	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
+	EXPECT_NEAR(status.accel_noise_var, 0.f, 1.f); // The noise learning is slow and takes more than 1s to go to zero
+}
+
+TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence)
+{
+	// GIVEN: a vehicle at hover, but the estimator is starting at hover_thrust = 0.5
+	const float thrust = 0.72f;
+	const float hover_thrust_true = 0.72f;
+
+	// WHEN: we input noiseless data and run the filter
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);
+
+	// THEN: the state should converge to the true value and its variance decrease
+	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f);
+	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
+	EXPECT_NEAR(status.accel_noise_var, 0.f, 1.f); // The noise learning is slow and takes more than 1s to go to zero
+}
+
+TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise)
+{
+	// GIVEN: a vehicle at hover, the estimator starts with the wrong estimate and the measurements are noisy
+	const float sigma_noise = 3.f;
+	const float noise_var = sigma_noise * sigma_noise;
+	const float thrust = 0.72f;
+	const float hover_thrust_true = 0.72f;
+	const float t_sim = 10.f;
+
+	// WHEN: we input noisy accel data and run the filter
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
+
+	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
+	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
+	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
+	EXPECT_NEAR(status.accel_noise_var, noise_var, 0.2f * noise_var);
+}
+
+TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias)
+{
+	// GIVEN: a vehicle descending, the estimator starts with the wrong estimate, the measurements are really noisy
+	const float sigma_noise = 7.f;
+	const float noise_var = sigma_noise * sigma_noise;
+	const float thrust = 0.4f; // Below hover thrust
+	const float hover_thrust_true = 0.72f;
+	const float t_sim = 15.f;
+
+	// WHEN: we input noisy accel data and run the filter
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
+
+	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
+	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 7e-2);
+	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
+	EXPECT_NEAR(status.accel_noise_var, noise_var, 0.2f * noise_var);
+}
+
+TEST_F(ZeroOrderHoverThrustEkfTest, testThrustAndAccelNoise)
+{
+	// GIVEN: a vehicle climbing, the estimator starts with the wrong estimate, the measurements
+	// and the input thrust are noisy
+	const float accel_noise = 2.f;
+	const float accel_var = accel_noise * accel_noise;
+	const float thr_noise = 0.01f;
+	const float thrust = 0.72f; // Above hover thrust
+	const float hover_thrust_true = 0.6f;
+	const float t_sim = 15.f;
+
+	// WHEN: we input noisy accel and thrust data, and run the filter
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+
+	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
+	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
+	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
+	// Because of the nonlinear measurment model and the thust noise, the accel noise estimation is a bit worse
+	EXPECT_NEAR(status.accel_noise_var, accel_var, 0.4f * accel_var);
+}
+
+TEST_F(ZeroOrderHoverThrustEkfTest, testHoverThrustJump)
+{
+	// GIVEN: a vehicle hovering, the estimator starts with the wrong estimate, the measurements
+	// and the input thrust are noisy
+	const float accel_noise = 2.f;
+	const float accel_var = accel_noise * accel_noise;
+	const float thr_noise = 0.01f;
+	float thrust = 0.8; // At hover
+	float hover_thrust_true = 0.8f;
+	float t_sim = 10.f;
+
+	// WHEN: we input noisy accel and thrust data, and run the filter
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+	// THEN: change the hover thrust and the current thrust (the velocity controller responds quickly)
+	// Note that this is an extreme jump in hover thrust
+	thrust = 0.3;
+	hover_thrust_true = 0.3f;
+	t_sim = 10.f;
+	status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+
+	// THEN: the estimate should converge to the new hover thrust and the accel noise variance should
+	// be close to the true noise value
+	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
+	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
+	// After a recovery, the noise variance estimate takes more time to converge back to the true value
+	EXPECT_NEAR(status.accel_noise_var, accel_var, 2.f * accel_var);
+}

src/modules/mc_pos_control/CMakeLists.txt

@@ -50,5 +50,4 @@ px4_add_module(
 		WeatherVane
 		CollisionPrevention
 		Takeoff
-		HoverThrustEstimator
 	)

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -45,7 +45,6 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/systemlib/mavlink_log.h>
 #include <lib/weather_vane/WeatherVane.hpp>
-#include <lib/hover_thrust_estimator/hover_thrust_estimator.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -67,6 +66,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/hover_thrust_estimate.h>
 
 #include "PositionControl/PositionControl.hpp"
 #include "Takeoff/Takeoff.hpp"
@@ -122,6 +122,7 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< notification of parameter updates */
 	uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};				/**< vehicle attitude */
 	uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; 			/**< home position */
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 
 	hrt_abstime	_time_stamp_last_loop{0};		/**< time stamp of last loop iteration */
 
@@ -159,6 +160,7 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
 		(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
 		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
+		(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
 
 		// Takeoff / Land
 		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
@@ -184,8 +186,6 @@ private:
 	PositionControl _control; /**< class for core PID position control */
 	PositionControlStates _states{}; /**< structure containing vehicle state information for position control */
 
-	HoverThrustEstimator _hover_thrust_estimator;
-
 	hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
 
 	bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */
@@ -283,7 +283,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
 	_vel_x_deriv(this, "VELD"),
 	_vel_y_deriv(this, "VELD"),
 	_vel_z_deriv(this, "VELD"),
-	_hover_thrust_estimator(this),
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time"))
 {
 	if (vtol) {
@@ -351,7 +350,6 @@ MulticopterPositionControl::parameters_update(bool force)
 		_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
 		_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
 		_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
-		_control.setHoverThrust(_param_mpc_thr_hover.get());
 
 		// Check that the design parameters are inside the absolute maximum constraints
 		if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
@@ -366,12 +364,16 @@ MulticopterPositionControl::parameters_update(bool force)
 			mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed");
 		}
 
-		if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
-		    _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
-			_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
-						 _param_mpc_thr_max.get()));
-			_param_mpc_thr_hover.commit();
-			mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
+		if (!_param_mpc_use_hte.get()) {
+			if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
+			    _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
+				_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
+							 _param_mpc_thr_max.get()));
+				_param_mpc_thr_hover.commit();
+				mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
+			}
+
+			_control.setHoverThrust(_param_mpc_thr_hover.get());
 		}
 
 		_flight_tasks.handleParameterUpdate();
@@ -408,6 +410,14 @@ MulticopterPositionControl::poll_subscriptions()
 			_states.yaw = Eulerf(Quatf(att.q)).psi();
 		}
 	}
+
+	if (_param_mpc_use_hte.get()) {
+		hover_thrust_estimate_s hte;
+
+		if (_hover_thrust_estimate_sub.update(&hte)) {
+			_control.setHoverThrust(hte.hover_thrust);
+		}
+	}
 }
 
 void
@@ -594,15 +604,6 @@ MulticopterPositionControl::Run()
 				_control.resetIntegral();
 				// reactivate the task which will reset the setpoint to current state
 				_flight_tasks.reActivate();
-				_hover_thrust_estimator.reset();
-
-			} else if (_takeoff.getTakeoffState() >= TakeoffState::flight) {
-				// Inform the hover thrust estimator about the measured vertical acceleration (positive acceleration is up)
-				if (PX4_ISFINITE(_local_pos.az)) {
-					_hover_thrust_estimator.setAccel(-_local_pos.az);
-				}
-
-				_hover_thrust_estimator.update(_dt);
 			}
 
 			if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
@@ -649,9 +650,6 @@ MulticopterPositionControl::Run()
 			_flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz),
 					Vector3f(local_pos_sp.thrust));
 
-			// Inform the hover thrust estimator about the current thrust (positive thrust is up)
-			_hover_thrust_estimator.setThrust(-local_pos_sp.thrust[2]);
-
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
 			_control.getAttitudeSetpoint(attitude_setpoint);

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -73,6 +73,18 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
  */
 PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
 
+/**
+ * Hover thrust source selector
+ *
+ * Set false to use the fixed parameter MPC_THR_HOVER
+ * (EXPERIMENTAL) Set true to use the value computed by the hover thrust estimator
+ *
+ * @boolean
+ * @category Developer
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(MPC_USE_HTE, 0);
+
 /**
  * Thrust curve in Manual Mode
  *