void NavEKF3_core::initialiseQuatCovariances(const Vector3f &rotVarVec)
{
//根据四元数计算等效旋转向量------ calculate an equivalent rotation vector from the quaternion
float q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3];
if (q0 < 0)
{
q0 = -q0;
q1 = -q1;
q2 = -q2;
q3 = -q3;
}
float delta = 2.0facosf(q0);
float scaler;
if (fabsf(delta) > 1e-6f)
{
scaler = (delta/sinf(delta0.5f));
} else
{
scaler = 2.0f;
}
float rotX = scalerq1;
float rotY = scalerq2;
float rotZ = scaler*q3;
//用matlab符号工具箱生成自动代码----- autocode generated using matlab symbolic toolbox
float t2 = rotX*rotX;
float t4 = rotY*rotY;
float t5 = rotZ*rotZ;
float t6 = t2+t4+t5; //rotX*rotX+rotY*rotY+rotZ*rotZ
if (t6 > 1e-9f)
{
float t7 = sqrtf(t6); //t7=sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)
float t8 = t7*0.5f; //t8=1/2*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)
float t3 = sinf(t8); //t3=sin[1/2*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)]
float t9 = t3*t3; //t9=sin2[1/2*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)]
float t10 = 1.0f/t6; //t10=1/(rotX*rotX+rotY*rotY+rotZ*rotZ)
float t11 = 1.0f/sqrtf(t6); //t11=1.0f/sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)
float t12 = cosf(t8); //t12=cosf[1/2*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)]
float t13 = 1.0f/powf(t6,1.5f); //t13=1/(rotX*rotX+rotY*rotY+rotZ*rotZ)^3/2
float t14 = t3*t11; //sin[1/2*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)]*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)
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*rotVarVec.y*t3*t11*t28*0.5f;
float t25 = rotX*rotZ*t10*t12*0.5f;
float t31 = t16-t25;
float t21 = rotZ*rotVarVec.z*t3*t11*t31*0.5f;
float t22 = t20+t21-rotX*rotVarVec.x*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*rotVarVec.z*t3*t11*t39*0.5f;
float t37 = t15-t23;
float t38 = t17-t36;
float t41 = rotVarVec.x*(t15-t23)*(t16-t25);
float t42 = t41-rotVarVec.y*t30*t39-rotVarVec.z*t33*t39;
float t43 = t16-t25;
float t44 = t17-t36;
//将所有四元数协方差归零---- zero all the quaternion covariances
zeroRows(P,0,3);
zeroCols(P,0,3);
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
//使用matlab符号工具箱生成的自动代码更新四元数内部协方差
////t9=sin2[1/2*sqrtf(rotX*rotX+rotY*rotY+rotZ*rotZ)]
//t10=1/(rotX*rotX+rotY*rotY+rotZ*rotZ)
//t2=rotX*rotX
P[0][0] = rotVarVec.x*t2*t9*t10*0.25f+rotVarVec.y*t4*t9*t10*0.25f+rotVarVec.z*t5*t9*t10*0.25f;
P[0][1] = t22;
P[0][2] = t35+rotX*rotVarVec.x*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarVec.y*t3*t11*t30*0.5f;
P[0][3] = rotX*rotVarVec.x*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarVec.y*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarVec.z*t3*t11*t33*0.5f;
P[1][0] = t22;
P[1][1] = rotVarVec.x*(t19*t19)+rotVarVec.y*(t24*t24)+rotVarVec.z*(t26*t26);
P[1][2] = rotVarVec.z*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarVec.x*t19*t28-rotVarVec.y*t28*t30;
P[1][3] = rotVarVec.y*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarVec.x*t19*t31-rotVarVec.z*t31*t33;
P[2][0] = t35-rotY*rotVarVec.y*t3*t11*t30*0.5f+rotX*rotVarVec.x*t3*t11*(t15-t23)*0.5f;
P[2][1] = rotVarVec.z*(t16-t25)*(t17-t36)-rotVarVec.x*t19*t28-rotVarVec.y*t28*t30;
P[2][2] = rotVarVec.y*(t30*t30)+rotVarVec.x*(t37*t37)+rotVarVec.z*(t38*t38);
P[2][3] = t42;
P[3][0] = rotZ*rotVarVec.z*t3*t11*t33*(-0.5f)+rotX*rotVarVec.x*t3*t11*(t16-t25)*0.5f+rotY*rotVarVec.y*t3*t11*(t17-t36)*0.5f;
P[3][1] = rotVarVec.y*(t15-t23)*(t17-t36)-rotVarVec.x*t19*t31-rotVarVec.z*t31*t33;
P[3][2] = t42;
P[3][3] = rotVarVec.z*(t33*t33)+rotVarVec.x*(t43*t43)+rotVarVec.y*(t44*t44);
} else
{
// the equations are badly conditioned so use a small angle approximation
//这些方程条件很差，所以使用小角度近似
P[0][0] = 0.0f;
P[0][1] = 0.0f;
P[0][2] = 0.0f;
P[0][3] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.25f*rotVarVec.x;
P[1][2] = 0.0f;
P[1][3] = 0.0f;
P[2][0] = 0.0f;
P[2][1] = 0.0f;
P[2][2] = 0.25f*rotVarVec.y;
P[2][3] = 0.0f;
P[3][0] = 0.0f;
P[3][1] = 0.0f;
P[3][2] = 0.0f;
P[3][3] = 0.25f*rotVarVec.z;
}
}