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

Merge pull request #145 from CarlOlsson/small_fix 

Deleted second update of transformation matrix
This commit is contained in:
Paul Riseborough 2016-05-23 07:40:58 +10:00
parent 0fafc49a49 eb70aca2e8
commit c8c2d6d963
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
EKF/ekf.cpp
View File

@ -570,8 +570,10 @@ void Ekf::predictState()
// save the previous value of velocity so we can use trapzoidal integration
Vector3f vel_last = _state.vel;
// update the rotation matrix and calculate the increment in velocity using the current orientation
// update transformation matrix from body to world frame
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate the increment in velocity using the current orientation
_state.vel += _R_to_earth * corrected_delta_vel;
// compensate for acceleration due to gravity
@ -580,9 +582,6 @@ void Ekf::predictState()
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
// update transformation matrix from body to world frame
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
constrainStates();
// calculate an average filter update time