From 074e787a919338e1b1f2d4070877a89d008827e3 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 9 Mar 2026 16:32:36 +0100 Subject: [PATCH] feat(ekf2): remove zero innovation heading update This is no longer necessary with the heading observability check --- src/modules/ekf2/CMakeLists.txt | 1 - src/modules/ekf2/EKF/CMakeLists.txt | 1 - .../zero_innovation_heading_update.cpp | 69 ------------------- src/modules/ekf2/EKF/control.cpp | 2 - src/modules/ekf2/EKF/ekf.h | 2 - 5 files changed, 75 deletions(-) delete mode 100644 src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index c12d7657ab..16661da5b4 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -132,7 +132,6 @@ list(APPEND EKF_SRCS EKF/aid_sources/fake_pos_control.cpp EKF/aid_sources/ZeroGyroUpdate.cpp EKF/aid_sources/ZeroVelocityUpdate.cpp - EKF/aid_sources/zero_innovation_heading_update.cpp ) if(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 7ef54d5995..dd31388e57 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -53,7 +53,6 @@ list(APPEND EKF_SRCS aid_sources/fake_pos_control.cpp aid_sources/ZeroGyroUpdate.cpp aid_sources/ZeroVelocityUpdate.cpp - aid_sources/zero_innovation_heading_update.cpp ) if(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp deleted file mode 100644 index 400cd9f19c..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 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_innovation_heading_update.cpp - * Control function for ekf heading update when at rest or no other heading source available - */ - -#include "ekf.h" - -void Ekf::controlZeroInnovationHeadingUpdate() -{ - const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D - || _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw; - - // fuse zero innovation at a limited rate if the yaw variance is too large - if (!yaw_aiding - && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { - - // Use an observation variance larger than usual but small enough - // to constrain the yaw variance just below the threshold - const float obs_var = _control_status.flags.tilt_align ? 0.25f : 0.001f; - - estimator_aid_source1d_s aid_src_status{}; - aid_src_status.observation = getEulerYaw(_state.quat_nominal); - aid_src_status.observation_variance = obs_var; - aid_src_status.innovation = 0.f; - - VectorState H_YAW; - - computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); - - if (!_control_status.flags.tilt_align - || (aid_src_status.innovation_variance - obs_var) > sq(_params.ekf2_head_noise)) { - // The yaw variance is too large, fuse fake measurement - fuseYaw(aid_src_status, H_YAW); - } - } -} diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0b20db7768..0624cba6d9 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -154,8 +154,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) updateTerrainValidity(); #endif // CONFIG_EKF2_TERRAIN - controlZeroInnovationHeadingUpdate(); - _zero_velocity_update.update(*this, imu_delayed); if (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 0bd7c7b9a0..70e117bd1a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -951,8 +951,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroInnovationHeadingUpdate(); - #if defined(CONFIG_EKF2_AUXVEL) // control fusion of auxiliary velocity observations void controlAuxVelFusion(const imuSample &imu_sample);