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

Merge pull request #153 from PX4/pr-ekf2ExternalVision

Browse Source 
EKF: external vision bug fixes
This commit is contained in:
Paul Riseborough
2016-05-27 18:57:41 +10:00
parent 53030b9297 ad02818b3d
commit 9557f64248
5 changed files with 137 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+1 -2
View File
@@ -231,9 +231,7 @@ struct parameters {
float rng_gnd_clearance; // minimum valid value for range when on ground (m)
// vision position fusion
float ev_noise; // observation noise for vision sensor estimates (m)
float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD)
float ev_gnd_clearance; // minimum valid value for vision based height estimate when on ground (m)
// optical flow fusion
float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec)
@@ -424,6 +422,7 @@ union filter_control_status_u {
uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference
uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused
uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused
uint16_t ev_hgt : 1; // 14 - true when height data from external vision measurements is being fused
} flags;
uint16_t value;
};
EKF/control.cpp
+74 -8
View File
@@ -323,6 +323,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF baro hgt timeout - reset to GPS\n");
@@ -333,6 +334,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF baro hgt timeout - reset to baro\n");
@@ -372,6 +374,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF gps hgt timeout - reset to baro\n");
@@ -382,6 +385,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF gps hgt timeout - reset to GPS\n");
@@ -414,6 +418,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF rng hgt timeout - reset to baro\n");
@@ -424,6 +429,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF rng hgt timeout - reset to rng hgt\n");
@@ -433,6 +439,48 @@ void Ekf::controlHeightSensorTimeouts()
}
}
// handle the case where we are using external vision data for height
if (_control_status.flags.ev_hgt) {
// check if vision data is available
extVisionSample ev_init = _ext_vision_buffer.get_newest();
bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
// check if baro data is available
baroSample baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// reset to baro if we have no vision data and baro data is available
bool reset_to_baro = !ev_data_available && baro_data_available;
// reset to ev data if it is available
bool reset_to_ev = ev_data_available;
if (reset_to_baro) {
// set height sensor health
_rng_hgt_faulty = true;
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF ev hgt timeout - reset to baro\n");
} else if (reset_to_ev) {
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
// request a reset
reset_height = true;
printf("EKF ev hgt timeout - reset to ev hgt\n");
} else {
// we have nothing to reset to
reset_height = false;
}
}
// Reset vertical position and velocity states to the last measurement
if (reset_height) {
resetHeight();
@@ -448,25 +496,43 @@ void Ekf::controlHeightAiding()
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// Control the soure of height measurements for the main filter
if (_control_status.flags.ev_pos) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
// Control the source of height measurements for the main filter
// do not switch to a sensor if it is unhealthy or the data is stale
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
!_baro_hgt_faulty &&
(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) &&
!_gps_hgt_faulty &&
(((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
!_rng_hgt_faulty &&
(((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) {
} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
(((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
}
}
EKF/ekf.cpp
+24 -11
View File
@@ -415,6 +415,13 @@ bool Ekf::initialiseFilter(void)
if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) {
// initialise the counter
_ev_counter = 1;
// set the height fusion mode to use external vision data when we start getting valid data from the buffer
if (_primary_hgt_source == VDIST_SENSOR_EV) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
}
} else if (_ev_counter != 0) {
// increment the sample count
_ev_counter ++;
@@ -423,13 +430,10 @@ bool Ekf::initialiseFilter(void)
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
if (_params.fusion_mode & MASK_USE_EVPOS) {
_primary_hgt_source = VDIST_SENSOR_EV;
} else {
_primary_hgt_source = _params.vdist_sensor_type;
}
_primary_hgt_source = _params.vdist_sensor_type;
}
// accumulate enough height measurements to be confident in the qulaity of the data
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
if (_hgt_counter == 0 && _range_sample_delayed.time_us != 0) {
@@ -437,6 +441,7 @@ bool Ekf::initialiseFilter(void)
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
_rng_filt_state = _range_sample_delayed.rng;
_hgt_counter = 1;
} else if (_hgt_counter != 0) {
@@ -446,7 +451,7 @@ bool Ekf::initialiseFilter(void)
}
}
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) {
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
// later when it passes checks.
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@@ -464,14 +469,16 @@ bool Ekf::initialiseFilter(void)
}
}
} else if (_primary_hgt_source == VDIST_SENSOR_EV) {
// do nothing becasue vision data is checked elsewhere
} else {
return false;
}
// check to see if we have enough measurements and return false if not
bool hgt_count_fail = _hgt_counter <= 10;
bool mag_count_fail = _mag_counter <= 10;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 10);
bool hgt_count_fail = _hgt_counter <= OBS_BUFFER_LENGTH;
bool mag_count_fail = _mag_counter <= OBS_BUFFER_LENGTH;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= OBS_BUFFER_LENGTH);
if (hgt_count_fail || mag_count_fail || ev_count_fail) {
return false;
@@ -516,12 +523,18 @@ bool Ekf::initialiseFilter(void)
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init);
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder
if (_control_status.flags.rng_hgt) {
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
} else if (_control_status.flags.ev_hgt) {
// if we are using external vision data for height, then the vertical position state needs to be reset
// because the initialisation position is not the zero datum
resetHeight();
}
// initialise the state covariance matrix
EKF/ekf_helper.cpp
+7 -4
View File
@@ -204,16 +204,19 @@ void Ekf::resetHeight()
// TODO: reset to last known gps based estimate
}
} else if (_control_status.flags.ev_pos) {
} else if (_control_status.flags.ev_hgt) {
// initialize vertical position with newest measurement
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
if (abs(dt_newest) < abs(dt_delayed)) {
_state.pos(2) = ev_newest.posNED(2);
} else {
// TODO: reset to last known baro based estimate
_state.pos(2) = _ev_sample_delayed.posNED(2);
}
}
// reset the vertical velocity covariance values
EKF/vel_pos_fusion.cpp
+31 -19
View File
@@ -86,7 +86,28 @@ void Ekf::fuseVelPosHeight()
// Calculate innovations and observation variance depending on type of observations
// being used
if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
if (_control_status.flags.gps) {
// we are using GPS measurements
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
// innovation gate size
gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
} else if (_control_status.flags.ev_pos) {
// we are using external vision measurements
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
// innovation gate size
gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);
} else {
// No observations - use a static position to constrain drift
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
@@ -96,25 +117,18 @@ void Ekf::fuseVelPosHeight()
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
} else if (_control_status.flags.gps) {
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
} else if (_control_status.flags.ev_pos) {
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
// glitch protection is not required so set gate to a large value
gate_size[3] = 100.0f;
}
// convert North position noise to variance
R[3] = R[3] * R[3];
// copy North axis values to East axis
R[4] = R[3];
// innovation gate sizes
gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
gate_size[4] = gate_size[3];
}
if (_fuse_height) {
@@ -151,7 +165,7 @@ void Ekf::fuseVelPosHeight()
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
} else if (_control_status.flags.ev_pos) {
} else if (_control_status.flags.ev_hgt) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
@@ -159,7 +173,7 @@ void Ekf::fuseVelPosHeight()
R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f);
}
}
@@ -183,9 +197,7 @@ void Ekf::fuseVelPosHeight()
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
&& (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f))
|| using_synthetic_measurements || !_control_status.flags.tilt_align;
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || !_control_status.flags.tilt_align;
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;