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

Enable vision velocity fusion

Browse Source
This commit is contained in:
kamilritz
2019-09-24 07:48:46 +10:00
committed by Paul Riseborough
parent 149233a9ab
commit 3e8139ff9f
5 changed files with 87 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+68 -33
View File
@@ -167,43 +167,36 @@ void Ekf::controlExternalVisionFusion()
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) {
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
// reset external vision rotation matrix independent if is actually used for debugging purposes
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)
&& (_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)){
// Reset transformation between EV and EKF navigation frames
resetExtVisRotMat();
}
// external vision position aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align
&& _control_status.flags.yaw_align) {
// external vision aiding selection logic
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
// check for a external vision measurement that has fallen behind the fusion time horizon
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
// turn on use of external vision measurements for position
_control_status.flags.ev_pos = true;
ECL_INFO("EKF commencing external vision position fusion");
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
_control_status.flags.ev_pos = true;
resetPosition();
ECL_INFO("EKF commencing external vision position fusion");
}
// reset the position if we are not already aiding using GPS, else use a relative position
// method for fusing the position data
if (_control_status.flags.gps) {
_fuse_hpos_as_odom = true;
} else {
resetPosition();
resetVelocity();
// turn on use of external vision measurements for velocity
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
_control_status.flags.ev_vel = true;
resetVelocity();
ECL_INFO("EKF commencing external vision velocity fusion");
}
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
&& !_ev_rot_mat_initialised) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
_ev_rot_mat_initialised = true;
}
}
}
@@ -332,7 +325,7 @@ void Ekf::controlExternalVisionFusion()
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {
// don't reset velocity if we have another source of aiding constraining it
if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) {
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) {
resetVelocity();
}
@@ -340,16 +333,55 @@ void Ekf::controlExternalVisionFusion()
}
}
// innovation gate size
_posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f);
}else{
_vel_pos_innov[3] = 0.0f;
_vel_pos_innov[4] = 0.0f;
}
// determine if we should use the velocity observations
if (_control_status.flags.ev_vel) {
_fuse_hor_vel = true;
_fuse_vert_vel = true;
Vector3f velNED_aligned{_ev_sample_delayed.velNED};
// rotate measurement into correct earth frame if required
if (_params.fusion_mode & MASK_ROTATE_EV) {
velNED_aligned = _ev_rot_mat * _ev_sample_delayed.velNED;
}
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
velNED_aligned -= vel_offset_earth;
_vel_pos_innov[0] = _state.vel(0) - velNED_aligned(0);
_vel_pos_innov[1] = _state.vel(1) - velNED_aligned(1);
_vel_pos_innov[2] = _state.vel(2) - velNED_aligned(2);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {
// don't reset velocity if we have another source of aiding constraining it
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) {
resetVelocity();
}
}
// observation 1-STD error
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = fmaxf(_ev_sample_delayed.velErr, 0.01f);
// innovation gate size
_posInnovGateNE = fmaxf(_params.ev_innov_gate, 1.0f);
_vvelInnovGate = _hvelInnovGate = fmaxf(_params.ev_vel_innov_gate, 1.0f);
}
// Fuse available NED position data into the main filter
if (_fuse_height || _fuse_pos) {
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_vert_vel = _fuse_hor_vel = false;
_fuse_pos = _fuse_height = false;
_fuse_hpos_as_odom = false;
@@ -361,12 +393,13 @@ void Ekf::controlExternalVisionFusion()
}
} else if (_control_status.flags.ev_pos
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
&& (_time_last_imu >= _time_last_ext_vision)
&& ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false;
_control_status.flags.ev_vel = false;
_control_status.flags.ev_yaw = false;
ECL_INFO("EKF External Vision Data Stopped");
@@ -688,7 +721,7 @@ void Ekf::controlGpsFusion()
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
// calculate innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
@@ -1627,6 +1660,7 @@ void Ekf::controlVelPosFusion()
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
!_control_status.flags.ev_vel &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
// We now need to use a synthetic position observation to prevent unconstrained drift of the INS states.
@@ -1690,7 +1724,8 @@ void Ekf::controlAuxVelFusion()
_fuse_hor_vel_aux = true;
_aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
_velObsVarNE = _auxvel_sample_delayed.velVarNE;
_velObsVarNED(0) = _auxvel_sample_delayed.velVarNE(0);
_velObsVarNED(1) = _auxvel_sample_delayed.velVarNE(1);
_hvelInnovGate = _params.auxvel_gate;
fuseVelPosHeight();
}