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

    文章目录

    本篇博客主要是记录自己的一些思考：在惯性卫星组合导航系统中，如果以姿态四元数作为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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

    其中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]);

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38

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

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

    在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);

}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    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);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    / 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
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99

    参考

    [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

  • 相关阅读:
    热更新简述
    再再肝3天，整理了70个Python面向对象编程案例，怎能不收藏？
    《论文笔记》Multi-UAV Collaborative Monocular SLAM
    业务中我们如何更新缓存？Redis
    室友打了一晚上游戏我学会了使用GDB调试
    一种基于遗传算法与神经网络算法(GA-BP)的新冠肺炎模型预测-含Matlab代码
    Redis高级
    备战金九银十，为Java岗准备全套架构视频 +大厂面经+面试专题
    APP上架需要的准备和流程
    知识分享 钡铼网关功能介绍：使用SSLTLS 加密，保证MQTT通信安全
  • 原文地址：https://blog.csdn.net/weixin_42918498/article/details/126473809