关于四元数与欧拉角协方差阵转换的一些思考

文章目录

惯性卫星组合模型
四元数与欧拉角之间的协方差阵转换 - 理论
四元数与欧拉角之间的协方差阵转换 - 代码
姿态四元数的初始协方差阵设置
参考

本篇博客主要是记录自己的一些思考：在惯性卫星组合导航系统中，如果以姿态四元数作为EKF的状态量，该如何设置初始协方差阵，以及如何利用四元数的协方差阵得到姿态角协方差阵的问题。

惯性卫星组合模型

在惯性卫星组合导航中，不同的算法选取不同的卡尔曼滤波状态量：

如PSINS的test_SINS_GPS_153.m，该demo中状态量为15维（包括失准角3维、速度误差3维、位置误差3维、陀螺常值误差3维、加速度计常值误差3维）
而在PX4 ECL中如果仅考虑GPS，忽略其他传感器，那么EKF的状态量为16维（姿态四元数4维、速度误差3维、位置误差3维、陀螺常值误差3维、加速度计常值误差3维）
当然还有其它模型，如乘性四元数误差做为状态量的模型，卡尔曼滤波状态量15维（姿态四元数误差3维、速度误差3维、位置误差3维、陀螺常值误差3维、加速度计常值误差3维）

对于姿态来说，采用失准角作为卡尔曼滤波状态量，在进行初始值以及不确定度的设置（即初始的协方差阵P）时都有比较明确的物理意义。

如果采用四元数作为EKF的状态量：

1.初始值的设置比较明确，可以通过初始对准后的姿态角直接计算得到初始的姿态四元数
2. 初始姿态四元数的协方差阵应该如何计算呢？
3. EKF滤波后，如何通过四元数状态量的协方差阵计算得到姿态角的协方差阵呢？

四元数与欧拉角之间的协方差阵转换 - 理论

在3-2-1的旋转顺序下，四元数与欧拉角之间的旋转关系如下[1]：

参考问题what-does-the-covariance-of-a-quaternion-mean的回答[2]，以及参考原文献[3]，要利用四元数的协方差阵计算姿态角协方差，需要根据上述转换公式，计算姿态角相对于四元数的偏导，形成雅可比矩阵，并在四元数的协方差阵上左乘该雅可比矩阵，右乘雅可比矩阵的转置。

原文献如下所示，不过需要注意的是该文献中的旋转顺序为3-1-2，所以雅可比矩阵与3-2-1不同，但是计算的思想是一致的。

四元数与欧拉角之间的协方差阵转换 - 代码

与上述理论一致，在PX4 ECL的matlab脚本RunFilter.m中可以看到，该仿真代码通过如下方式，利用姿态四元数的协方差阵计算欧拉角的协方差阵：

        % output equivalent euler angle varianceserror_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);for i=1:3output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);end

其中quat_to_euler_error_transfer_matrix函数代码如下：

function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3)
%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX
%    ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3)%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    13-Jul-2017 08:41:41t8 = q0.*q1.*2.0;
t9 = q2.*q3.*2.0;
t2 = t8+t9;
t4 = q0.^2;
t5 = q1.^2;
t6 = q2.^2;
t7 = q3.^2;
t3 = t4-t5-t6+t7;
t10 = t3.^2;
t11 = t2.^2;
t12 = t10+t11;
t13 = 1.0./t12;
t14 = 1.0./t3;
t15 = 1.0./t3.^2;
t17 = q0.*q2.*2.0;
t18 = q1.*q3.*2.0;
t16 = t17-t18;
t19 = t16.^2;
t20 = -t19+1.0;
t21 = 1.0./sqrt(t20);
t24 = q0.*q3.*2.0;
t25 = q1.*q2.*2.0;
t22 = t24+t25;
t23 = t4+t5-t6-t7;
t26 = t23.^2;
t27 = t22.^2;
t28 = t26+t27;
t29 = 1.0./t28;
t30 = 1.0./t23;
t31 = 1.0./t23.^2;
error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]);

姿态四元数的初始协方差阵设置

关于如何设置姿态四元数的初始协方差阵，目前还没有找到理论相关的文献。大多数使用四元数作为状态量的论文中，没有解释就直接设了初始不确定度，或者几乎不提及这个问题。如果有同学看到相关理论的参考文献，欢迎交流。

在PX4 ECL的matlab代码，给出的参数也是直接设置四元数的不确定度，而在C代码中给出的参数是设置初始对准后的角度不确定度，再根据角度不确定度来设置四元数的协方差阵，这给了我们一个参考方向。

相关代码如下，可以看到：

在covariance.cpp中，初始化协方差函数initialiseCovariance调用resetQuatCov函数
而resetQuatCov函数根据设置的初始对准角度误差不确定_params.initial_tilt_err，利用在ekf_helper.cpp中的initialiseQuatCovariances函数来计算四元数的协方差阵
观察initialiseQuatCovariances可以发现，此时四元数的协方差阵并不是一个对角阵
initialiseQuatCovariances函数的输入实际上并不是姿态角的不确定度，而是旋转矢量的不确定度

