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

EKF: Fix use of incorrect timestamp

Browse Source 
This was incorrectly using the IMU (1/250 sec) timestamp instead of the EKF (1/100 sec) value.
The corresponding accelerometer limit has been made a parameter and adjusted to match previous behaviour.
This commit is contained in:
Paul Riseborough
2017-03-10 10:14:06 +01:00
committed by Lorenz Meier
parent 879ad1fd2c
commit cf31945038
2 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+6
View File
@@ -269,6 +269,9 @@ struct parameters {
float vel_Tau; // velocity state correction time constant (1/sec) float vel_Tau; // velocity state correction time constant (1/sec)
float pos_Tau; // postion state correction time constant (1/sec) float pos_Tau; // postion state correction time constant (1/sec)
// state limits
float acc_bias_lim; // maximum accel bias magnitude (m/s/s)
unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected // rejected
@@ -373,6 +376,9 @@ struct parameters {
vel_Tau = 0.25f; vel_Tau = 0.25f;
pos_Tau = 0.25f; pos_Tau = 0.25f;
// state limiting
acc_bias_lim = 0.4f;
no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps
} }
EKF/ekf_helper.cpp
+2 -2
View File
@@ -535,11 +535,11 @@ void Ekf::constrainStates()
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_imu_avg, 0.349066f * _dt_imu_avg); _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg);
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg); _state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {