捷联惯导系统学习2.6(圆锥误差补偿多子样算法)

若圆锥运动的四元数更新方程为：
$$Q(t_m)=Q(T_{m-1})。Q(T)$$
（ $。$四元数乘法）
（ $Q(T)$四元数周期内变化量）
得到：（$\Omega$震动频率;$\theta =|\varphi |$）
$$Q(T)=Q^{\ast }(t_{m-1})Q(T_m)=M_{Q^{\ast }(t_{m-1})}Q(t_m)$$
$$=\left[\begin{array}{c}1-2(sin(\frac{\theta }{2})sin\frac{\Omega T}{2}{)}^2\\ -sin\theta sin\frac{\Omega T}{2}sin\Omega (t_m-\frac{T}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega (t_m-\frac{T}{2})\\ -si{n}^2\frac{\theta }{2}sin\Omega T\end{array}\right](推导略)=\left[\begin{array}{c}cos\frac{\theta }{2}\\ u\ast sin(\frac{\theta }{2})\end{array}\right]$$
若：在$[t_{m-1},tm]$时间内的等效旋转矢量为$\varphi (T)$;$u=\frac{\varphi (T)}{\theta }$
得到$$\frac{\varphi (T)}{\theta (t)}\ast sin(\frac{\theta (t)}{2})=\left[\begin{array}{c}-sin\theta sin\frac{\Omega T}{2}sin\Omega (t_m-\frac{T}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega (t_m-\frac{T}{2})\\ -si{n}^2\frac{\theta }{2}sin\Omega T\end{array}\right]$$
左右两边取模值，得到
$$sin\frac{\Omega T}{2}=\sqrt{si{n}^2\theta si{n}^2\frac{\Omega T}{2}+si{n}^4\frac{\theta }{2}si{n}^2\Omega T}$$
当$\theta 和\Omega T$均为小量时：
$$sin\frac{\Omega T}{2}\approx \sqrt{si{n}^2\theta si{n}^2\frac{\Omega T}{2}}\approx \frac{\theta \Omega T}{2}$$
从而进一步的到： $\theta (t)=\theta \Omega T$

多子样补偿算法
当使用角增量代替旋转矢量进行姿态更新时会在z轴产生误差，这个误差会随时间不断积累，常使用多子样补偿算法，弥补这一算法(证略)
在$[t_{m-1},t_m]$内采样N次，$h=\frac{T}{N}$为采样间隔
$$\Delta {\theta }_{mi}={\int }_{t_{m-1}+(i-1)h}^{t_{m-1}+ih}w(t)dt$$
$$=\left[\begin{array}{c}-2sin\theta sin\frac{\Omega T}{2}sin\Omega (t_{m-1}+ih-\frac{h}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega (t_{m-1}+ih-\frac{h}{2})\\ -si{n}^2\frac{\theta }{2}sin\Omega T\end{array}\right]$$
记$\lambda =\Omega T$得到在$[t_{m-1},t_m]$内的总角增量为：
$$\Delta {\theta }_m={\int }_{t_{m-1}}^{t_m}w(t)dt=\sum _{i=1}^N\Delta {\theta }_{mi}$$
求解不同子样的角增量之间的叉积（$\theta 和\lambda$均为小量）
$$\Delta {\theta }_{mi}\times \Delta {\theta }_{mj}=\left[\begin{array}{c}8\lambda si{n}^2\frac{\theta }{2}sin\theta sin\frac{\lambda }{2}sin\frac{(i-j)\lambda }{2}sin\Omega (t_{m-1}-\frac{i+j-1}{2})h\\ -8\lambda si{n}^2\frac{\theta }{2}sin\theta sin\frac{\lambda }{2}sin\frac{(i-j)\lambda }{2}cos\Omega (t_{m-1}-\frac{i+j-1}{2})h\\ -4si{n}^2\frac{\theta }{2}si{n}^2\frac{\lambda }{2}sin(i-j)\lambda \end{array}\right]$$
$\theta 和\lambda$均为小量
$$\approx \left[\begin{array}{c}\frac{(i-j)(\theta \lambda {)}^3}{2}sin\Omega (t_{m-1}-\frac{i+j-1}{2})h\\ -\frac{(i-j)(\theta \lambda {)}^3}{2}cos\Omega (t_{m-1}-\frac{i+j-1}{2})h\\ -4si{n}^2\frac{\theta }{2}si{n}^2\frac{\lambda }{2}sin(i-j)\lambda \end{array}\right]$$
定义圆锥误差补偿系数
$$\delta {\theta }^{\wedge }(T)=\sum _{j=2}^N\sum _{i=1}^{j-1}{k}_{ij}\Delta {\theta }_{mi}\times \Delta {\theta }_{mj}=\sum _{i=1}^{N-1}{k}_{N-1}^{\ast }\Delta {\theta }_{mi}\times \Delta {\theta }_{mj}$$

$k_{N-1}=k_{1(N-1)}^{\ast }$
$k_{N-2}=k_{1(N-1)}^{\ast }+k_{2(N)}^{\ast }$
…
$k_2=k_{13}^{\ast }+k_{24}^{\ast }+k_{35}^{\ast }+...+k_{(N-2)(N)}^{\ast }$
$k_1=k_{12}^{\ast }+k_{23}^{\ast }+k_{34}^{\ast }+...+k_{(N-1)(N)}^{\ast }$
因为x,y轴上的误差为$(\theta \lambda {)}^3$量级，不会起时姿态累计漂移，所以只用考虑z轴上的分量,将z轴分量，用泰勒级数展开为：
$$\delta {\theta }^{\wedge }(T{)}_z=(\varphi (T)-\Delta {\theta }_m{)}_z=2si{n}^2\frac{\theta }{2}(\Omega T-sin\Omega T)$$
$$=4si{n}^2\frac{\theta }{2}\sum _{i=1}^{\infty }(-1{)}^{i+1}{c}_i{\lambda }^{2i+1}(c_i=\frac{N^{2i+1}}{2(2i+1)!})$$
$$\delta {\theta }^{\wedge }(T{)}_z=\sum _{j=2}^N\sum _{i=1}^{j-1}{k}_{ij}\Delta {\theta }_{mi}\times \Delta {\theta }_{mj}=si{n}^2\theta \sum _{i=1}^{\infty }(-1{)}^{i+1}\sum _{j=1}^{N-1}{A}_{ij}{k}_j{\lambda }^{2i+1}$$
使${\lambda }^{2i+1}$对应系数相等，建立线性方程求解$K$可以得到：
$$AK=C$$
其中:(i,j=1,2,3,…,N-1)
$A=(A_{ij}{)}_{(N-1)(N-1)}=\frac{(j+1{)}^{2i+1}+(j+1{)}^{2i-1}-(2j{)}^{2i+1}}{(2i+1)!}$
$K=(k_j{)}_{(N-1)\times 1}$
$C=(c_i{)}_{(N-1)\times 1}=\frac{N^{2i+1}}{2(2i+1)!}$
误差漂移系数为:(只要$\frac{{\theta }^2(\Omega T{)}^{2N+1}}{T}<\frac{1^{。}}{h}$就能满足多数惯性导航系统要求)
$${\xi }_n={\rho }_n\frac{{\theta }^2(\Omega T{)}^{2N+1}}{T}(N\ge 1)$$

圆锥误差补偿系数和漂移误差分数解

N	k1	k2	k3	k4	k5	ρn
1	-					1/12
2	2/3					1/960
3	27/20	9/12				1/204120
4	214/105	92/105	54/105			1/82575360
5	1375/504	650/504	525/504	250/504		1/82575360
6	15797/4620	7834/4620	7296/4620	4558/4620	2315/4620	1/82575360

EX:四子样子求解等效旋转矢量
$$\varphi (T)=\Delta {\theta }_m+\delta {\theta }^{\wedge }(T)=\sum _{i=1}^4\Delta {\theta }_{mi}+\sum _{i=1}^{4-1}{k}_{4-1}^{\ast }\Delta {\theta }_{mi}\times \Delta {\theta }_{mj}$$
$$(\Delta {\theta }_{m1}+\Delta {\theta }_{m2}+\Delta {\theta }_{m3}+\Delta {\theta }_{m4})+(k_1\Delta {\theta }_{m1}+k_2\Delta {\theta }_{m2}+k_3\Delta {\theta }_{m3})\times \Delta {\theta }_{m4}$$