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

fix delta angle bias usage:

Browse Source 
the delta angle bias was applied to imu data which was not coming in the same
time intervall as the filter was operating. Therefore, the delta angle bias
applied to new imu data had to be scaled correctly in order to match
the imu time interval.
This commit is contained in:
Roman
2016-01-14 17:06:02 +01:00
parent 749156d4aa
commit 0b5c90574c
2 changed files with 11 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+6 -8
View File
@@ -232,14 +232,14 @@ void Ekf::calculateOutputStates()
{
imuSample imu_new = _imu_sample_new;
Vector3f delta_angle;
delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);
delta_angle -= _state.gyro_bias;
// Note: We do no not need to consider any bias or scale correction here
// since the base class has already corrected the imu sample
delta_angle(0) = imu_new.delta_ang(0);
delta_angle(1) = imu_new.delta_ang(1);
delta_angle(2) = imu_new.delta_ang(2);
Vector3f delta_vel = imu_new.delta_vel;
delta_vel(2) -= _state.accel_z_bias;
delta_angle += _delta_angle_corr;
Quaternion dq;
@@ -265,9 +265,7 @@ void Ekf::calculateOutputStates()
_imu_updated = false;
}
if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
return;
}
_output_sample_delayed = _output_buffer.get_oldest();
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv;
EKF/estimator_base.cpp
+5 -4
View File
@@ -83,14 +83,15 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
imu_sample_new.time_us = time_usec;
_imu_sample_new = imu_sample_new;
imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
imu_sample_new.delta_ang -= _state.gyro_bias;
imu_sample_new.delta_vel(2) -= _state.accel_z_bias;
imu_sample_new.delta_ang -= _state.gyro_bias * imu_sample_new.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
imu_sample_new.delta_vel(2) -= _state.accel_z_bias * imu_sample_new.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
// store the new sample for the complementary filter prediciton
_imu_sample_new = imu_sample_new;
_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;