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

Merge pull request #10 from PX4/delta_ang_bias_fix

Browse Source 
fix delta angle bias usage:
This commit is contained in:
Roman Bapst
2016-01-14 17:10:47 +01:00
parent 749156d4aa 0b5c90574c
commit 2c80614e20
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;