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

ekf2: fix delta angle coning metric

Browse Source
This commit is contained in:
bresch 2022-01-04 11:29:14 +01:00 committed by Daniel Agar
parent 6fbc0e95d0
commit d094fbbd70
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/ekf2/EKF2.cpp
View File

@ -1380,12 +1380,17 @@ void EKF2::UpdateImuStatus()
return;
}
const float dt = _ekf.get_dt_imu_avg();
// accel -> delta velocity
_ekf.setDeltaVelocityHighFrequencyVibrationMetric(imu_status.accel_vibration_metric * _ekf.get_dt_imu_avg());
_ekf.setDeltaVelocityHighFrequencyVibrationMetric(imu_status.accel_vibration_metric * dt);
// gyro -> delta angle
_ekf.setDeltaAngleHighFrequencyVibrationMetric(imu_status.gyro_vibration_metric * _ekf.get_dt_imu_avg());
_ekf.setDeltaAngleConingMetric(imu_status.gyro_coning_vibration * _ekf.get_dt_imu_avg());
_ekf.setDeltaAngleHighFrequencyVibrationMetric(imu_status.gyro_vibration_metric * dt);
// note: the coning metric uses the cross product of two consecutive angular velocities
// this is why the conversion to delta angle requires dt^2
_ekf.setDeltaAngleConingMetric(imu_status.gyro_coning_vibration * dt * dt);
}
}