方向余弦阵、四元数、等效旋转矢量的关系和不可交换误差的分析

四元数、等效旋转矢量意义及其原理

四元数：因为旋转轴的位置（定轴或不定轴）、旋转角度是四个数，故而矩阵的变换可以由四个数表示，即四元数的表达矩阵旋转形式

等效旋转矢量：矩阵沿着旋转轴u（定轴或不定轴）旋转一定的角度（白phi）的到新的矩阵，进而可以的到一种新的矩阵变化的关系表达式，即等效旋转矢量（黑phi）

u为旋转轴，白phi为旋转的角度，黑phi为等效旋转矢量。对上面两个公式进行分析：
1.当u为定轴时，此时不存在不可交换误差，此时的黑phi为角速度的变化值。
2.当u为不定轴时，此时的存在不可交换误差，u和四元数Q都是变化值，而非常数。

欧拉角A、变换矩阵、四元数、等效旋转矢量关系

假设:欧拉角A、变换矩阵、四元数、等效旋转矢量为：

，，，

则有：

故而可以通过三者任意一个可以求得其他两个的

function Cnb = q2mat(qnb)  % 四元数转换为姿态阵
    q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
    q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);     
    q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);  
    q44 = qnb(4)*qnb(4);
    Cnb = [ q11+q22-q33-q44,  2*(q23-q14),     2*(q24+q13);
            2*(q23+q14),      q11-q22+q33-q44, 2*(q34-q12);
            2*(q24-q13),      2*(q34+q12),     q11-q22-q33+q44 ];
end

function rv = q2rv(q) % 变换四元数转换为等效旋转矢量
	if q(1)<0,  q = -q;  end
    nmhalf = acos(q(1));  % 等效旋转矢量模值的一半
    if nmhalf>1e-20,   b = 2*nmhalf/sin(nmhalf);
    else             b = 2;    end
    rv = b*q(2:4);   % q = [ cos(|rv|/2); sin(|rv|/2)/|rv|*rv ];
end


function m = rv2m(rv)   % 等效旋转矢量转换为变换矩阵
	nm2 = rv'*rv;  % 旋转矢量的模方
    if nm2<1.e-8   % 如果模方很小，则可用泰勒展开前几项求三角函数
        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;
end

function q = rv2q(rv)  % 等效旋转矢量转换为变换四元数
    nm2 = rv'*rv;  % 旋转矢量的模方
    if nm2<1.0e-8  % 如果模方很小，则可用泰勒展开前几项求三角函数
        q0 = 1-nm2*(1/8-nm2/384); s = 1/2-nm2*(1/48-nm2/3840);
    else
        nm = sqrt(nm2);
        q0 = cos(nm/2); s = sin(nm/2)/nm;
    end
    q = [q0; s*rv];
end

Bortz方程和不可交换误差

利用四元数微分方程可以的得到

定义广义转动矢量：

将其两边微分

当f（phi）=phi时，可以得到Bortz，即g=黑phi

在利用将bortz方程微分，同时线性化w即可得到二子样的不可交换误差表达式

可以分析出，等效旋转矢量变化值是角速度的积分值和不可交换误差组成。