fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:43:33 +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/control.cpp
+2 -2
View File
@@ -357,7 +357,7 @@ void Ekf::controlExternalVisionFusion()
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_body = ang_rate % pos_offset_body;
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
vel_aligned -= vel_offset_earth;
@@ -692,7 +692,7 @@ void Ekf::controlGpsFusion()
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_body = ang_rate % pos_offset_body;
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;