fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(ekf2): remove zero innovation heading update

Browse Source 
This is no longer necessary with the heading observability check
This commit is contained in:
bresch 2026-03-09 16:32:36 +01:00 committed by Mathieu Bresciani
parent 643c6fec24
commit 074e787a91
5 changed files with 0 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/CMakeLists.txt
View File

@ -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)

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -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)

69
src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp
View File

@ -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);
}
}
}

2
src/modules/ekf2/EKF/control.cpp
View File

@ -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<int32_t>(ImuCtrl::GyroBias)) {

2
src/modules/ekf2/EKF/ekf.h
View File

@ -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);