// Sets initial values for the covariance matrix
// Do not call before quaternion states have been initialised
void Ekf::initialiseCovariance()
{P.zero();_delta_angle_bias_var_accum.setZero();_delta_vel_bias_var_accum.setZero();const float dt = FILTER_UPDATE_PERIOD_S;resetQuatCov();// velocityP(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f));P(5,5) = P(4,4);P(6,6) = sq(1.5f) * P(4,4);// positionP(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));P(8,8) = P(7,7);if (_control_status.flags.rng_hgt) {P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));} else if (_control_status.flags.gps_hgt) {P(9,9) = getGpsHeightVariance();} else {P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));}// gyro biasP(10,10) = sq(_params.switch_on_gyro_bias * dt);P(11,11) = P(10,10);P(12,12) = P(10,10);// accel bias_prev_dvel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias * dt);_prev_dvel_bias_var(1) = P(14,14) = P(13,13);_prev_dvel_bias_var(2) = P(15,15) = P(13,13);resetMagCov();// windP(22,22) = sq(_params.initial_wind_uncertainty);P(23,23) = P(22,22);}

void Ekf::resetQuatCov(){zeroQuatCov();// define the initial angle uncertainty as variances for a rotation vectorVector3f rot_vec_var;rot_vec_var.setAll(sq(_params.initial_tilt_err));initialiseQuatCovariances(rot_vec_var);
}

/ initialise the quaternion covariances using rotation vector variances
// do not call before quaternion states are initialised
void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
{// calculate an equivalent rotation vector from the quaternionfloat q0,q1,q2,q3;if (_state.quat_nominal(0) >= 0.0f) {q0 = _state.quat_nominal(0);q1 = _state.quat_nominal(1);q2 = _state.quat_nominal(2);q3 = _state.quat_nominal(3);} else {q0 = -_state.quat_nominal(0);q1 = -_state.quat_nominal(1);q2 = -_state.quat_nominal(2);q3 = -_state.quat_nominal(3);}float delta = 2.0f*acosf(q0);float scaler = (delta/sinf(delta*0.5f));float rotX = scaler*q1;float rotY = scaler*q2;float rotZ = scaler*q3;// autocode generated using matlab symbolic toolboxfloat t2 = rotX*rotX;float t4 = rotY*rotY;float t5 = rotZ*rotZ;float t6 = t2+t4+t5;if (t6 > 1e-9f) {float t7 = sqrtf(t6);float t8 = t7*0.5f;float t3 = sinf(t8);float t9 = t3*t3;float t10 = 1.0f/t6;float t11 = 1.0f/sqrtf(t6);float t12 = cosf(t8);float t13 = 1.0f/powf(t6,1.5f);float t14 = t3*t11;float t15 = rotX*rotY*t3*t13;float t16 = rotX*rotZ*t3*t13;float t17 = rotY*rotZ*t3*t13;float t18 = t2*t10*t12*0.5f;float t27 = t2*t3*t13;float t19 = t14+t18-t27;float t23 = rotX*rotY*t10*t12*0.5f;float t28 = t15-t23;float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;float t25 = rotX*rotZ*t10*t12*0.5f;float t31 = t16-t25;float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;float t24 = t15-t23;float t26 = t16-t25;float t29 = t4*t10*t12*0.5f;float t34 = t3*t4*t13;float t30 = t14+t29-t34;float t32 = t5*t10*t12*0.5f;float t40 = t3*t5*t13;float t33 = t14+t32-t40;float t36 = rotY*rotZ*t10*t12*0.5f;float t39 = t17-t36;float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;float t37 = t15-t23;float t38 = t17-t36;float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;float t43 = t16-t25;float t44 = t17-t36;// zero all the quaternion covariancesP.uncorrelateCovarianceSetVariance<2>(0, 0.0f);P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolboxP(0,0) = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;P(0,1) = t22;P(0,2) = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;P(0,3) = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;P(1,0) = t22;P(1,1) = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);P(1,2) = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;P(1,3) = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;P(2,0) = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;P(2,1) = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;P(2,2) = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);P(2,3) = t42;P(3,0) = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;P(3,1) = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;P(3,2) = t42;P(3,3) = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);} else {// the equations are badly conditioned so use a small angle approximationP.uncorrelateCovarianceSetVariance<1>(0, 0.0f);P.uncorrelateCovarianceSetVariance<3>(1, 0.25f * rot_vec_var);}
}

参考

[1] https://handwiki.org/wiki/Conversion_between_quaternions_and_Euler_angles
[2] https://stats.stackexchange.com/questions/119780/what-does-the-covariance-of-a-quaternion-mean
[3] https://www.ucalgary.ca/engo_webdocs/GL/96.20096.JSchleppe.pdf