forked from Archive/PX4-Autopilot
63 lines
2.0 KiB
Matlab
63 lines
2.0 KiB
Matlab
%% 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;
|
|
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);
|
|
rotVec = (delta/sin(delta/2))*[q1;q2;q3];
|
|
|
|
% 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');
|
|
|
|
%% 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');
|