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

EKF increase delta velocity process noise per axis if clipping (#663)

Browse Source
This commit is contained in:
Daniel Agar
2020-04-08 08:51:51 +10:00
committed by GitHub
parent d6b6276cdc
commit 47624a0f02
5 changed files with 25 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+14
View File
@@ -246,6 +246,20 @@ void Ekf::predictCovariance()
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
// Accelerometer Clipping
// delta velocity X: increase process noise if sample contained any X axis clipping
if (_imu_sample_delayed.delta_vel_clipping[0]) {
dvxVar = sq(dt * BADACC_BIAS_PNOISE);
}
// delta velocity Y: increase process noise if sample contained any Y axis clipping
if (_imu_sample_delayed.delta_vel_clipping[1]) {
dvyVar = sq(dt * BADACC_BIAS_PNOISE);
}
// delta velocity Z: increase process noise if sample contained any Z axis clipping
if (_imu_sample_delayed.delta_vel_clipping[2]) {
dvzVar = sq(dt * BADACC_BIAS_PNOISE);
}
// predict the covariance
// intermediate calculations