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

Improve external vision position fusion

Browse Source
This commit is contained in:
kamilritz
2019-09-24 07:48:46 +10:00
committed by Paul Riseborough
parent 3e8139ff9f
commit 4237d7ccd7
1 changed files with 14 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+14 -9
View File
@@ -283,10 +283,9 @@ void Ekf::controlExternalVisionFusion()
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
// Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED
if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) {
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.fusion_mode & MASK_USE_GPS) {
_fuse_hpos_as_odom = true;
} else {
_fuse_hpos_as_odom = false;
}
@@ -301,15 +300,15 @@ void Ekf::controlExternalVisionFusion()
// calculate the change in position since the last measurement
Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev;
// rotate measurement into body frame if required
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
}
// rotate measurement into body frame is required when fusing with GPS
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
// use the change in position since the last measurement
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.5f);
}
// record observation and estimate for use next time
@@ -319,8 +318,14 @@ void Ekf::controlExternalVisionFusion()
} else {
// use the absolute position
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
Vector3f ev_pos_meas = _ev_sample_delayed.posNED;
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
}
_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0);
_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1);
// observation 1-STD error
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {