捷联惯导系统学习2.7(多项式条件下的等效旋转矢量精确求解)

在使用角增量多样子式求解等效旋转时，原理是使用Bortz方程2阶近似性得到的（忽略高阶项），导致原理上不可避免的会差生误差，使用Bortz方程给出基于多项时迭代的等效旋转矢量，可以避免原理性近似误差。
角运动的多项式描述：
假设：角速度$w(t)$
$$w(t)=W\left[\begin{array}{c}{t}^{N-1}\\ t^{N-2}\\ ...\\ 1\end{array}\right]$$
$$W=\left[\begin{array}{c}{W}_x\\ W_y\\ W_z\end{array}\right]=\left[\begin{array}{ccccc}{w}_{n-1}& w_{n-2}& w_{n-3}& ...& w_0\end{array}\right]=\left[\begin{array}{ccccc}{w}_{n-1,x}& w_{n-2,x}& w_{n-3,x}& ...& w_{0,x}\\ w_{n-1,y}& w_{n-2,y}& w_{n-3,y}& ...& w_{0,y}\\ w_{n-1,z}& w_{n-2,z}& w_{n-3,z}& ...& w_{0,z}\end{array}\right]$$
假设：陀螺仪在时间段$[0,T]$时间内采样N次，分别计为${\theta }_i(i=1,2,...,N)$
积分可得:
$$\Delta {\theta }_j={\int }_{t_{j-1}}^{tj}w(t)dt=\frac{t_j^N-t_{j-1}^N}{N}{w}_{N-1}+\frac{t_j^{N-1}-t_{j-1}^{N-1}}{N-1}{w}_{N-2}+...+(t_j-t_{j-1})w_0$$

令:
$\Theta =\left[\begin{array}{cccc}\Delta {\theta }_1& \Delta {\theta }_2& ...& \Delta {\theta }_N\end{array}\right]$
$\Gamma =\left[\begin{array}{ccccc}\frac{t_1^N-t_0^N}{N}& \frac{t_2^N-t_1^N}{N}& ...& \frac{t_N^N-t_{N-1}^N}{N}& \\ \frac{t_1^N-t_0^N}{N-1}& \frac{t_2^N-t_1^N}{N-1}& ...& \frac{t_N^N-t_{N-1}^N}{N-1}& \\ ...& ...& ...& ...& \\ t_1-t_0& t_2-t_1& ...& t_N-t_{N-1}& \end{array}\right]$
可以的得到：
$$\Theta =W\Gamma$$
$$W=\Theta {\Gamma }^{-1}$$
等效旋转矢量的迭代求解：
$${\varphi }_j^{(i+1)}=\left[\begin{array}{cc}{U}_j^{\prime \prime }& 0\end{array}\right]\odot \left[\begin{array}{ccccc}...& \frac{1}{3}& \frac{1}{2}& 1& 0\end{array}\right](j=x,y,z;)$$
$\odot$表示两个相同阶次的多项式系数向量，之间的同幂次系数分量相乘结果
推导：
已知：$\dot{\varphi }=w+\frac{1}{2}\varphi \times w+(\frac{1}{12}+\frac{{\theta }^2}{720}+\frac{{\theta }^4}{30240}+...)(\varphi \times {)}^{2}w$
在[0,t]时间内对两式左右两边积分
$\varphi -\varphi (0)={\int }_0^{t}w+\frac{1}{2}\varphi \times w+(\frac{1}{12}+\frac{{\theta }^2}{720}+\frac{{\theta }^4}{30240}+...)(\varphi \times {)}^{2}wdt$
令$\varphi (0)=0$得到第i次迭代的后旋转矢量
$${\varphi }^{(i)}=\left[\begin{array}{c}{\varphi }_x^{(i)}\\ {\varphi }_y^{(i)}\\ {\varphi }_z^{(i)}\end{array}\right]\left[\begin{array}{c}...\\ t^2\\ t\\ 1\end{array}\right]=\left[\begin{array}{cccc}...& {\varphi }_{2,x}^{(i)}& {\varphi }_{1,x}^{(i)}& {\varphi }_{0,x}^{(i)}\\ ...& {\varphi }_{2,y}^{(i)}& {\varphi }_{1,y}^{(i)}& {\varphi }_{0,y}^{(i)}\\ ...& {\varphi }_{2,z}^{(i)}& {\varphi }_{1,z}^{(i)}& {\varphi }_{0,z}^{(i)}\end{array}\right]$$
将$W=\Theta {\Gamma }^{-1}$带入积分求得递推公式为：
$${\varphi }^{i+1}={\int }_0^T\left[\begin{array}{c}{W}_x\\ W_y\\ W_z\end{array}\right]\left[\begin{array}{c}{t}^{N-1}\\ t^{N-2}\\ ...\\ 1\end{array}\right]+\frac{1}{2}\left[\begin{array}{c}{U}_x\\ U_y\\ U_z\end{array}\right]\left[\begin{array}{c}{t}^{N-1}\\ t^{N-2}\\ ...\\ 1\end{array}\right]+\left[\begin{array}{c}{\theta }^{\prime }\ast U_x^{\prime }\\ {\theta }^{\prime }\ast U_y^{\prime }\\ {\theta }^{\prime }\ast U_z^{\prime }\end{array}\right]\left[\begin{array}{c}{t}^{N-1}\\ t^{N-2}\\ ...\\ 1\end{array}\right]$$
其中:
${\theta }^{\prime }=\left[\begin{array}{ccccc}\frac{1}{12}& \frac{({\theta }^{(i)}{)}^2}{720}& \frac{({\theta }^{(i)}{)}^4}{30140}& \frac{({\theta }^{(i)}{)}^6}{1209600}& ...\end{array}\right]$
$\left[\begin{array}{c}{U}_x\\ U_y\\ U_z\end{array}\right]=\left[\begin{array}{c}{\varphi }_y^{(i)}\ast W_z-{\varphi }_z^{(i)}{W}_y\\ -{\varphi }_x^{(i)}\ast W_z+{\varphi }_z^{(i)}{W}_x\\ {\varphi }_x^{(i)}\ast W_y-{\varphi }_y^{(i)}{W}_x\end{array}\right]$
$\left[\begin{array}{c}{U}_x^{\prime }\\ U_y^{\prime }\\ U_z^{\prime }\end{array}\right]=\left[\begin{array}{c}{\varphi }_y^{(i)}\ast U_z-{\varphi }_z^{(i)}{U}_y\\ -{\varphi }_x^{(i)}\ast U_z+{\varphi }_z^{(i)}{U}_x\\ {\varphi }_x^{(i)}\ast U_y-{\varphi }_y^{(i)}{U}_x\end{array}\right]$
实际使用时需要对无穷级数，进行有限次处理：
$w$一般零次多项式不为0，当迭代次数$i>0$时，${\varphi }^{(i)}$不为0的最低次项为1;
如果存在不可交换误差，$({\varphi }^{(i)}\times )w$中最低幂次不为0应当为2;
如果存在不可交换误差，$({\varphi }^{(i)}\times {)}^{2}w$中最低幂次不为0应当为3;
$({\varphi }^{(i)}{)}^k({\varphi }^{(i)}\times {)}^{2}w$中最低幂次不为0应当为k+3(k=0,2,4,6,…)
若忽略等效旋转矢量中，与$\theta$有关不高于$k$次项,等效旋转矢量只有$k+3$的精度，精度高于多子样法