旋轉向量與姿態矩陣之間相互轉換的數值計算問題分析
1、概述
在做新算法期間仔細考量了嚴老師的psins工具箱中關于旋轉向量RV和姿態矩陣M的轉換關系，其中的數值計算存在分母為零的問題，為了更穩定的結果可以做一些變通，但是也存在類似問題。
常規的旋轉向量RV與姿態矩陣之間的關系[T1][C1],
m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2 （1）
其中存在sin(x)/x,[1-cos(x)]/x^2這兩個典型數值計算函數的問題。
2、傳統數值計算處理方案
sin(x)/x,[1-cos(x)]/x^2兩個函數的計算一般采取泰勒展開形式，定下一個小量閾值eps,小于這個eps閾值采取泰勒分解，把分母消去,代碼參見附錄[C1]。
3、矩陣近似處理方案
對于頻繁的這種近似數值計算，可能也是有代價的，為了避開這個方案，需要采取一些新的思路[T2],使用矩陣指數計算方法。Matlab提供了矩陣指數計算expm[T3]，底層采用了Pade近似方法[T4][C2]，避開分母為零方法，
4、解決數值穩定性
Pade數值計算穩定性應該更好。
[T1]: 捷聯慣導算法與組合導航原理(2nd),嚴恭敏，chap2.2,p12,(2.2.23)
[T2]:捷聯慣導算法與組合導航原理(2nd),嚴恭敏，chap2.1.2,p9,(2.1.20)
[T3]:https://ww2.mathworks.cn/help/matlab/ref/expm.html
[T3]: Matrix Computations(3rd),Gene H.Golub.Charles F.Van Loan,chap11.3.1,p572
附錄代碼：
[代碼C1]：
function m = rv2m(rv)
% Convert rotation vector to transformation matrix.
%
% Prototype: m = rv2m(rv)
% Input: rv - rotation vector
% Output: m - corresponding DCM, such that
% m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2
% where rvx is the askew matrix or rv.
%
% See also m2rv, rv2q, q2rv, a2mat, rotv.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 05/02/2009, 22/05/2014
xx = rv(1)*rv(1); yy = rv(2)*rv(2); zz = rv(3)*rv(3);
n2 = xx+yy+zz;
if n2<1.e-8
a = 1-n2*(1/6-n2/120); b = 0.5-n2*(1/24-n2/720); % a->1, b->0.5
else
n = sqrt(n2);
a = sin(n)/n; b = (1-cos(n))/n2;
end
arvx = a*rv(1); arvy = a*rv(2); arvz = a*rv(3);
bxx = b*xx; bxy = b*rv(1)*rv(2); bxz = b*rv(1)*rv(3);
byy = b*yy; byz = b*rv(2)*rv(3); bzz = b*zz;
m = zeros(3,3);
% m = I + a*(rvx) + b*(rvx)^2;
m(1)=1 -byy-bzz; m(4)= -arvz+bxy; m(7)= arvy+bxz;
m(2)= arvz+bxy; m(5)=1 -bxx-bzz; m(8)= -arvx+byz;
m(3)= -arvy+bxz; m(6)= arvx+byz; m(9)=1 -bxx-byy;
[代碼C2]：
Matlab version E = expm(A)
A=[ -0.130000000000000 0.020000000000000 0.030000000000000
0.020000000000000 -0.100000000000000 0.060000000000000
0.030000000000000 0.060000000000000 -0.050000000000000];
% Scale A by power of 2 so that its norm is < 1/2 .
[f,e] = log2(norm(A,'inf'));
s = max(0,e+1);
A = A/2^s;
% Pade approximation for exp(A)
X = A;
c = 1/2;
E = eye(size(A)) + c*A;
D = eye(size(A)) - c*A;
q = 6;
p = 1;
for k = 2:q
c = c * (q-k+1) / (k*(2*q-k+1));
X = A*X;
cX = c*X;
E = E + cX;
if p
D = D + cX;
else
D = D - cX;
end
p = ~p;
end
E = D\E;
% Undo scaling by repeated squaring
for k = 1:s
E = E*E;
end
