本帖最后由 AmovlabQYP 于 2020-8-5 09:47 编辑
首先看一下EKF2_AID_MASK的定义，rotate external vision的定义为set to true if the EV observations are in a non NED reference frame and need to be rotated before being used。
/**
* Integer bitmask controlling data fusion and aiding methods.
*
* Set bits in the following positions to enable:
* 0 : Set to true to use GPS data if available
* 1 : Set to true to use optical flow data if available
* 2 : Set to true to inhibit IMU delta velocity bias estimation
* 3 : Set to true to enable vision position fusion
* 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
* 5 : Set to true to enable multi-rotor drag specific force fusion
* 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
* 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
*
* @group EKF2
* @min 0
* @max 511
* @bit 0 use GPS
* @bit 1 use optical flow
* @bit 2 inhibit IMU bias estimation
* @bit 3 vision position fusion
* @bit 4 vision yaw fusion
* @bit 5 multi-rotor drag fusion
* @bit 6 rotate external vision
* @bit 7 GPS yaw fusion
* @bit 8 vision velocity fusion
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
找一下相关源码，可以看到官方的注释也是说需要计算这个转换矩阵，重点就在于看看calcExtVisRotMat();这个函数到底做了什么事情。
// 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();
}
来看看这个函数到底干了什么，加粗那句是重点
// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
void Ekf::calcExtVisRotMat()
{
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
q_error.normalize();
// convert to a delta angle and apply a spike and low pass filter
Vector3f rot_vec = q_error.to_axis_angle();
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-6f) {
// apply an input limiter to protect from spikes
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
float input_delta_len = _input_delta_vec.norm();
if (input_delta_len > 0.1f) {
rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);
}
// Apply a first order IIR low pass filter
const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec
float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us), 0.0f, 1.0f);
_ev_rot_last_time_us = _time_last_imu;
_ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
}
// convert filtered vector to a quaternion and then to a rotation matrix
q_error.from_axis_angle(_ev_rot_vec_filt);
_ev_rot_mat = quat_to_invrotmat(q_error); // rotation from EV reference to EKF reference
}
所以应该是，PX4会自动校准相机和机体坐标系之间的旋转矩阵。
但是，一般这个标志位我们不会用这个参数，所以不确定是否真的有用。 一般会把相机和无人机都水平放置，保证在一个平面内，然后选择@bit 3 vision position fusion和@bit 4 vision yaw fusion，即设置为24.这样我们就遵循相机的yaw角即可，就不是真正意义的NED坐标系了。
| 
发表于 2020-8-4 18:30:35
3662 浏览 6 回复
