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

EKF: Compensate optical flow data for sensor position offset

Browse Source
This commit is contained in:
Paul Riseborough
2016-04-12 11:14:31 +10:00
parent 48b105b748
commit b46053415f
5 changed files with 27 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+1 -1
View File
@@ -266,7 +266,7 @@ bool Ekf::update()
// 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 = ang_rate % pos_offset_body;
Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body);
Vector3f vel_offset_earth = _R_prev.transpose() * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;