dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: remove aid src status fusion_enabled flag

This commit is contained in:
Daniel Agar 2023-09-25 23:39:27 -04:00
parent 99197919d7
commit 14a967e2ca
27 changed files with 105 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/EstimatorAidSource1d.msg
View File

@ -14,7 +14,6 @@ float32 innovation
float32 innovation_variance
float32 test_ratio
bool fusion_enabled # true when measurements are being fused
bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused

1
msg/EstimatorAidSource2d.msg
View File

@ -14,7 +14,6 @@ float32[2] innovation
float32[2] innovation_variance
float32[2] test_ratio
bool fusion_enabled # true when measurements are being fused
bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused

1
msg/EstimatorAidSource3d.msg
View File

@ -14,7 +14,6 @@ float32[3] innovation
float32[3] innovation_variance
float32[3] test_ratio
bool fusion_enabled # true when measurements are being fused
bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused

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

@ -151,8 +151,6 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
aid_src.innovation = innov;
aid_src.innovation_variance = innov_var;
aid_src.fusion_enabled = _control_status.flags.fuse_aspd;
aid_src.timestamp_sample = airspeed_sample.time_us;
const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f);

1
src/modules/ekf2/EKF/auxvel_fusion.cpp
View File

@ -45,7 +45,6 @@ void Ekf::controlAuxVelFusion()
updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
if (isHorizontalAidingActive()) {
_aid_src_aux_vel.fusion_enabled = true;
fuseVelocity(_aid_src_aux_vel);
}
}

1
src/modules/ekf2/EKF/baro_height_control.cpp
View File

@ -121,7 +121,6 @@ void Ekf::controlBaroHeightFusion()
&& isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
if (_control_status.flags.baro_hgt) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {

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

@ -1241,7 +1241,6 @@ private:
status.innovation_variance = 0;
status.test_ratio = INFINITY;
status.fusion_enabled = false;
status.innovation_rejected = true;
status.fused = false;
}
@ -1265,7 +1264,6 @@ private:
status.test_ratio[i] = INFINITY;
}
status.fusion_enabled = false;
status.innovation_rejected = true;
status.fused = false;
}

