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

Merge pull request #234 from PX4/pr-optFlowUpdates

Browse Source 
EKF: optical flow improvements
This commit is contained in:
Paul Riseborough
2017-02-22 09:00:48 +11:00
committed by GitHub
parent 66b8f7ec5b bcf7cac5d9
commit 6e5a9aabe1
2 changed files with 26 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+13 -7
View File
@@ -279,15 +279,21 @@ void Ekf::controlOpticalFlowFusion()
_state.pos(0) = 0.0f;
_state.pos(1) = 0.0f;
// reset the corresponding covariances
// we are by definition at the origin at commencement so variances are also zeroed
zeroRows(P,7,8);
zeroCols(P,7,8);
// align the output observer to the EKF states
alignOutputFilter();
} else {
// set to the last known position
_state.pos(0) = _last_known_posNE(0);
_state.pos(1) = _last_known_posNE(1);
}
// reset the corresponding covariances
// we are by definition at the origin at commencement so variances are also zeroed
zeroRows(P,7,8);
zeroCols(P,7,8);
// align the output observer to the EKF states
alignOutputFilter();
}
}
EKF/estimator_interface.cpp
+13 -4
View File
@@ -282,9 +282,10 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// limit data rate to prevent data being lost
if (time_usec - _time_last_optflow > _min_obs_interval_us) {
// check if enough integration time
// check if enough integration time and fail if integration time is less than 50%
// of min arrival interval because too much data is being lost
float delta_time = 1e-6f * (float)flow->dt;
bool delta_time_good = (delta_time >= 0.05f);
bool delta_time_good = (delta_time >= 5e-7f * (float)_min_obs_interval_us);
// check magnitude is within sensor limits
float flow_rate_magnitude;
@@ -298,7 +299,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// check quality metric
bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
if (delta_time_good && flow_magnitude_good && flow_quality_good) {
if (delta_time_good && flow_magnitude_good && (flow_quality_good || !_control_status.flags.in_air)) {
flowSample optflow_sample_new;
// calculate the system time-stamp for the mid point of the integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
@@ -306,8 +307,16 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
optflow_sample_new.quality = flow->quality;
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
// copy the optical and gyro measured delta angles
optflow_sample_new.flowRadXY = - flow->flowdata;
optflow_sample_new.gyroXYZ = - flow->gyrodata;
if (flow_quality_good) {
optflow_sample_new.flowRadXY = - flow->flowdata;
} else {
// when on the ground with poor flow quality, assume zero ground relative velocity
optflow_sample_new.flowRadXY(0) = + flow->gyrodata(0);
optflow_sample_new.flowRadXY(1) = + flow->gyrodata(1);
}
// compensate for body motion to give a LOS rate
optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);