fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 14:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: derive auto-code for quaternion state covariance initialisation

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-17 12:28:58 +10:00
parent 88860d0307
commit a4728bc748
2 changed files with 86 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m
+27 -5
View File
@@ -1,4 +1,4 @@
% calculate the variances for an equivalent rotation vector
%% calculate the rotation vector variances from an equivalent quaternion
% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions
% output is a vector of variances for the rotation vector that is equivalent to the quaternion
clear all;
@@ -10,10 +10,7 @@ quat = [q0;q1;q2;q3];
% convert to a rotation vector
delta = 2*acos(q0);
rotX = delta*(q1/sin(delta/2));
rotY = delta*(q2/sin(delta/2));
rotZ = delta*(q3/sin(delta/2));
rotVec = [rotX;rotY;rotZ];
rotVec = (delta/sin(delta/2))*[q1;q2;q3];
% calculate transfer matrix from quaternion to rotation vector
G = jacobian(rotVec, quat);
@@ -38,3 +35,28 @@ rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)];
% convert to c-code
ccode(rotVarVec,'file','rotVarVec.c');
%% calculate the quaternion variances from an equivalent rotation vector
% define a rotation vector
syms rotX rotY rotZ real;
rotVec = [rotX;rotY;rotZ];
% convert to a quaternion
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
quat = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
% calculate transfer matrix from rotation vector to quaternion
G = jacobian(quat, rotVec);
% define the rotation vector variances
syms rotVarX rotVarY rotVarZ real;
% define the rotation vector covariance matrix
rotCovMat = diag([rotVarX;rotVarY;rotVarZ]);
% rotate the covariance matrix into quaternion coordinates
quatCovMat = G*rotCovMat*transpose(G);
% convert to c-code
ccode(quatCovMat,'file','quatCovMat.c');
matlab/scripts/Inertial Nav EKF/quatCovMat.c
+59
View File
@@ -0,0 +1,59 @@
float t2 = rotX*rotX;
float t4 = rotY*rotY;
float t5 = rotZ*rotZ;
float t6 = t2+t4+t5;
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*rotVarY*t3*t11*t28*0.5f;
float t25 = rotX*rotZ*t10*t12*0.5f;
float t31 = t16-t25;
float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f;
float t22 = t20+t21-rotX*rotVarX*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*rotVarZ*t3*t11*t39*0.5f;
float t37 = t15-t23;
float t38 = t17-t36;
float t41 = rotVarX*(t15-t23)*(t16-t25);
float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39;
float t43 = t16-t25;
float t44 = t17-t36;
P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f;
P[0][1] = t22;
P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f;
P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f;
P[1][0] = t22;
P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26);
P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30;
P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33;
P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f;
P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30;
P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38);
P[2][3] = t42;
P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f;
P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33;
P[3][2] = t42;
P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44);