zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Fix rebase error

master
Paul Riseborough 9 years ago
parent
67d23948b5
commit
54e713969d
1 changed files with 4 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      EKF/ekf.cpp

9
EKF/ekf.cpp
Unescape View File

@ -612,13 +612,12 @@ void Ekf::calculateOutputStates() @@ -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;

Write Preview
Loading…
Cancel
Save