dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix rebase error

This commit is contained in:
Paul Riseborough 2016-04-30 15:33:09 +10:00
parent 67d23948b5
commit 54e713969d
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/ekf.cpp
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;