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

EKF: Rationalise use of rotation matrices and improve efficiency

Browse Source
This commit is contained in:
Paul Riseborough
2016-04-12 11:14:31 +10:00
parent e10093854a
commit 5bf02517a7
6 changed files with 35 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+1 -1
View File
@@ -94,7 +94,7 @@ void Ekf::fuseOptFlow()
Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ , pos_offset_body);
// calculate the velocity of the sensor in the earth frame
Vector3f vel_rel_earth = _state.vel + _R_prev.transpose() * vel_rel_imu_body;
Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
// rotate into body frame
Vector3f vel_body = earth_to_body * vel_rel_earth;