Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

EKF: optical flow improvements #234

Merged
merged 3 commits into from
Feb 21, 2017
Merged

EKF: optical flow improvements #234

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
21270bb
EKF: Use last known position when resuming OF aiding
priseborough Jan 16, 2017
3fb7eff
EKF: Allow optical flow use when on-ground with poor data quality
priseborough Jan 16, 2017
bcf7cac
EKF: remove hardcoded limit on optical flow time delta
priseborough Jan 17, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 13 additions & 7 deletions EKF/control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Resetting the state should be avoided. Better if you can stop estimating once the covariance becomes too large.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's not a good idea given the federated architecture. It needs to fuse in position measurements at the last known position to constrain angular drift.


}

// 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();

}
}

Expand Down
17 changes: 13 additions & 4 deletions EKF/estimator_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -287,9 +287,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;
Expand All @@ -303,16 +304,24 @@ 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;
// copy the quality metric returned by the PX4Flow sensor
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
Copy link
Contributor

@ChristophTobler ChristophTobler Jan 17, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I can't really see how it will be in this state, because of (flow_quality_good || _control_status.flags.in_air). If flow is bad, which is the case in this else, it can't be on the ground because of the upper if statement (_control_status.flags.in_air). I guess it should be (flow_quality_good || !_control_status.flags.in_air)?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Agree - will fix

optflow_sample_new.flowRadXY(0) = + flow->gyrodata(0);
optflow_sample_new.flowRadXY(1) = + flow->gyrodata(1);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You will get drift here because the flow gyro has bias. Best to just set it to 0.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The EKF uses the angular motion compensated rate which is the optical rate minus the gyro rate. The drift will therefore be cancelled out.

Copy link
Member

@jgoppert jgoppert Jan 17, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, see you have flow gyro bias. OK.


}
// 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);
Expand Down