捷联惯导系统学习2.5（等效旋转矢量微分方程的泰勒级数解）

在高精度的捷联惯导系统中，陀螺仪姿态的解算往往是通过采集一定时间内的角增量信息， 计算角增量信息计算出等效旋转矢量，在通过等效旋转矢量递推余弦阵或者四元数，完成姿态更新。

等效旋转矢量微分方程的泰勒级数解 利用泰勒级，计算出数角增量的等效旋转矢量。

二字样法： 假设：0-T时间（4元数更新时间段）内陀螺仪为线性输出即： $$w(t)=a+2bt(t\in [0,T])$$

$\Delta \theta (t)={\int }_0^{t}w(t^{\prime })d{t}^{\prime }=at+b{t}^2$ $\sigma (t)=\Delta \theta (t)\times w(t)$ $\Delta \theta (t)$(角输出增量,2阶导以上全部为零(证略)); $\sigma (t)$(不可交换误差修正量，4阶导以上全部为零(证略)) $$\varphi (t)=\Delta \theta (t)+\sigma (t)$$ 将$\varphi (t)$在t=0初用泰勒级数展开 $$\varphi (t)=\varphi (0)+T\dot{\varphi }(0)+\frac{T^2}{2!}\varphi (0{)}^{{}^{\prime \prime }}+....=0+Ta+T^{2}b+\frac{T^3}{6}a\times b$$ $$=Ta+T^{2}b+\frac{T^3}{6}a\times b$$ 为了求解a,b需要在一个四元数更新周期内进行两次采样： $$\Delta {\theta }_1={\int }_0^{T/2}w(t)dt=\frac{T}{2}a+\frac{T^2}{4}b$$ $$\Delta {\theta }_2={\int }_{T/2}^{T}w(t)dt=\frac{T}{2}a+\frac{3T^2}{4}b$$ $$\varphi (T)=\Delta {\theta }_1+\Delta {\theta }_2+\frac{2}{3}\Delta {\theta }_1\times \Delta {\theta }_2$$ 三字样法： 假设：0-T时间（4元数更新时间段）内陀螺仪为线性输出即： $$w(t)=a+2bt+3c{t}^2(t\in [0,T])$$ 需要在[0,T]内进行3次采样： $$\Delta {\theta }_1={\int }_0^{T/3}w(t)dt$$ $$\Delta {\theta }_2={\int }_{T/3}^{\frac{2}{3}T}w(t)dt$$ $$\Delta {\theta }_3={\int }_{\frac{2}{3}T}^{T}w(t)dt$$ $$\varphi (T)=\Delta {\theta }_1+\Delta {\theta }_2+\Delta {\theta }_3+\frac{33}{80}\Delta {\theta }_1\times \Delta {\theta }_3+\frac{57}{80}\Delta {\theta }_1\times \Delta {\theta }_2+\frac{57}{80}\Delta {\theta }_2\times \Delta {\theta }_3$$ 单字样法：（频率相同时精度与二子样法相同，频率更高） 需要知道前一周期角增量$\Delta {\theta }_0$ 角速度输出为: $$w(t)=a+2bt(t\in [-T,T])$$ $$\varphi (T)=\Delta {\theta }_1+\frac{1}{12}\Delta {\theta }_0\times \Delta {\theta }_1$$