3
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -104,10 +104,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
&& continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
if (ev_reset) {
if (quality_sufficient) {

1
src/modules/ekf2/EKF/ev_pos_control.cpp
View File

@ -175,7 +175,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
&& continuing_conditions_passing;
if (_control_status.flags.ev_pos) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive());

1
src/modules/ekf2/EKF/ev_vel_control.cpp
View File

@ -138,7 +138,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
if (_control_status.flags.ev_vel) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {

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

@ -76,8 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
&& isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6);
if (_control_status.flags.ev_yaw) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
if (ev_reset) {

6
src/modules/ekf2/EKF/fake_height_control.cpp
View File

@ -63,9 +63,9 @@ void Ekf::controlFakeHgtFusion()
if (continuing_conditions_passing) {
// always protect against extreme values that could result in a NaN
aid_src.fusion_enabled = aid_src.test_ratio < sq(100.0f / innov_gate);
fuseVerticalPosition(aid_src);
if (aid_src.test_ratio < sq(100.0f / innov_gate)) {
fuseVerticalPosition(aid_src);
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);

9
src/modules/ekf2/EKF/fake_pos_control.cpp
View File

@ -76,10 +76,11 @@ void Ekf::controlFakePosFusion()
if (continuing_conditions_passing) {
// always protect against extreme values that could result in a NaN
aid_src.fusion_enabled = (aid_src.test_ratio[0] < sq(100.0f / innov_gate))
&& (aid_src.test_ratio[1] < sq(100.0f / innov_gate));
fuseHorizontalPosition(aid_src);
if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
&& (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
) {
fuseHorizontalPosition(aid_src);
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);

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

@ -98,8 +98,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
&& !gps_checks_failing;
if (_control_status.flags.gps_hgt) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
fuseVerticalPosition(aid_src);

53
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -95,7 +95,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
_aid_src_gnss_vel);
_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
const bool gnss_vel_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
// GNSS position
const Vector2f position{gps_sample.pos};
@ -117,13 +117,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
pos_obs_var, // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
_aid_src_gnss_pos);
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
const bool gnss_pos_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
bool mandatory_conditions_passing = false;
if (((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
if ((gnss_pos_enabled || gnss_vel_enabled)
&& _control_status.flags.tilt_align
&& _NED_origin_initialised
) {
@ -148,8 +148,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
if (continuing_conditions_passing
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
fuseVelocity(_aid_src_gnss_vel);
fuseHorizontalPosition(_aid_src_gnss_pos);
if (gnss_vel_enabled) {
fuseVelocity(_aid_src_gnss_vel);
}
if (gnss_pos_enabled) {
fuseHorizontalPosition(_aid_src_gnss_pos);
}
bool do_vel_pos_reset = shouldResetGpsFusion();
@ -196,15 +201,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
if (do_vel_pos_reset) {
ECL_WARN("GPS fusion timeout, resetting velocity and position");
// reset velocity
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
if (gnss_vel_enabled) {
// reset velocity
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
}
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
if (gnss_pos_enabled) {
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
}
}
} else {
@ -228,15 +237,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|| !_control_status_prev.flags.yaw_align
) {
// reset velocity
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
if (gnss_vel_enabled) {
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
}
}
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
if (gnss_pos_enabled) {
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
}
_control_status.flags.gps = true;
}

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

@ -73,8 +73,6 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
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);

4
src/modules/ekf2/EKF/gravity_fusion.cpp
View File

@ -80,11 +80,9 @@ void Ekf::controlGravityFusion(const imuSample &imu)
float innovation_gate = 1.f;
setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
// perform fusion for each axis
_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
&& measurementUpdate(Ky, innovation_variance(1), innovation(1))

3
src/modules/ekf2/EKF/mag_3d_control.cpp
View File

@ -89,7 +89,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
if (_control_status.flags.mag) {
aid_src.timestamp_sample = mag_sample.time_us;
aid_src.fusion_enabled = true;
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
@ -190,8 +189,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
_nb_mag_3d_reset_available = 2;
}
}
aid_src.fusion_enabled = _control_status.flags.mag;
}
void Ekf::stopMagFusion()

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

@ -133,8 +133,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
aid_src_mag.innovation[i] = mag_innov(i);
}
aid_src_mag.fusion_enabled = _control_status.flags.mag;
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
if (_control_status.flags.synthetic_mag_z) {
aid_src_mag.innovation[2] = 0.0f;

3
src/modules/ekf2/EKF/mag_heading_control.cpp
View File

@ -97,7 +97,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
&& isTimedOut(aid_src.time_last_fuse, 3e6);
if (_control_status.flags.mag_hdg) {
aid_src.fusion_enabled = true;
aid_src.timestamp_sample = mag_sample.time_us;
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
@ -185,8 +184,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
}
}
aid_src.fusion_enabled = _control_status.flags.mag_hdg;
// record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only)
_mag_heading_prev = measured_hdg;
_mag_heading_pred_prev = getEulerYaw(_state.quat_nominal);

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

@ -90,8 +90,6 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
void Ekf::fuseOptFlow()
{
_aid_src_optical_flow.fusion_enabled = true;
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
// calculate the height above the ground of the optical flow camera. Since earth frame is NED

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

@ -131,8 +131,6 @@ void Ekf::controlRangeHeightFusion()
&& _range_sensor.isRegularlySendingData();
if (_control_status.flags.rng_hgt) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
fuseVerticalPosition(aid_src);

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

@ -94,8 +94,6 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
sideslip.innovation = innov;
sideslip.innovation_variance = innov_var;
sideslip.fusion_enabled = _control_status.flags.fuse_aspd;
sideslip.timestamp_sample = _time_delayed_us;
const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f);

6
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -176,8 +176,6 @@ void Ekf::controlHaglRngFusion()
// No data anymore. Stop until it comes back.
stopHaglRngFusion();
}
_aid_src_terrain_range_finder.fusion_enabled = _hagl_sensor_status.flags.range_finder;
}
float Ekf::getRngVar() const
@ -241,7 +239,6 @@ void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.fusion_enabled = false;
aid_src.fused = false;
}
@ -319,8 +316,6 @@ void Ekf::controlHaglFlowFusion()
// No data anymore. Stop until it comes back.
stopHaglFlowFusion();
}
_aid_src_terrain_optical_flow.fusion_enabled = _hagl_sensor_status.flags.flow;
}
void Ekf::stopHaglFlowFusion()
@ -345,7 +340,6 @@ void Ekf::resetHaglFlow()
void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
{
flow.fusion_enabled = true;
flow.fused = true;
const float R_LOS = flow.observation_variance[0];

8
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -134,7 +134,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve
void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
{
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
if (!aid_src.innovation_rejected) {
// vx, vy
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
@ -150,7 +150,7 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
if (!aid_src.innovation_rejected) {
// vx, vy, vz
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
@ -168,7 +168,7 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
if (!aid_src.innovation_rejected) {
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
) {
@ -184,7 +184,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
if (!aid_src.innovation_rejected) {
if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;

121
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -55,80 +55,77 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
// innovation test ratio
setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
if (aid_src_status.fusion_enabled) {
// check if the innovation variance calculation is badly conditioned
if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_hdg = false;
// check if the innovation variance calculation is badly conditioned
if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_hdg = false;
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("yaw fusion numerical error - covariance reset");
return false;
}
// calculate the Kalman gains
// only calculate gains for states we are using
VectorState Kfusion;
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
for (uint8_t row = 0; row < State::size; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row, col) * H_YAW(col);
}
Kfusion(row) *= heading_innov_var_inv;
}
// set the magnetometer unhealthy if the test fails
if (aid_src_status.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (!_control_status.flags.in_air
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
) {
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
// also reset the yaw gyro variance to converge faster and avoid
// being stuck on a previous bad estimate
resetGyroBiasZCov();
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("yaw fusion numerical error - covariance reset");
return false;
}
// calculate the Kalman gains
// only calculate gains for states we are using
VectorState Kfusion;
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
} else {
_innov_check_fail_status.flags.reject_yaw = false;
}
for (uint8_t row = 0; row < State::size; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row, col) * H_YAW(col);
}
if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
Kfusion(row) *= heading_innov_var_inv;
}
_time_last_heading_fuse = _time_delayed_us;
// set the magnetometer unhealthy if the test fails
if (aid_src_status.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
aid_src_status.time_last_fuse = _time_delayed_us;
aid_src_status.fused = true;
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (!_control_status.flags.in_air
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
) {
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
_fault_status.flags.bad_hdg = false;
// also reset the yaw gyro variance to converge faster and avoid
// being stuck on a previous bad estimate
resetGyroBiasZCov();
return true;
} else {
return false;
}
} else {
_innov_check_fail_status.flags.reject_yaw = false;
}
if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
_time_last_heading_fuse = _time_delayed_us;
aid_src_status.time_last_fuse = _time_delayed_us;
aid_src_status.fused = true;
_fault_status.flags.bad_hdg = false;
return true;
} else {
_fault_status.flags.bad_hdg = true;
}
} else {
_fault_status.flags.bad_hdg = true;
}
// otherwise

1
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
View File

@ -62,7 +62,6 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
// The yaw variance is too large, fuse fake measurement
aid_src_status.fusion_enabled = true;
fuseYaw(aid_src_status, H_YAW);
}
}