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

EKF: use Matrix cross product

Browse Source
This commit is contained in:
Daniel Agar
2019-12-17 09:35:08 +01:00
committed by Mathieu Bresciani
parent 0831c158f2
commit 98a1aae494
7 changed files with 7 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+1 -1
View File
@@ -74,7 +74,7 @@ void Ekf::fuseOptFlow()
// calculate the velocity of the sensor relative to the imu in body frame
// Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body);
Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body;
// calculate the velocity of the sensor in the earth frame
Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;