姿态解算(一)，坐标系变换

$b$系:body坐标系
$n$系:navigation坐标系，即NED(北东地)坐标系
$C_b^n:$从b系 转换到n系
$C_n^b:$从n系 转换到b系
四元数$q=[w,x,y,z]$

矢量$v^b$可以直接利用四元数将其在n系中表示为$v^n$. 首先定义一个四元数$v^{b'}$，它的虚部等于$v^b$的相应分量，标量分量为0，即：

$$v^{b} = xi+yj+zk$$
$$v^{b'} = 0+xi+yj+zk=[0~x~y~z]$$
则$v^{n'}$可以表示为:
$$v^{n'}=qv^{b'}q^*$$
其中$q^*=[w,-x,-y,-z]$,为q的复共轭.
$v^{n'}$可以写成矩阵形式：
$$v^{n'}= \begin{bmatrix} 0& 0\\ 0& C \end{bmatrix}v^{b'}\tag 1$$
其中
$$C=\begin{bmatrix} (w^2+x^2-y^2-z^2) & 2(xy-wz) & 2(xz+wy) \\ 2(xy+wz) & (w^2-x^2+y^2-z^2) & 2(yz-wx) \\ 2(xz-wy) & 2(yz+wx) & (w^2-x^2-y^2+z^2) \end{bmatrix}$$
若满足$|q|=1$，则C可以进一步简化为：
$$C=\begin{bmatrix} (1-2y^2-2z^2) & 2(xy-wz) & 2(xz+wy) \\ 2(xy+wz) & (1-2x^2-2z^2) & 2(yz-wx) \\ 2(xz-wy) & 2(yz+wx) & (1-2x^2-2y^2) \end{bmatrix}$$
(1)式等价为$v^{n}=Cv^{b}$,即
$$C_b^n=C$$
根据$v^{b'}=q^*v^{n'}q$，类似可得$v^{b}=C^Tv^{n}$,即 $$C_n^b=C^T$$
$$C^T=\begin{bmatrix} (1-2y^2-2z^2) &2(xy+wz) & 2(xz-wy) \\ 2(xy-wz) & (1-2x^2-2z^2) & 2(yz+wx) \\ 2(xz+wy) & 2(yz-wx) & (1-2x^2-2y^2)\end{bmatrix}$$