本帖最后由 wilsonleong 于 2020-8-13 11:06 编辑
如果 MASK_ROTATE_EV 和 MASK_USE_EVYAW 同时启用，EKF2只会算校准一次外部视觉的旋转矩阵，后面就不再校准了。具体是，程序是否进行外部视觉旋转矩阵校准是检查 _control_status.flags.ev_yaw 而不是 MASK_USE_EVYAW，所以刚开始 _control_status.flags.ev_yaw 为 false，程序进行了一次校准。到后面 MASK_USE_EVYAW 把 _control_status.flags.ev_yaw 设置为 true，就不再进行校准了。
但是这样不知道会不会对yaw角融合有什么影响？似乎是这个外部视觉的旋转矩阵只会拿去乘以外部视觉的位置，而外部视觉的姿态不做额外处理。wiki上说这两个选项不要同时启用的
Either bit 4 (EV_YAW) or bit 6 (EV_ROTATE) should be set to true, but not both together.
EKF\control.cpp (1.10)
void Ekf::controlExternalVisionFusion()
{
// Check for new external vision data
if (_ev_data_ready) {
// 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) && ((_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();
}
// external vision aiding selection logic
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
/* ...... */
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;
ECL_INFO_TIMESTAMPED("EKF external vision aligned");
}
}
}
// external vision yaw aiding selection logic
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// don't start using EV data unless daa is arriving frequently
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
/* ...... */
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_dec = false;
/* ...... */
}
}
/* ...... */
// determine if we should use the yaw observation
if (_control_status.flags.ev_yaw) {
fuseHeading();
}
} 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_TIMESTAMPED("EKF External Vision Data Stopped");
}
}
EKF\control.cpp (1.11)
void Ekf::controlExternalVisionFusion()
{
// Check for new external vision data
if (_ev_data_ready) {
// 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) && ((_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();
}
/* ...... */
// external vision yaw aiding selection logic
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// don't start using EV data unless data is arriving frequently
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
// reset the yaw angle to the value from the vision quaternion
const Eulerf euler_obs(_ev_sample_delayed.quat);
const float yaw = euler_obs(2);
const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
resetQuatStateYaw(yaw, yaw_variance, true);
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_dec = false;
stopMagHdgFusion();
stopMag3DFusion();
ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
}
}
/* ...... */
// determine if we should use the yaw observation
if (_control_status.flags.ev_yaw) {
fuseHeading();
}
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
&& isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received
stopEvFusion();
ECL_INFO_TIMESTAMPED("vision data stopped");
}
}
| 
发表于 2020-8-13 10:32:57
4494 浏览 5 回复
