捷联惯导系统学习2.2（等效旋转矢量）

二 等效旋转矢量：

1 一些重要的三维矢量运算关系（证明请自己找）

$ u为单位矢量 ;u’是u的一阶导数$
$(1):V_1\times (V_2\times V_3)=(V_1\ast V_3)V_2-(V_1\ast V_2)V_3$
$(2):(u\times )(u^{\prime }\times )(u\times )=(u^{\prime }\times )(u\times )(u^{\prime }\times )=0$
$(3):(V_2\times )(V_3\times )=-(V_1\ast V_2)+V_2{V}_1^T$
$(4):(V_1\times )(V_2\times )-(V_2\times )(V_1\times )=V_2{V}_1^T-V_1{V}_2^T=(V_1\times V_2)\times$
$(5):V_1\ast (V_2\times V_3)=V_2\ast (V_3\times V_1)=V_3\ast (V_1\times V_2)$

参数说明：
		u:作为旋转轴的单位矢量
		r:绕u旋转前的矢量 				OA=r
		r':绕u旋转后的矢量				OA'=r'
		r//:r在u上的投影				OO'=rr//
		r//':r'在u上的投影				OO'=rr//'
		r⊥：r相对于u的垂直分量	
		r'⊥;r’相对于u的垂直分量
		θ ：r与r'的夹角
		O'B⊥O'A: 					OB'=rxu

2 已知:（没看懂的把上边的图画一遍）
$r=\overrightarrow{O{O}^{\prime }}+\overrightarrow{O^{\prime }A}=r//+r\perp$
$r\perp =\overrightarrow{O^{\prime }B}\times u=(r\times u)u$
不难证明（将r’用r和u表示）：
$$\overrightarrow{O^{\prime }{A}^{\prime }}=\overrightarrow{O^{\prime }B}\ast cos\theta +\overrightarrow{O^{\prime }A}\ast sin\theta$$
$$（A）r^{\prime }=\overrightarrow{O^{\prime }O}+\overrightarrow{O^{\prime }{A}^{\prime }}=(r\ast u)u+(u\times r)\times u\ast cos\theta +u\times r\ast sin\theta$$

罗德里格旋转公式（如果r;r’分别为两个坐标系的单位矢量;D就是过渡矩阵P）
已知：
三重矢积公式:$V\ast V\ast V_3=V\times V\times V_3+v^2\ast V_3;$
得到：$r\ast u\ast u=u\ast r\ast u=(u\times u)\times r+v^2\ast r=(I+(u\times {)}^2)r;$
带入公式：A
$$r^{\prime }=$$$$\overrightarrow{O^{\prime }O}+\overrightarrow{O^{\prime }{A}^{\prime }}=(r\ast u)u+(u\times r)\times u\ast cos\theta +u\times r\ast sin\theta$$
$$=(I+(u\times {)}^2)r+(r\times (u\times u)-u\times u\times r)r\ast cos\theta +u\times r\ast sin\theta$$
$$=(I-sin\theta \ast (u\times )+(1-cos\theta )(u\times {)}^2)r=D\ast r$$
$$D=[I+sin\theta \ast (u\times )+(1-cos\theta )(u\times {)}^2](D为罗德里格旋转公式)$$
等效旋转矢量
$$\phi =\theta \ast u(其中：\phi =|\theta |)$$
当从坐标系b系到i系的时候 $D=C_b^i$
将D用等效旋转矢量时表示如下$u=\frac{\phi }{\theta }$：
$$D=C_b^i=[I+\frac{sin\theta \ast (\phi \times )}{\theta }+\frac{(1-cos\theta )(\phi \times {)}^2}{{\theta }^2}]=M_{RV}(\phi )$$

旋转矩阵（Givens矩阵）
取：$${\phi }_1=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]$$
$${\phi }_2=\left[\begin{array}{ccc}0& 1& 0\end{array}\right]$$
$${\phi }_3=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]$$
带入$M_{RV}(\phi$)中得到三个矩阵，成为旋转矩阵（Gives矩阵）
求解等效旋转矢量
$tr(C_b^i)={\lambda }_1+{\lambda }_2+{\lambda }_3=1+2cos\theta$
$C_b^i-(C_b^i{)}^T=2\frac{sin\theta }{\theta }(\phi \times )$
可得
$$\theta =arcos\frac{tr(C_b^i)-1}{2}$$
$$\phi \times =\frac{\theta }{2sin\theta }[C_b^i-(C_b^i{)}^T]$$

利用$\phi \times$即可求得$\phi$