diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 7d040abc33..dc29cac2f1 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -98,7 +98,6 @@ px4_add_module( EKF/gnss_height_control.cpp EKF/gps_checks.cpp EKF/gps_control.cpp - EKF/gps_fusion.cpp EKF/gps_yaw_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index ad14129159..1594e01fce 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -49,7 +49,6 @@ add_library(ecl_EKF gnss_height_control.cpp gps_checks.cpp gps_control.cpp - gps_fusion.cpp gps_yaw_fusion.cpp height_control.cpp imu_down_sampler.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index ec66a5470a..fd7f9442fd 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -429,6 +429,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi return; } + updateGpsYaw(gps_sample); + const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw); if (is_new_data_available) { @@ -447,7 +449,7 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi if (continuing_conditions_passing) { - fuseGpsYaw(gps_sample); + fuseGpsYaw(); const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f192069d92..836782a77a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -677,7 +677,7 @@ private: bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status); // fuse the yaw angle obtained from a dual antenna GPS unit - void fuseGpsYaw(const gpsSample &gps_sample); + void fuseGpsYaw(); // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit // return true if the reset was successful diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 66db64579b..e45d08123c 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -51,8 +51,6 @@ void Ekf::controlGpsFusion() const gpsSample &gps_sample{_gps_sample_delayed}; - updateGpsYaw(gps_sample); - const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp deleted file mode 100644 index 182e8b808f..0000000000 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021-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 gps_fusion.cpp - * Function for fusing gps measurements - */ - -/* #include */ -#include "ekf.h" - -void Ekf::updateGpsYaw(const gpsSample &gps_sample) -{ - auto &gps_yaw = _aid_src_gnss_yaw; - resetEstimatorAidStatus(gps_yaw); - - if (PX4_ISFINITE(gps_sample.yaw)) { - // initially populate for estimator_aid_src_gnss_yaw logging - - // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset); - - gps_yaw.observation = measured_hdg; - gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); - - // define the predicted antenna array vector and rotate into earth frame - const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; - const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; - const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0)); - gps_yaw.innovation = wrap_pi(predicted_hdg - gps_yaw.observation); - - gps_yaw.fusion_enabled = _control_status.flags.gps_yaw; - - gps_yaw.timestamp_sample = gps_sample.time_us; - } -} diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index b2847984aa..1873c06324 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -46,33 +46,67 @@ #include #include -void Ekf::fuseGpsYaw(const gpsSample& gps_sample) +void Ekf::updateGpsYaw(const gpsSample &gps_sample) { - // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset); + auto &gnss_yaw = _aid_src_gnss_yaw; + resetEstimatorAidStatus(gnss_yaw); - // using magnetic heading process noise - // TODO extend interface to use yaw uncertainty provided by GPS if available - const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + if (PX4_ISFINITE(gps_sample.yaw)) { + // initially populate for estimator_aid_src_gnss_yaw logging + + // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement + const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset); + + // using magnetic heading process noise + // TODO extend interface to use yaw uncertainty provided by GPS if available + const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + + float heading_innov; + float heading_innov_var; + + { + Vector24f H; + sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H); + } + + gnss_yaw.observation = measured_hdg; + gnss_yaw.observation_variance = R_YAW; + gnss_yaw.innovation = heading_innov; + gnss_yaw.innovation_variance = heading_innov_var; + + gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; + + gnss_yaw.timestamp_sample = gps_sample.time_us; + + const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); + setEstimatorAidStatusTestRatio(gnss_yaw, innov_gate); + } +} + +void Ekf::fuseGpsYaw() +{ + auto &gnss_yaw = _aid_src_gnss_yaw; + + if (gnss_yaw.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; + return; + } - float heading_innov; - float heading_innov_var; Vector24f H; - sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H); + { + float heading_innov; + float heading_innov_var; + + // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H + // making a separate function just for H uses more flash space without reducing CPU load significantly + sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation, gnss_yaw.observation_variance, FLT_EPSILON, &heading_innov, &heading_innov_var, &H); + } const SparseVector24f<0,1,2,3> Hfusion(H); - _aid_src_gnss_yaw.timestamp_sample = gps_sample.time_us; - _aid_src_gnss_yaw.observation = measured_hdg; - _aid_src_gnss_yaw.observation_variance = R_YAW; - _aid_src_gnss_yaw.innovation = heading_innov; - _aid_src_gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; - - _aid_src_gnss_yaw.innovation_variance = heading_innov_var; - // check if the innovation variance calculation is badly conditioned - if (heading_innov_var < R_YAW) { + if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_hdg = true; @@ -83,23 +117,12 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample) } _fault_status.flags.bad_hdg = false; + _innov_check_fail_status.flags.reject_yaw = false; - const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - - setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate); - - if (_aid_src_gnss_yaw.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; - return; - - } else { - _innov_check_fail_status.flags.reject_yaw = false; - } - - _gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(heading_innov) * _aid_src_gnss_yaw.test_ratio); + _gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(gnss_yaw.innovation) * gnss_yaw.test_ratio); if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f) - && !_control_status.flags.in_air && isTimedOut(_aid_src_gnss_yaw.time_last_fuse, (uint64_t)1e6)) { + && !_control_status.flags.in_air && isTimedOut(gnss_yaw.time_last_fuse, (uint64_t)1e6)) { // A constant large signed test ratio is a sign of wrong gyro bias // Reset the yaw gyro variance to converge faster and avoid @@ -109,15 +132,15 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample) // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion = P * Hfusion / _aid_src_gnss_yaw.innovation_variance; + Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, Hfusion, heading_innov); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; - _aid_src_gnss_yaw.fused = is_fused; + gnss_yaw.fused = is_fused; if (is_fused) { _time_last_heading_fuse = _imu_sample_delayed.time_us; - _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; + gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; } }