fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Zero gyro update (#21691)

Browse Source 
When at rest, directly fuse the gyro data as an observation of its bias.
This allows to strongly observe the gyro biases without having to fuse a
constant heading that makes the ekf too confident about its heading.
This commit is contained in:
Mathieu Bresciani
2023-06-09 21:13:18 -04:00
committed by GitHub
parent c95539e8ce
commit 24de623989
13 changed files with 974 additions and 790 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+14 -43
View File
@@ -43,52 +43,23 @@ 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.gps_yaw;
if (!_control_status.flags.tilt_align) {
// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
float innovation = 0.f;
float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
_time_last_heading_fuse = 0;
_last_static_yaw = NAN;
// fuse zero innovation at a limited rate if the yaw variance is too large
if (_control_status.flags.tilt_align
&& !yaw_aiding
&& isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
} else if (_control_status.flags.vehicle_at_rest) {
// When at rest or no source of yaw aiding is active yaw fusion is run selectively to enable yaw gyro
// bias learning when stationary on ground and to prevent uncontrolled yaw variance growth
const float euler_yaw = getEulerYaw(_R_to_earth);
// Use an observation variance larger than usual but small enough
// to constrain the yaw variance just below the threshold
float obs_var = 0.25f;
estimator_aid_source1d_s aid_src_status;
Vector24f H_YAW;
if (PX4_ISFINITE(_last_static_yaw)) {
// fuse last static yaw at a limited rate (every 200 milliseconds)
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = wrap_pi(euler_yaw - _last_static_yaw);
float obs_var = 0.01f;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
} else {
// record static yaw when transitioning to at rest
_last_static_yaw = euler_yaw;
if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
// The yaw variance is too large, fuse fake measurement
float innovation = 0.f;
fuseYaw(innovation, obs_var, aid_src_status, H_YAW);
}
} else {
// vehicle moving and tilt alignment completed
// 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)) {
float obs_var = 0.25f;
estimator_aid_source1d_s aid_src_status;
Vector24f H_YAW;
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
// The yaw variance is too large, fuse fake measurement
float innovation = 0.f;
fuseYaw(innovation, obs_var, aid_src_status, H_YAW);
}
}
_last_static_yaw = NAN;
}
}