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

EKF: Fix rebase error

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent 67d23948b5
commit 54e713969d
1 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+4 -5
View File
@@ -612,13 +612,12 @@ 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_scale(0) - _state.gyro_bias(0);
delta_angle(1) = _imu_sample_new.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1);
delta_angle(2) = _imu_sample_new.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2);
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);
// correct delta velocity for bias offsets
Vector3f delta_vel = _imu_sample_new.delta_vel;
delta_vel(2) -= _state.accel_z_bias;
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias;
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
delta_angle += _delta_angle_corr;