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

Support full vision variance

Browse Source
This commit is contained in:
kamilritz
2020-01-08 14:36:03 +01:00
committed by Mathieu Bresciani
parent 4ab3dd50c9
commit 07e8e88e88
5 changed files with 28 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+15 -9
View File
@@ -238,7 +238,7 @@ void Ekf::controlExternalVisionFusion()
uncorrelateQuatStates();
// adjust the quaternion covariances estimated yaw error
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)));
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
@@ -307,7 +307,10 @@ void Ekf::controlExternalVisionFusion()
_ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.5f));
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
}
// record observation and estimate for use next time
@@ -318,13 +321,16 @@ void Ekf::controlExternalVisionFusion()
} else {
// use the absolute position
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
}
_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
// observation 1-STD error
ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.01f));
ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.01f));
ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 0), sq(0.01f));
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) {
@@ -349,11 +355,13 @@ void Ekf::controlExternalVisionFusion()
Vector3f ev_vel_obs_var;
Vector2f ev_vel_innov_gates;
Vector3f vel_aligned(_ev_sample_delayed.vel);
Vector3f vel_aligned{_ev_sample_delayed.vel};
Matrix3f ev_vel_var = matrix::diag(_ev_sample_delayed.velVar);
// rotate measurement into correct earth frame if required
if (_params.fusion_mode & MASK_ROTATE_EV) {
vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel;
ev_vel_var = _R_ev_to_ekf * ev_vel_var * _R_ev_to_ekf.transpose();
}
// correct velocity for offset relative to IMU
@@ -373,10 +381,8 @@ void Ekf::controlExternalVisionFusion()
}
}
// observation 1-STD error
ev_vel_obs_var(0) = ev_vel_obs_var(1) = ev_vel_obs_var(2) = sq(fmaxf(_ev_sample_delayed.velErr, 0.01f));
ev_vel_obs_var = matrix::max(ev_vel_var.diag(), sq(0.01f));
// innovation gate size
ev_vel_innov_gates(0) = ev_vel_innov_gates(1) = fmaxf(_params.ev_vel_innov_gate, 1.0f);
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
@@ -1180,7 +1186,7 @@ void Ekf::controlHeightFusion()
// calculate the innovation assuming the external vision observation is in local NED frame
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
ev_hgt_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f));
ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
// innovation gate size
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
// fuse height information