本篇博客主要是记录自己的一些思考：在惯性卫星组合导航系统中，如果以姿态四元数作为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 variances
error_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:3
output.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:41
t8 = 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();
// velocity
P(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);
// position
P(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 bias
P(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();
// wind
P(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 vector
Vector3f 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 quaternion
float 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 toolbox
float 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 covariances
P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
P(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 approximation
P.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
331
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
