捷联惯导系统学习2.5（等效旋转矢量微分方程）

已知三维旋转矢量关系如下：（证明略）
参数说明：
$V_i$表示三维空间矢量
$v=|V|=\sqrt{V{V}^T}$表示矢量模值
$u$为与V同方向的单位矢量即：$|u|=1$
(1)$V\dot{v}=\frac{V\times V\times \dot{V}}{v}+v\dot{V}$
(2)$\dot{u}=-\frac{V\times V\times \dot{V}}{v^3}$
(3)$\dot{u}\times u=\frac{\dot{V}\times V}{v^2}$
(4)$\varphi =\theta u$;$\varphi$为等效旋转矢量$\theta =|\varphi |$
已知：$\dot{Q}=Q。w$( Q 为4元数;。为4元数乘法;w为b系绕i系的转动速度)
得到：
$$w=u\dot{\theta }+\dot{u}sin\theta +\dot{u}\times u(1-cos\theta )$$
根据公式（1）（4）得到：
$$u\dot{\theta }=\frac{\varphi \times \varphi \times \varphi }{{\theta }^2}$$
根据公式（2）得到：
$$\dot{u}=-\frac{\varphi \times \varphi \times \dot{\varphi }}{{\theta }^3}$$
根据公式（3）得到：
$$\dot{u}\times u=\frac{\dot{\varphi }\times \varphi }{{\theta }^2}$$
令$a=-\frac{1-cos\theta }{{\theta }^2};b=\frac{(1-\frac{sin\theta }{\theta })}{{\theta }^2}$将上述带入原式，解得为：
$$w=\dot{\varphi }+a\varphi \times \dot{\varphi }+b(\varphi \times {)}^2\dot{\varphi }$$
将上述公式两边，左乘$\varphi \times$,$(\varphi \times {)}^2$,可以得到如下矩阵（推导过程略）：得到
Bortz方程：
$$\dot{\varphi }=w+\frac{1}{2}\varphi \times w+\frac{1}{{\theta }^2}(1-\frac{\theta }{2}cot\frac{\theta }{2})(\varphi \times {)}^{2}w$$
展开$cot\frac{\theta }{2}$：
应满足如下条件：(1)$\theta =|\varphi |$为向量时，将$cot\frac{\theta }{2}$用泰勒级数并忽略高次项

$$\dot{\varphi }=w+\frac{1}{2}\varphi \times w+\frac{1}{{\theta }^2}(1-\frac{\theta }{2}(\frac{2}{\theta }-\frac{1}{3}\frac{\theta }{2}-\frac{1}{45}(\frac{\theta }{2}{)}^3-...))(\varphi \times {)}^{2}w$$
$$\approx w+\frac{1}{2}\varphi \times w$$
当$t_1,t_2\in [t_{m-1},t_m]$
$\varphi (t)$从$t_{m-1}$时刻开始的等效旋转矢量的增量
$$\varphi (t)=\Delta \theta (t)+\sigma (t)$$
$$\Delta \theta (t)={\int }_0^{t}w(t_1)d{t}_1(不可交换误差)$$
$$\sigma (t)={\int }_0^t\Delta \theta (t_1)\times w(t_1)d{t}_1$$
$$\dot{\varphi }=w(t)+\frac{1}{2}\Delta \varphi (t)\times w(t)$$