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

EKF: Handle flow data without valid gyro data

Browse Source
This commit is contained in:
Paul Riseborough
2018-06-12 16:24:25 +10:00
parent 26dcf05d80
commit de7c9d85a4
6 changed files with 60 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
-1
View File
@@ -139,7 +139,6 @@ struct airspeedSample {
struct flowSample {
uint8_t quality; ///< quality indicator between 0 and 255
Vector2f flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
Vector2f flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
Vector3f gyroXYZ; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
float dt; ///< amount of integration time (sec)
uint64_t time_us; ///< timestamp of the integration period mid-point (uSec)
EKF/control.cpp
+20 -8
View File
@@ -455,16 +455,28 @@ void Ekf::controlOpticalFlowFusion()
}
}
// fuse the data if the terrain/distance to bottom is valid but use a more relaxed check to enable it to survive bad range finder data
if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
// Update optical flow bias estimates
calcOptFlowBias();
// Only fuse optical flow if valid body rate compensation data is available
if (calcOptFlowBodyRateComp()) {
// Fuse optical flow LOS rate observations into the main filter
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
if (!flow_quality_good && !_control_status.flags.in_air) {
// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
_flowRadXYcomp.zero();
} else {
// compensate for body motion to give a LOS rate
_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);
}
// Fuse optical flow LOS rate observations into the main filter if height above ground has been updated recently but use a relaxed time criteria to enable it to coast through bad range finder data
if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
}
}
}
EKF/ekf.h
+4 -2
View File
@@ -355,6 +355,7 @@ private:
uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec)
uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
Vector2f _flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad)
@@ -486,8 +487,9 @@ private:
// fuse optical flow line of sight rate measurements
void fuseOptFlow();
// calculate optical flow bias errors
void calcOptFlowBias();
// calculate optical flow body angular rate compensation
// returns false if bias corrected body rate data is unavailable
bool calcOptFlowBodyRateComp();
// initialise the terrain vertical position estimator
// return true if the initialisation is successful
EKF/ekf_helper.cpp
+2 -2
View File
@@ -71,8 +71,8 @@ bool Ekf::resetVelocity()
// we should have reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
vel_optflow_body(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flowRadXYcomp(0) / _flow_sample_delayed.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
EKF/estimator_interface.cpp
+1 -14
View File
@@ -405,20 +405,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// 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.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);
optflow_sample_new.flowRadXY = - flow->flowdata;
// convert integration interval to seconds
optflow_sample_new.dt = delta_time;
EKF/optflow_fusion.cpp
+33 -21
View File
@@ -93,8 +93,8 @@ void Ekf::fuseOptFlow()
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis.
Vector2f opt_flow_rate;
opt_flow_rate(0) = _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
opt_flow_rate(1) = _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
if (opt_flow_rate.norm() < _flow_max_rate) {
_flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
@@ -527,39 +527,51 @@ void Ekf::get_drag_innov_var(float drag_innov_var[2])
memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var));
}
// calculate optical flow gyro bias errors
void Ekf::calcOptFlowBias()
// calculate optical flow body angular rate compensation
// returns false if bias corrected body rate data is unavailable
bool Ekf::calcOptFlowBodyRateComp()
{
// reset the accumulators if the time interval is too large
if (_delta_time_of > 1.0f) {
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
return;
return false;
}
// if accumulation time differences are not excessive and accumulation time is adequate
// compare the optical flow and and navigation rate data and calculate a bias error
if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
// calculate a reference angular rate
Vector3f reference_body_rate;
reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
// calculate the optical flow sensor measured body rate
Vector3f of_body_rate;
of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
if (use_flow_sensor_gyro) {
// calculate the bias estimate using a combined LPF and spike filter
_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
-0.1f, 0.1f);
_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
-0.1f, 0.1f);
_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
-0.1f, 0.1f);
// if accumulation time differences are not excessive and accumulation time is adequate
// compare the optical flow and and navigation rate data and calculate a bias error
if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
// calculate a reference angular rate
Vector3f reference_body_rate;
reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
// calculate the optical flow sensor measured body rate
Vector3f of_body_rate;
of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
// calculate the bias estimate using a combined LPF and spike filter
_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
-0.1f, 0.1f);
_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
-0.1f, 0.1f);
_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
-0.1f, 0.1f);
}
} else {
// Use the EKF gyro data if optical flow sensor gyro data is not available
_flow_sample_delayed.gyroXYZ = _imu_del_ang_of;
_flow_gyro_bias.zero();
}
// reset the accumulators
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
return true;
}
// calculate the measurement variance for the optical flow sensor (rad/sec)^2