mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 07:33:34 +08:00
Enable vision velocity fusion
This commit is contained in:
kamilritz
committed by
Paul Riseborough
Paul Riseborough
parent
149233a9ab
commit
3e8139ff9f
+6
-2
@@ -189,11 +189,12 @@ struct auxVelSample {
|
||||
#define MASK_USE_GPS (1<<0) ///< set to true to use GPS data
|
||||
#define MASK_USE_OF (1<<1) ///< set to true to use optical flow data
|
||||
#define MASK_INHIBIT_ACC_BIAS (1<<2) ///< set to true to inhibit estimation of accelerometer delta velocity bias
|
||||
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision NED position data
|
||||
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision position data
|
||||
#define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw
|
||||
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
|
||||
#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
|
||||
#define MASK_USE_EVVEL (1<<8) ///< sset to true to use external vision velocity data
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
@@ -299,7 +300,9 @@ struct parameters {
|
||||
float range_stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m)
|
||||
|
||||
// vision position fusion
|
||||
float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD)
|
||||
float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD)
|
||||
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
|
||||
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
|
||||
|
||||
// optical flow fusion
|
||||
float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
@@ -455,6 +458,7 @@ union filter_control_status_u {
|
||||
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
|
||||
uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused
|
||||
uint32_t mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed
|
||||
uint32_t ev_vel : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused
|
||||
} flags;
|
||||
uint32_t value;
|
||||
};
|
||||
|
||||
+68
-33
@@ -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();
|
||||
}
|
||||
|
||||
@@ -298,17 +298,19 @@ private:
|
||||
float _posObsNoiseNE{0.0f}; ///< 1-STD observation noise used for the fusion of NE position data (m)
|
||||
float _posInnovGateNE{1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check
|
||||
|
||||
Vector2f _velObsVarNE; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2
|
||||
float _hvelInnovGate{1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check
|
||||
Vector3f _velObsVarNED; ///< 1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2
|
||||
float _hvelInnovGate{1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check
|
||||
float _vvelInnovGate{1.0f}; ///< Number of standard deviations used for the vertical velocity fusion innovation consistency check
|
||||
|
||||
// variables used when position data is being fused using a relative position odometry model
|
||||
bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption
|
||||
Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m)
|
||||
Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
|
||||
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
|
||||
Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation from EKF to EV reference (rad)
|
||||
Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame
|
||||
uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the ekf to ev rotation matrix was updated (uSec)
|
||||
Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad)
|
||||
Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
|
||||
uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec)
|
||||
bool _ev_rot_mat_initialised{0}; ///< _ev_rot_mat should only be initialised once in the beginning through the reset function
|
||||
|
||||
// booleans true when fresh sensor data is available at the fusion time horizon
|
||||
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
|
||||
+1
-1
@@ -1336,7 +1336,7 @@ bool Ekf::global_position_is_valid()
|
||||
// return true if we are totally reliant on inertial dead-reckoning for position
|
||||
void Ekf::update_deadreckoning_status()
|
||||
{
|
||||
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
|
||||
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
||||
&& (((_time_last_imu - _time_last_pos_fuse) <= _params.no_aid_timeout_max)
|
||||
|| ((_time_last_imu - _time_last_vel_fuse) <= _params.no_aid_timeout_max)
|
||||
|| ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max));
|
||||
|
||||
@@ -67,8 +67,8 @@ void Ekf::fuseVelPosHeight()
|
||||
}
|
||||
|
||||
// Set observation noise variance and innovation consistency check gate size for the NE position observations
|
||||
R[0] = _velObsVarNE(0);
|
||||
R[1] = _velObsVarNE(1);
|
||||
R[0] = _velObsVarNED(0);
|
||||
R[1] = _velObsVarNED(1);
|
||||
gate_size[1] = gate_size[0] = _hvelInnovGate;
|
||||
|
||||
}
|
||||
@@ -76,12 +76,12 @@ void Ekf::fuseVelPosHeight()
|
||||
if (_fuse_vert_vel) {
|
||||
fuse_map[2] = true;
|
||||
// observation variance - use receiver reported accuracy with parameter setting the minimum value
|
||||
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
|
||||
R[2] = _velObsVarNED(2);
|
||||
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
|
||||
R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc);
|
||||
R[2] = R[2] * R[2];
|
||||
// innovation gate size
|
||||
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
|
||||
gate_size[2] = _vvelInnovGate;
|
||||
}
|
||||
|
||||
if (_fuse_pos) {
|
||||
@@ -152,7 +152,7 @@ void Ekf::fuseVelPosHeight()
|
||||
R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
|
||||
R[5] = R[5] * R[5];
|
||||
// innovation gate size
|
||||
gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f);
|
||||
gate_size[5] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
}
|
||||
|
||||
// update innovation class variable for logging purposes
|
||||
|
||||
Reference in New Issue
Block a user