fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix IMU bias compensation scale error in output filter

Browse Source 
Delta angle and velocities are calculated assuming data is at the filter update rate, not the IMU update rate
This commit is contained in:
Paul Riseborough 2016-05-04 13:59:18 +10:00
parent fd109b00ab
commit 5523a4f225
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
EKF/ekf.cpp
View File

@ -546,6 +546,10 @@ void Ekf::predictState()
constrainStates();
// calculate an average filter update time
float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value
input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS));
_dt_ekf_avg = 0.99f*_dt_ekf_avg + 0.005f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
}
@ -618,12 +622,13 @@ void Ekf::calculateOutputStates()
// correct delta angles for bias offsets and scale factors
Vector3f delta_angle;
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0);
delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1);
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2);
float dt_scale_correction = _dt_imu_avg/_dt_ekf_avg;
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0)*dt_scale_correction;
delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1)*dt_scale_correction;
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2)*dt_scale_correction;
// correct delta velocity for bias offsets
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias;
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias*dt_scale_correction;
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
delta_angle += _delta_angle_corr;