dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

optical flow: fixed sign in calculation of optical flow sensor velocity 

- the velocity of the optical flow sensor relative to the IMU expressed in
body frame is the cross product of the angular velocity with the vector
from IMU to the sensor. If we use the angular velocity stored in the flow
sample struct we need to use a negative sign as that angular velocity
follows the opposite sign convention.

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2018-12-02 16:34:28 +01:00 committed by Paul Riseborough
parent c49ab760c7
commit 27f7846495
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/optflow_fusion.cpp
View File

@ -75,7 +75,8 @@ void Ekf::fuseOptFlow()
Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
// calculate the velocity of the sensor reelative to the imu in body frame
Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body);
// 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);
// calculate the velocity of the sensor in the earth frame
Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;