dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Support full vision variance

This commit is contained in:
kamilritz 2019-12-09 14:27:22 +01:00 committed by Mathieu Bresciani
parent 4ab3dd50c9
commit 07e8e88e88
5 changed files with 28 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
EKF/common.h
View File

@ -83,10 +83,9 @@ struct ext_vision_message {
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
float velErr; ///< 1-Sigma velocity accuracy (m/sec)
float angErr; ///< 1-Sigma angular error (rad)
Vector3f posVar; ///< XYZ position variances (m**2)
Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2)
float angVar; ///< angular heading variance (rad**2)
};
struct outputSample {
@ -156,10 +155,9 @@ struct extVisionSample {
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
float velErr; ///< 1-Sigma velocity accuracy (m/sec)
float angErr; ///< 1-Sigma angular error (rad)
Vector3f posVar; ///< XYZ position variances (m**2)
Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2)
float angVar; ///< angular heading variance (rad**2)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};

24
EKF/control.cpp
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

6
EKF/ekf_helper.cpp
View File

@ -100,7 +100,7 @@ bool Ekf::resetVelocity()
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
}
_state.vel = _ev_vel;
P.uncorrelateCovarianceSetVariance<3>(4, sq(_ev_sample_delayed.velErr));
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
} else {
ECL_INFO_TIMESTAMPED("reset velocity to zero");
// Used when falling back to non-aiding mode of operation
@ -165,7 +165,7 @@ bool Ekf::resetPosition()
_state.pos(1) = _ev_pos(1);
// use EV accuracy to reset variances
P.uncorrelateCovarianceSetVariance<2>(7, sq(_ev_sample_delayed.posErr));
P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
} else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("reset position to last known position");
@ -705,7 +705,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// update the yaw angle variance using the variance of the measurement
if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));

7
EKF/estimator_interface.cpp
View File

@ -434,10 +434,9 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
// calculate the system time-stamp for the mid point of the integration period
ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
ev_sample_new.angErr = evdata->angErr;
ev_sample_new.posErr = evdata->posErr;
ev_sample_new.velErr = evdata->velErr;
ev_sample_new.hgtErr = evdata->hgtErr;
ev_sample_new.angVar = evdata->angVar;
ev_sample_new.posVar = evdata->posVar;
ev_sample_new.velVar = evdata->velVar;
ev_sample_new.quat = evdata->quat;
ev_sample_new.pos = evdata->pos;
ev_sample_new.vel = evdata->vel;

2
EKF/mag_fusion.cpp
View File

@ -626,7 +626,7 @@ void Ekf::fuseHeading()
} else if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
R_YAW = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
} else {
// default value