forked from ZhanLi/UWBIns
19 lines
553 B
Mathematica
19 lines
553 B
Mathematica
|
function m = rv2m(rv)
|
|||
|
|
% <EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>תʸ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
%
|
|||
|
|
% Input: rv - <EFBFBD><EFBFBD>תʸ<EFBFBD><EFBFBD>
|
|||
|
|
% Output: m -<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD> Cb2n <EFBFBD>Ϲ<EFBFBD><EFBFBD><EFBFBD> 2.2.23
|
|||
|
|
% m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2
|
|||
|
|
% where rvx is the askew matrix or rv.
|
|||
|
|
|
|||
|
|
nm2 = rv'*rv; % <EFBFBD><EFBFBD>תʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>
|
|||
|
|
if nm2<1.e-8 % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̩<EFBFBD><EFBFBD>չ<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
a = 1-nm2*(1/6-nm2/120); b = 0.5-nm2*(1/24-nm2/720); % a->1, b->0.5
|
|||
|
|
else
|
|||
|
|
nm = sqrt(nm2);
|
|||
|
|
a = sin(nm)/nm; b = (1-cos(nm))/nm2;
|
|||
|
|
end
|
|||
|
|
VX = askew(rv);
|
|||
|
|
m = eye(3) + a*VX + b*VX^2;
|
|||
|
|
|
|||
|
|
|