You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
62 lines
2.0 KiB
62 lines
2.0 KiB
%% 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');
|
|
|