Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-12 23:42:23 +08:00
Code Issues Actions 9 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+8 -3
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);
}
}