diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index e47d1f2f60..7fdf48dfaf 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -66,6 +66,7 @@ px4_add_module( EKF/mag_control.cpp EKF/mag_fusion.cpp EKF/optflow_fusion.cpp + EKF/range_finder_consistency_check.cpp EKF/sensor_range_finder.cpp EKF/sideslip_fusion.cpp EKF/terrain_estimator.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 745317d132..3fce773cb0 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -51,6 +51,7 @@ add_library(ecl_EKF mag_control.cpp mag_fusion.cpp optflow_fusion.cpp + range_finder_consistency_check.cpp sensor_range_finder.cpp sideslip_fusion.cpp terrain_estimator.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 7771b669d4..f36872f28c 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -140,6 +140,7 @@ void Ekf::controlFusionModes() const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), static_cast(_time_last_imu) * 1e-6f); } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index c22065be75..328c5fd537 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -63,6 +63,7 @@ #include "common.h" #include "RingBuffer.h" #include "imu_down_sampler.hpp" +#include "range_finder_consistency_check.hpp" #include "sensor_range_finder.hpp" #include "utils.hpp" @@ -297,6 +298,8 @@ protected: extVisionSample _ev_sample_delayed_prev{}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) + RangeFinderConsistencyCheck _rng_consistency_check; + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Sensor limitations diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp new file mode 100644 index 0000000000..37a27bfcfa --- /dev/null +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4. 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 range_finder_consistency_check.cpp + */ + +#include "range_finder_consistency_check.hpp" + +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, float time_s) +{ + const float dt = time_s - _time_last_update_s; + + if ((_time_last_update_s < FLT_EPSILON) + || (dt < 0.001f) || (dt > 0.5f)) { + _time_last_update_s = time_s; + _dist_bottom_prev = dist_bottom; + return; + } + + const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt; + const float innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down + const float vel_bottom_var = 2.f * dist_bottom_var / (dt * dt); + const float innov_var = vel_bottom_var + vz_var; + const float normalized_innov_sq = (innov * innov) / innov_var; + _vel_bottom_test_ratio = normalized_innov_sq / (_vel_bottom_gate * _vel_bottom_gate); + + _vel_bottom_signed_test_ratio_lpf.setParameters(dt, _vel_bottom_signed_test_ratio_tau); + const float signed_test_ratio = matrix::sign(innov) * normalized_innov_sq / (_vel_bottom_signed_gate * _vel_bottom_signed_gate); + _vel_bottom_signed_test_ratio_lpf.update(signed_test_ratio); + + _time_last_update_s = time_s; + _dist_bottom_prev = dist_bottom; +} diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp new file mode 100644 index 0000000000..f041d66dbd --- /dev/null +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4. 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 range_finder_consistency_check.hpp + * @brief Compute statistical tests of the range finder data + * using the estimated velocity as a reference in order to detect sensor faults + */ + +#pragma once + +#include + +class RangeFinderConsistencyCheck final +{ +public: + RangeFinderConsistencyCheck() = default; + ~RangeFinderConsistencyCheck() = default; + + void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, float time_s); + + float getTestRatio() const { return _vel_bottom_test_ratio; } + float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); } + bool isKinematicallyConsistent() const { return _vel_bottom_signed_test_ratio_lpf.getState() < 1.f; } + +private: + float _time_last_update_s{}; + float _dist_bottom_prev{}; + + float _vel_bottom_test_ratio{}; + AlphaFilter _vel_bottom_signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data + + static constexpr float _vel_bottom_signed_test_ratio_tau = 2.f; + static constexpr float _vel_bottom_gate = 3.f; + static constexpr float _vel_bottom_signed_gate = 0.1f; +};