HoverThrustEstimator: add a new single state estimator

with measurement noise auto-tuning

The purpose of this estimator is to improve land detection and vertical
velocity feedforward

Recovery strategy:
This is required when the setpoint suddenly changes in air or that the
EKF is diverging. A lowpassed test ratio is used as a trigger for the recovery logic
Also, a lowpassed residual is used to estimate the steady-state value
and remove it when estimating the accel noise to avoid increasing the
accel noise when the redisual is caused by an offset.

msg/CMakeLists.txt

@@ -68,6 +68,7 @@ set(msg_files
 	gps_dump.msg
 	gps_inject_data.msg
 	home_position.msg
+	hover_thrust_estimate.msg
 	input_rc.msg
 	iridiumsbd_status.msg
 	irlock_report.msg

msg/hover_thrust_estimate.msg

@@ -0,0 +1,10 @@
+uint64 timestamp		# time since system start (microseconds)
+
+float32 hover_thrust		# estimated hover thrust [0.1, 0.9]
+float32 hover_thrust_var	# estimated hover thrust variance
+
+float32 accel_innov		# innovation of the last acceleration fusion
+float32 accel_innov_var		# innovation variance of the last acceleration fusion
+float32 accel_innov_test_ratio	# normalized innovation squared test ratio
+
+float32 accel_noise_var		# vertical acceleration noise variance estimated form innovation residual

src/lib/CMakeLists.txt

@@ -46,6 +46,7 @@ add_subdirectory(conversion)
 add_subdirectory(drivers)
 add_subdirectory(ecl)
 add_subdirectory(flight_tasks)
+add_subdirectory(hover_thrust_estimator)
 add_subdirectory(hysteresis)
 add_subdirectory(landing_slope)
 add_subdirectory(led)

src/lib/hover_thrust_estimator/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   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(HoverThrustEstimator
+	hover_thrust_estimator.cpp
+	zero_order_hover_thrust_ekf.cpp
+)

src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 "hover_thrust_estimator.hpp"
+
+void HoverThrustEstimator::reset()
+{
+	_thrust = 0.f;
+	_acc_z = 0.f;
+	_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
+	_hover_thrust_ekf.resetAccelNoise();
+
+}
+
+void HoverThrustEstimator::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 HoverThrustEstimator::update(const float dt)
+{
+	_hover_thrust_ekf.predict(dt);
+
+	ZeroOrderHoverThrustEkf::status status{};
+	_hover_thrust_ekf.fuseAccZ(_acc_z, _thrust, status);
+
+	publishStatus(status);
+}
+
+void HoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status)
+{
+	hover_thrust_estimate_s status_msg{};
+	status_msg.timestamp = hrt_absolute_time();
+	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;
+	_hover_thrust_ekf_pub.publish(status_msg);
+}

src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/hover_thrust_estimate.h>
+#include <drivers/drv_hrt.h>
+
+#include "zero_order_hover_thrust_ekf.hpp"
+
+class HoverThrustEstimator : public ModuleParams
+{
+public:
+	HoverThrustEstimator(ModuleParams *parent) :
+		ModuleParams(parent)
+	{
+		ZeroOrderHoverThrustEkf::status status{};
+		publishStatus(status);
+	}
+	~HoverThrustEstimator() = default;
+
+	void reset();
+
+	void update(float dt);
+
+	void setThrust(float thrust) { _thrust = thrust; };
+	void setAccel(float accel) { _acc_z = accel; };
+
+	float getHoverThrustEstimate() const { return _hover_thrust_ekf.getHoverThrustEstimate(); }
+
+protected:
+	void updateParams() override;
+
+private:
+	void publishStatus(ZeroOrderHoverThrustEkf::status &status);
+
+	ZeroOrderHoverThrustEkf _hover_thrust_ekf{};
+	float _acc_z{};
+	float _thrust{};
+
+	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
+	)
+
+	uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
+};

src/lib/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/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 <mathlib/mathlib.h>
+
+#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/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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>
+
+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 = hover_thrust; }
+	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; }
+
+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/logger/logged_topics.cpp

@@ -61,6 +61,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_sensor_bias", 1000);
 	add_topic("estimator_status", 200);
 	add_topic("home_position");
+	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 200);
 	add_topic("manual_control_setpoint", 200);
 	add_topic("mission");

src/modules/mc_pos_control/CMakeLists.txt

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

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -45,6 +45,7 @@
 #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>
@@ -183,6 +184,8 @@ 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 */
@@ -280,6 +283,7 @@ 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) {
@@ -590,6 +594,15 @@ 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])) {
@@ -636,6 +649,9 @@ 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);