fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 04:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add matlab derivation for calculation of rotation vector variance

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-17 10:49:02 +10:00
parent d5046b078e
commit 6ca8a36782
2 changed files with 59 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m Normal file
View File

@ -0,0 +1,40 @@
% calculate the variances for an equivalent rotation vector
% 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;
reset(symengine);
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
% define quaternion rotation
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];
% calculate transfer matrix from quaternion to rotation vector
G = jacobian(rotVec, quat);
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:4
for colIndex = 1:4
eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']);
eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']);
end
end
% rotate the covariance from quaternion to rotation vector
rotCovMat = G*quatCovMat*transpose(G);
% take the variances
rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)];
% convert to c-code
ccode(rotVarVec,'file','rotVarVec.c');

19
matlab/scripts/Inertial Nav EKF/rotVarVec.c Normal file
View File

@ -0,0 +1,19 @@
t2 = q0*q0;
t3 = acos(q0);
t4 = -t2+1.0;
t5 = t2-1.0;
t6 = 1.0/t5;
t7 = q1*t6*2.0;
t8 = 1.0/pow(t4,3.0/2.0);
t9 = q0*q1*t3*t8*2.0;
t10 = t7+t9;
t11 = 1.0/sqrt(t4);
t12 = q2*t6*2.0;
t13 = q0*q2*t3*t8*2.0;
t14 = t12+t13;
t15 = q3*t6*2.0;
t16 = q0*q3*t3*t8*2.0;
t17 = t15+t16;
A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0;
A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0;
A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0;