diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 1842774ea3..62191914a3 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/hover_thrust_estimate.msg b/msg/hover_thrust_estimate.msg new file mode 100644 index 0000000000..5391d047ca --- /dev/null +++ b/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 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index dd5396ccfc..614dda71be 100644 --- a/src/lib/CMakeLists.txt +++ b/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) diff --git a/src/lib/hover_thrust_estimator/CMakeLists.txt b/src/lib/hover_thrust_estimator/CMakeLists.txt new file mode 100644 index 0000000000..230a9d3b31 --- /dev/null +++ b/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 +) diff --git a/src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp b/src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp new file mode 100644 index 0000000000..253e5ed6f8 --- /dev/null +++ b/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 + */ + +#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); +} diff --git a/src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp b/src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp new file mode 100644 index 0000000000..2c4aaf6519 --- /dev/null +++ b/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 + */ + +#pragma once + +#include +#include +#include +#include + +#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) _param_hte_ht_noise, + (ParamFloat) _param_hte_acc_gate, + (ParamFloat) _param_hte_ht_err_init + ) + + uORB::Publication _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)}; +}; diff --git a/src/lib/hover_thrust_estimator/hover_thrust_estimator_params.c b/src/lib/hover_thrust_estimator/hover_thrust_estimator_params.c new file mode 100644 index 0000000000..597d46f4a6 --- /dev/null +++ b/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 + */ + +/** + * 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); diff --git a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp new file mode 100644 index 0000000000..69e669f901 --- /dev/null +++ b/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 + */ + +#include + +#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; +} diff --git a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp new file mode 100644 index 0000000000..4a63edad10 --- /dev/null +++ b/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 + */ + +#pragma once + +#include +#include + +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 +}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 4e95f94396..5848922dfb 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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"); diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index a6394aced8..09872cffa3 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -50,4 +50,5 @@ px4_add_module( WeatherVane CollisionPrevention Takeoff + HoverThrustEstimator ) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5bf1edea8b..9f7bcd7752 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -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);