fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 03:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Pr opt flow (#482)

Browse Source 
* optical flow: fixed calculation of velocity of the flow sensor relative to
the IMU

- gyroXYZ holds a delta angle and first needs to converted to a gyro rate

Signed-off-by: Roman <bapstroman@gmail.com>

* optical flow: calculate height above the ground with respect to the flow
camera
- the flow camera can be offset from the IMU which needs to be considered

Signed-off-by: Roman <bapstroman@gmail.com>

* estimator interface: fixed comment regarding optical flow sample timestamp

- the timestamp on an optical flow sample corresponds to the trailing
edge of the flow integration period

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst 2018-07-16 03:41:35 +02:00 committed by Paul Riseborough
parent 4d59c834eb
commit ee85a29202
2 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/estimator_interface.cpp
View File

@ -380,7 +380,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
flowSample optflow_sample_new;
// calculate the system time-stamp for the leading edge of the flow data integration period
// calculate the system time-stamp for the trailing edge of the flow data integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
// copy the quality metric returned by the PX4Flow sensor

18
EKF/optflow_fusion.cpp
View File

@ -67,9 +67,6 @@ void Ekf::fuseOptFlow()
float H_LOS[2][24] = {}; // Optical flow observation Jacobians
float Kfusion[24][2] = {}; // Optical flow Kalman gains
// constrain height above ground to be above minimum height when sitting on ground
float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance);
// get rotation nmatrix from earth to body
Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
@ -78,7 +75,7 @@ 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, pos_offset_body);
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;
@ -86,6 +83,19 @@ void Ekf::fuseOptFlow()
// rotate into body frame
Vector3f vel_body = earth_to_body * vel_rel_earth;
// height above ground of the IMU
float heightAboveGndEst = _terrain_vpos - _state.pos(2);
// calculate the sensor position relative to the IMU in earth frame
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positve offset in earth frame leads to a a smaller height above the ground.
heightAboveGndEst -= pos_offset_earth(2);
// constrain minimum height above ground
heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);
// calculate range from focal point to centre of image
float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view