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

deleted second update of transformation matrix

Browse Source
This commit is contained in:
CarlOlsson
2016-05-22 16:03:30 +02:00
parent 4c5252269c
commit eb70aca2e8
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+3 -4
View File
@@ -570,8 +570,10 @@ void Ekf::predictState()
// save the previous value of velocity so we can use trapzoidal integration // save the previous value of velocity so we can use trapzoidal integration
Vector3f vel_last = _state.vel; 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); _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; _state.vel += _R_to_earth * corrected_delta_vel;
// compensate for acceleration due to gravity // compensate for acceleration due to gravity
@@ -580,9 +582,6 @@ void Ekf::predictState()
// predict position states via trapezoidal integration of velocity // predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; _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(); constrainStates();
// calculate an average filter update time // calculate an average filter update time