CN106153073A - 一种全姿态捷联惯导系统的非线性初始对准方法 - Google Patents

Abstract

本发明公开了一种全姿态捷联惯导系统的非线性初始对准方法，主要步骤包括：捷联惯性导航系统粗对准；捷联惯性导航系统双欧拉角直接法非线性滤波器的初始化、时间更新、量测更新及正/反欧拉角直接法的切换。本发明融合了双欧拉角法和直接法，解决了单欧拉角直接法的奇异性问题，可以适用于载体大机动的情况；系统方程以速度微分方程和欧拉角微分方程为主，模型比间接法的一阶近似更精确；滤波过程中同时实现了导航解算，滤波输出为导航参数，避免了力学编排方程的许多重复计算，算法比间接法更简单；相较于四元数直接法而言，不需要修改滤波步骤，算法简单。

Description

一种全姿态捷联惯导系统的非线性初始对准方法

技术领域

本发明涉及一种全姿态捷联惯导系统的非线性初始对准方法，用于确定捷联惯性导航系统的初始姿态，属于导航、制导与控制技术领域。

背景技术

捷联惯性导航系统在进入导航工作前均需进行初始对准过程，其主要任务是初始姿态的确定，即确定初始数学平台。

初始对准一般需要经过粗对准和精对准两个阶段，粗对准阶段，通过陀螺仪测得的角速度信息和加速度计测得的比力信息获得粗略的初始姿态信息。精对准阶段一般采用最优估计滤波方法。根据所估计的系统状态的不同，卡尔曼滤波在应用时有两种方法：直接法和间接法。

间接法中采用捷联惯性导航系统的误差模型，以导航参数的误差量作为状态量，卡尔曼滤波器经过计算，获得各导航参数误差量的最优估计值。在非线性滤波器研究不够深入的早期，均采用线性滤波器，需要对误差模型作一阶线性近似，会损失模型精度。直接法滤波中,以导航参数作为状态,卡尔曼滤波器经过计算,得到导航参数的最优估计值，其系统方程以惯性导航系统的力学编排方程为主，无需近似，系统方程是非线性的，随着非线性滤波技术的发展，直接法的研究成为热点，在直接法中常用四元数微分方程和欧拉角微分方程。

四元数包含四个参数，存在模值为1的非线性约束关系，无论是线性滤波器还是非线性滤波器，都要求状态量之间是不相关的，因此用四元数作为状态量时，需要对滤波算法做相应处理，如可采用二次映射或者虚拟观测量等方法，增加了复杂度。载体姿态角是按照欧拉角的概念定义的，姿态角就是一组欧拉角，欧拉角法概念直观、容易理解。欧拉角法通过求解欧拉角微分方程直接计算航向角、纵摇角和横摇角，三个参数之间没有约束关系，用欧拉角作为直接法的状态量时，滤波算法不用改变。但欧拉角姿态表示方法存在奇异点问题，根据旋转顺序的不同，不同的欧拉角组合的奇异点不同。如旋转顺序为航向角、纵摇角和横摇角时，定义为正欧拉角组合，则奇异点为θ＝±90°，如旋转顺序为航向角、横摇角和纵摇角时，定义为反欧拉角组合，则奇异点为γ_r＝±90°，而当θ＝±90°时，根据正欧拉角与反欧拉角的转换关系可以得到γ_r＝0°或者180°，为反欧拉角的精华点，正欧拉角与反欧拉角的奇异点和精华点位置正好倒挂，因此可以用正、反欧拉角即双欧拉角法来解决单欧拉角的奇异性问题。欧拉角在直观定义域内取值时，双欧拉法的切换最佳角度±45°。

发明内容

技术问题：本发明提供一种状态量直接采用欧拉角、系统方程采用双欧拉角微分方程、数据融合利用非线性滤波的，可克服欧拉角奇异性的全姿态捷联惯导系统的非线性初始对准方法。

技术方案：本发明的全姿态捷联惯导系统的非线性初始对准方法，包括以下步骤：

步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的比力信息，使用基于惯性系重力加速度的凝固解析粗对准算法，求得粗略的以正欧拉角表示的初始纵摇角θ₀、横摇角γ₀和航向角ψ₀；

步骤2、根据所述步骤1得到的初始纵摇角θ₀，设置正/反欧拉角直接法的运行标志Flag：如果θ₀∈[-45°,+45°]，则Flag＝0，采用正欧拉角直接法；如果θ₀∈[-90°,-45°)∪(45°,+90°]，则Flag＝1，采用反欧拉角直接法；

步骤3、正/反欧拉角直接法非线性滤波器的初始化：根据标志Flag，在滤波开始时刻t₀，利用粗对准的结果初始化滤波器的系统状态向量及其方差矩阵P₀；

其中系统状态向量根据Flag标志设定：如果Flag＝0，则采用正欧拉角直接法的状态向量，将粗对准得到的正欧拉角直接赋给系统状态向量如果Flag＝1，则采用反欧拉角直接法的状态向量，先将粗对准获得的正欧拉角转换为反欧拉角，然后将得到的反欧拉角赋给系统状态向量

步骤4、每当获得新的陀螺和加速度计测量值，就进行正/反欧拉角直接法非线性滤波器的时间更新：根据标志Flag，利用陀螺和加速度计的输出以及正/反欧拉角直接法的系统方程，进行非线性滤波器的时间更新，得到t_k时刻系统状态向量的一步预测值及其方差矩阵的一步预测值P_k|k-1，如果Flag＝0，则采用正欧拉角直接法的系统方程进行非线性滤波器的时间更新；如果Flag＝1，则采用反欧拉角直接法的系统方程进行非线性滤波器的时间更新；

未获得新的速度观测值时，时间更新后，将赋给t_k时刻系统状态向量的估计值将P_k|k-1赋给t_k时刻系统状态向量方差矩阵的估计值P_k|k，进入步骤6；

若获得了新的速度观测值，时间更新后，则进入步骤5；

步骤5、根据标志Flag，利用载体系速度的输出以及正/反欧拉角直接法的量测方程，进行非线性滤波器的量测更新，得到更新后的和P_k|k：如果Flag＝0，则采用正欧拉角直接法的量测方程进行非线性滤波器的量测更新；如果Flag＝1，则采用反欧拉角直接法的量测方程进行非线性滤波器的量测更新；

步骤6、如果Flag＝0，则直接输出中的正欧拉角和速度分量，如果Flag＝1，则先将中的反欧拉角转换为正欧拉角，然后输出该正欧拉角和中的速度分量；每次输出后，判断对准时间是否截止，如是，则结束本方法流程，否则进入步骤7；

步骤7、根据标志Flag，以及以正欧拉角表示的纵摇角θ的大小，每次量测更新后，进行正/反欧拉角直接法的切换，重置标志Flag：

如果Flag＝0，且θ∈[-45°,+45°]，则Flag保持不变，返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝0，且θ∈[-90°,-45°)∪(45°,+90°]，则令Flag＝1，将中的正欧拉角转换为反欧拉角，然后返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝1，且θ∈[-45°,+45°]，则令Flag＝0，将中的反欧拉角转换为正欧拉角，然后返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝1，且θ∈[-90°,-45°)∪(45°,+90°]，则Flag保持不变，返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新。

进一步的，本发明方法中，所述的步骤4中正/反欧拉角直接法的系统方程具体为：

正欧拉角直接法的系统方程为：

${\stackrel{\·}{x}}_p=F_p\left(x_p\right)+G_p\left(x_p\right)w$

式中，x_p为正欧拉角直接法的状态向量，w为正欧拉角直接法的系统噪声向量，F_p(x_p)为正欧拉角直接法的非线性状态转移函数矩阵，G_p(x_p)为正欧拉角直接法的系统噪声输入函数矩阵，分别根据下式确定：

$x_p={\[v_{east},v_{north},\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{ax},{\▿}_{ay},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

w＝[w_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0]^T

$F_p\left(x_p\right)=\left(\begin{array}{c}{\tilde{f}}_{ibz}^b(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ})-(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})({\▿}_{ax}-{\tilde{f}}_{ibx}^b)+v_{north}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})+\cos \mathrm{\θ}\sin \mathrm{\ψ}({\▿}_{ay}-{\tilde{f}}_{iby}^b)\\ {\tilde{f}}_{ibz}^b(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ})-(\cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})({\▿}_{ax}-{\tilde{f}}_{ibx}^b)-v_{east}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})-\cos \mathrm{\ψ}\cos \mathrm{\θ}({\▿}_{ay}-{\tilde{f}}_{iby}^b)\\ {\widetilde{\mathrm{\ω}}}_{ibx}^b\cos \mathrm{\γ}+{\widetilde{\mathrm{\ω}}}_{ibz}^b\sin \mathrm{\γ}-{\mathrm{\ϵ}}_{gx}\cos \mathrm{\γ}-{\mathrm{\ϵ}}_{gz}\sin \mathrm{\γ}+\frac{v_{north}\cos \mathrm{\ψ}}{r_M}-{\mathrm{\ω}}_{ie}\cos L\sin \mathrm{\ψ}-\frac{v_{east}}{r_N}\sin \mathrm{\ψ}\\ {\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\ϵ}}_{gy}-{\widetilde{\mathrm{\ω}}}_{ibz}^b\cos \mathrm{\γ}\tan \mathrm{\θ}+{\widetilde{\mathrm{\ω}}}_{ibx}^b\sin \mathrm{\γ}\tan \mathrm{\θ}+{\mathrm{\ϵ}}_{gz}\cos \mathrm{\γ}\tan \mathrm{\θ}-{\mathrm{\ϵ}}_{gx}\sin \mathrm{\γ}\tan \mathrm{\θ}-\frac{v_{north}\sin \mathrm{\ψ}}{r_M\cos \mathrm{\θ}}-{\mathrm{\ω}}_{ie}\cos L\frac{\cos \mathrm{\ψ}}{\cos \mathrm{\θ}}-\frac{v_{east}\cos \mathrm{\ψ}}{r_N\cos \mathrm{\θ}}\\ \frac{{\widetilde{\mathrm{\ω}}}_{ibz}^b\cos \mathrm{\γ}}{\cos \mathrm{\θ}}-\frac{v_{east}}{r_N}\tan L-{\mathrm{\ω}}_{ie}\sin L-\frac{{\widetilde{\mathrm{\ω}}}_{ibx}^b\sin \mathrm{\γ}}{\cos \mathrm{\θ}}-\frac{{\mathrm{\ϵ}}_{gz}\cos \mathrm{\γ}}{\cos \mathrm{\θ}}+\frac{{\mathrm{\ϵ}}_{gx}\sin \mathrm{\γ}}{\cos \mathrm{\θ}}+\frac{v_{north}}{r_M}\sin \mathrm{\ψ}\tan \mathrm{\θ}+{\mathrm{\ω}}_{ie}\cos L\cos \mathrm{\ψ}\tan \mathrm{\θ}+\frac{v_{east}}{r_N}\cos \mathrm{\ψ}\tan \mathrm{\θ}\\ 0_{5\×1}\end{array}\right)$

G_p(x_p)为10行10列的矩阵，其中，

$G_{p1}\left(x_p\right)=\left[\begin{array}{cc}\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}\end{array}\right]$

$G_{p2}\left(x_p\right)=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& sin\mathrm{\γ}\\ tan\mathrm{\θ}\sin \mathrm{\γ}& 1& -\cos \mathrm{\γ}tan\mathrm{\θ}\\ -\frac{sin\mathrm{\γ}}{\cos \mathrm{\θ}}& 0& \frac{cos\mathrm{\γ}}{cos\mathrm{\θ}}\end{array}\right]$

v_east为东向速度，v_north为北向速度，θ、γ和ψ分别为正欧拉角组合的纵摇角、横摇角和航向角；分别为载体系中x、y向加速度计测量的随机常值偏置，ε_gx、ε_gy和ε_gz分别为载体系中x、y、z向陀螺仪测量的随机常值漂移；w_ax、w_ay分别为载体系中x、y向加速度计测量的随机噪声误差，w_gx、w_gy和w_gz分别为载体系中x、y、z向陀螺仪测量的随机噪声误差；和分别为载体系x、y、z向加速度计的测量值，ω_ie为地球自转角速率，L为当地地理纬度，r_M为子午圈曲率半径，r_N为卯酉圈曲率半径；和为载体系x、y、z向陀螺仪测量值；0_i×j表示元素为0的i行j列矩阵；

反欧拉角直接法的系统方程为：

${\stackrel{\·}{x}}_r=F_r\left(x_r\right)+G_r\left(x_r\right)w$

式中，反欧拉角直接法的系统噪声向量w与正欧拉角直接法的相同；x_r为反欧拉角直接法的状态向量，F_r(x_r)为反欧拉角直接法的非线性状态转移函数矩阵，G_r(x_r)为反欧拉角直接法的系统噪声输入函数矩阵，分别根据下式确定：

$x_r={\[v_{east},v_{north},{\mathrm{\θ}}_r,{\mathrm{\γ}}_r,{\mathrm{\ψ}}_r,{\▿}_{ax},{\▿}_{ay},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

$F_r\left(x_r\right)=\left(\begin{array}{c}{\tilde{f}}_{ibz}^b({\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r+{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r{\mathrm{sin\γ}}_r)+({\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)({\▿}_{ay}-{\tilde{f}}_{iby}^b)+v_{north}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})-{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r({\▿}_{ax}-{\tilde{f}}_{ibx}^b)\\ -{\tilde{f}}_{ibz}^b({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r-{\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r{\mathrm{sin\γ}}_r)-({\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)({\▿}_{ay}-{\tilde{f}}_{iby}^b)-v_{east}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})-{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r({\▿}_{ax}-{\tilde{f}}_{ibx}^b)\\ {\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\ϵ}}_{gx}+{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r+{\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ϵ}}_{gz}{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ϵ}}_{gy}{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ω}}_{ie}\cos L\frac{{\mathrm{sin\ψ}}_r}{{\mathrm{cos\γ}}_r}+\frac{v_{north}{\mathrm{cos\ψ}}_r}{r_M{\mathrm{cos\γ}}_r}-\frac{v_{east}{\mathrm{sin\ψ}}_r}{r_N{\mathrm{cos\γ}}_r}\\ {\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{cos\θ}}_r-{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{sin\θ}}_r-{\mathrm{\ϵ}}_{gy}{\mathrm{cos\θ}}_r+{\mathrm{\ϵ}}_{gz}{\mathrm{sin\θ}}_r-{\mathrm{\ω}}_{ie}{\mathrm{cosLcos\ψ}}_r-\frac{v_{east}}{r_N}{\mathrm{cos\ψ}}_r-\frac{v_{north}{\mathrm{sin\ψ}}_r}{r_M}\\ \frac{{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{v_{east}}{r_N}\tan L-{\mathrm{\ω}}_{ie}\sin L+\frac{{\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{{\mathrm{\ϵ}}_{gz}{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{{\mathrm{\ϵ}}_{gy}{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}-{\mathrm{\ω}}_{ie}{\mathrm{cosLsin\ψ}}_r{\mathrm{tan\γ}}_r+\frac{v_{north}}{r_M}{\mathrm{cos\ψ}}_r{\mathrm{tan\γ}}_r-\frac{v_{east}}{r_N}{\mathrm{sin\ψ}}_r{\mathrm{tan\γ}}_r\\ 0_{5\×1}\end{array}\right)$

G_r(x_r)为10行10列矩阵，其中，

$G_{r1}\left(x_r\right)=\left[\begin{array}{cc}-{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r& {\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\\ -{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r& -{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\end{array}\right]$

$G_{r2}\left(x_r\right)=\left[\begin{array}{ccc}1& -{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r& -{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r\\ 0& -{\mathrm{cos\θ}}_r& {\mathrm{sin\θ}}_r\\ 0& -\frac{{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}& -\frac{{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}\end{array}\right]$

式中：ψ_r、θ_r和γ_r分别为反欧拉角的航向角、纵摇角和横摇角。

进一步的，本发明方法中，所述的步骤5中正/反欧拉角直接法的量测方程具体为：

正欧拉角直接法的量测方程为：

z＝H_p(x_p)+η

式中，z为正欧拉角直接法的量测量，η为正欧拉角直接法的量测噪声向量，

H_p(x_p)为正欧拉角直接法的非线性量测函数，分别根据下式确定：

z＝[v_bx,v_by,v_bz]^T

η＝[η_vbx,η_vby,η_vbz]^T

$H_p\left(x_p\right)=\left[\begin{array}{c}{v}_{east}(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})+v_{north}(\cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})\\ v_{north}\cos \mathrm{\ψ}\cos \mathrm{\θ}-v_{east}\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ v_{east}(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ})+v_{north}(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ})\end{array}\right]$

v_bx、v_by和v_bz分别为载体系中x、y、z向的速度分量；η_vbx、η_vby、η_vbz分别为载体系中x、y、z向的速度量测噪声；

反欧拉角直接法的量测方程为：

z＝H_r(x_r)+η

式中，反欧拉角直接法的量测量z以及量测噪声向量η与正欧拉角直接法的相同；H_r(x_r)为反欧拉角直接法的非线性量测函数，为

$H_r\left(x_r\right)=\left[\begin{array}{c}{v}_{east}{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r+v_{north}{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r\\ v_{north}({\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)-v_{east}({\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)\\ v_{east}({\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r+{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r{\mathrm{sin\γ}}_r)-v_{north}({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r-{\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r{\mathrm{sin\γ}}_r)\end{array}\right].$

进一步的，本发明方法中，所述非线性滤波器采用sigma点非线性滤波。

本发明中状态方程采用双欧拉角微分方程，正欧拉角直接法与反欧拉角直接法分区运行，当θ∈[-45°,+45°]，采用正欧拉角直接法；当θ∈[-90°,-45°)∪(45°,+90°]，采用反欧拉角直接法；在θ＝±45°时进行切换，输出统一用正欧拉角表示。该方案融合了双欧拉法和直接法，既解决了单欧拉角的奇异性问题，可以适用于载体大机动的情况，比如飞机筋斗飞行时的初始对准问题，又具有欧拉角直接法的优点：系统状态方程采用速度微分方程和欧拉角微分方程，不需要任何假设和近似，直接描述导航参数的动态变化过程，能较准确地反映真实状态的演变情况，比间接法的一阶近似更精确；滤波过程既实现了导航解算，又能起到滤波估计的作用，避免了力学编排方程的许多重复计算，滤波输出为导航参数，不需要单独进行误差修正，算法更简单；相较于四元数直接法而言，不需要对滤波步骤进行额外处理，算法简单。

有益效果：本发明与现有技术相比，具有以下优点：

1)本发明中状态量直接采用欧拉角，欧拉角之间互不相关，且只有三个参数，相较于常用的四元数直接法而言，四元数有四个参数，四个参数之间有模值为1的约束，且四个参数之间是相关的，因此本发明不需要对滤波步骤进行额外处理，算法简单。

2)本发明中滤波输出直接为导航参数，相较于以往的间接法，不需要单独进行误差修正，只需要一套滤波算法，滤波器既能达到导航解算的目的，又能起到滤波估计的作用，可避免力学编排方程的许多重复计算，算法更简单；系统状态方程以速度微分方程和双欧拉角微分方程为主，直接描述导航参数的动态变化过程，能较准确地反映真实状态的演变情况，且模型不做任何近似和约束，比间接法的一阶近似更精确。

3)本发明中系统方程采用双欧拉角微分方程，可以解决单欧拉角的奇异性问题，可以适用于载体大机动的情况。

因此本发明为捷联惯性导航系统初始对准提供了一种新的无奇异点的直接法非线性解决方案，可以在单欧拉角出现奇异点时精确估计出初始的姿态欧拉角，为后续的导航提供精确的数学平台。

本发明提出的方案通过如下半物理试验加以验证：

1)本试验数据来源于某型惯导IMU的转台实测数据，转台模拟载体做连续大姿态角机动，其纵摇角θ运动经过正欧拉角微分方程的奇异点θ＝±90°。转台纵摇角θ在-90°～+90°之间变化，在θ＝±90°的位置有短暂的停留，横摇角为γ＝0°，航向角为ψ＝0°，同步记录转台真实的姿态值，以便与滤波输出的姿态值进行对比。

2)惯性传感器数据更新周期T为10ms，滤波周期T_f为0.1s，试验时间41分钟，前面5分钟用于粗对准；

3)三只加速度计随机常值偏置均为0.2mg，随机噪声均设0.2mg，三只陀螺仪随机常值漂移均为0.03°/h，随机噪声为0.03°/h；

4)位置：北纬32.067°，东经118.8°；

5)滤波器的初始条件设为

x＝[0,0,θ₀,γ₀,ψ₀,0,0,0,0,0]^T，式中θ₀，γ₀，ψ₀分别为5分钟粗对准结束时的正欧拉角；

z＝[0,0,0]^T；

P₀＝diag{(0.1m/s)²,(0.1m/s)²,(0.1°)²,(0.1°)²,(5°)²,(0.2mg)²,(0.2mg)²,(0.03°/h)²,(0.03°/h)²,(0.03°/h)²}；

Q＝diag{(0.2mg)²,(0.2mg)²,(0.03°/h)²,(0.03°/h)²,(0.03°/h)²,0,0,0,0,0}；

R＝diag{(0.1m/s)²,(0.1m/s)²,(0.1m/s)²}。

式中，diag{.}表示对角矩阵。

通过计算机仿真，摇摆台的初始对准估计的姿态角θ，γ，ψ(以DualEu表示)以及记录的转台真实姿态角(以True表示)的曲线如附图2-4所示。纵摇角对准误差Δθ如附图5所示。θ，γ，ψ对准误差分别为0.86′，0.13′，15.92′，满足系统要求。从图2中可以看到，滤波输出的纵摇角和转台记录的真实纵摇角吻合很好。对比图2和图6可以看到，正欧拉角直接法和反欧拉角直接法有序交替运行，在正欧拉角的奇异点±90°处，flag＝1，采用了反欧拉角直接法，解决了正欧拉角直接法在奇异点处不能使用的问题。

附图说明

图1为本发明实现克服单欧拉角奇异性的初始对准的具体实施方式；

图2为本发明的半物理仿真初始对准的纵摇角θ以及记录的转台真实θ的曲线图；

图3为本发明的半物理仿真初始对准的横摇角γ以及记录的转台真实γ的曲线图；

图4为本发明的半物理仿真初始对准的航向角ψ以及记录的转台真实ψ的曲线图；

图5为本发明的半物理仿真初始对准的纵摇角对准误差Δθ的曲线图；

图6为本发明的半物理仿真初始对准的正/反欧拉角直接法切换标志flag的曲线图。

具体实施方式

下面结合实施例和说明书附图对本发明作进一步的说明。

步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的比力信息，使用基于惯性系重力加速度的凝固解析粗对准算法，求得粗略的以正欧拉角表示的初始纵摇角θ₀、横摇角γ₀和航向角ψ₀；

步骤2、根据所述步骤1得到的初始纵摇角θ₀，设置正/反欧拉角直接法的运行标志Flag：如果θ₀∈[-45°,+45°]，则Flag＝0，采用正欧拉角直接法；如果θ₀∈[-90°,-45°)∪(45°,+90°]，则Flag＝1，采用反欧拉角直接法；

步骤3、正/反欧拉角直接法非线性滤波器的初始化：根据标志Flag，在滤波开始时刻t₀，利用粗对准的结果初始化滤波器的系统状态向量及其方差矩阵P₀，初始化滤波器的系统状态向量及其方差矩阵P₀，具体为：

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

式中，E[.]表示均值；[.]^T表示矩阵转置；和P₀分别为t₀时刻的系统状态向量估计值及其方差矩阵；

其中系统状态向量根据Flag标志设定：如果Flag＝0，则采用正欧拉角直接法的状态向量，将粗对准得到的正欧拉角直接赋给系统状态向量如果Flag＝1，则采用反欧拉角直接法的状态向量，先将粗对准获得的正欧拉角转换为反欧拉角，然后将得到的反欧拉角赋给系统状态向量

以东北天地理坐标系作为导航坐标系，即n系，以载体右前上方向矢量右手定则构成的坐标系作为载体坐标系，即b系；

设正欧拉角的旋转顺序依次为航向角、纵摇角和横摇角，分别以ψ、θ和γ来表示；设反欧拉角的旋转顺序依次为航向角、横摇角和纵摇角，分别以ψ_r、γ_r和θ_r来表示；

正欧拉角转换为反欧拉角，采用：

当γ_r≠±90°时，

正/反欧拉角直接法的状态向量分别以x_p和x_r表示，分别取为：

$x_p={\[v_{east},v_{north},\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{ax},{\▿}_{ay},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

$x_r={\[v_{east},v_{north},{\mathrm{\θ}}_r,{\mathrm{\γ}}_r,{\mathrm{\ψ}}_r,{\▿}_{ax},{\▿}_{ay},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

其中，v_east为东向速度，v_north为北向速度，分别为载体系中x、y向加速度计测量的随机常值偏置，ε_gx、ε_gy和ε_gz分别为载体系中x、y、z向陀螺仪测量的随机常值漂移；

步骤4、每当获得新的陀螺和加速度计测量值，就进行正/反欧拉角直接法非线性滤波器的时间更新：根据标志Flag，利用陀螺和加速度计的输出以及正/反欧拉角直接法的系统方程，基于t_k-1时刻的系统状态向量及其方差矩阵P_k-1|k-1进行非线性滤波器的时间更新，得到t_k时刻系统状态向量的一步预测值及其方差矩阵的一步预测值P_k|k-1，如果Flag＝0，则采用正欧拉角直接法的系统方程进行非线性滤波器的时间更新；如果Flag＝1，则采用反欧拉角直接法的系统方程进行非线性滤波器的时间更新；

其中，正/反欧拉角直接法的系统方程具体为：

正欧拉角直接法的系统方程为：

${\stackrel{\·}{x}}_p=F_p\left(x_p\right)+G_p\left(x_p\right)w$

式中，w为正欧拉角直接法的系统噪声向量，假设为高斯白噪声，其方差强度阵为Q；F_p(x_p)为正欧拉角直接法的非线性状态转移函数，G_p(x_p)为正欧拉角直接法的系统噪声输入函数，分别根据下式确定：

w＝[w_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0]^T

$F_p\left(x_p\right)=\left(\begin{array}{c}{\tilde{f}}_{ibz}^b(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ})-(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})({\▿}_{ax}-{\tilde{f}}_{ibx}^b)+v_{north}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})+\cos \mathrm{\θ}\sin \mathrm{\ψ}({\▿}_{ay}-{\tilde{f}}_{iby}^b)\\ {\tilde{f}}_{ibz}^b(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ})-(\cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})({\▿}_{ax}-{\tilde{f}}_{ibx}^b)-v_{east}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})-\cos \mathrm{\ψ}\cos \mathrm{\θ}({\▿}_{ay}-{\tilde{f}}_{iby}^b)\\ {\widetilde{\mathrm{\ω}}}_{ibx}^b\cos \mathrm{\γ}+{\widetilde{\mathrm{\ω}}}_{ibz}^b\sin \mathrm{\γ}-{\mathrm{\ϵ}}_{gx}\cos \mathrm{\γ}-{\mathrm{\ϵ}}_{gz}\sin \mathrm{\γ}+\frac{v_{north}\cos \mathrm{\ψ}}{r_M}-{\mathrm{\ω}}_{ie}\cos L\sin \mathrm{\ψ}-\frac{v_{east}}{r_N}\sin \mathrm{\ψ}\\ {\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\ϵ}}_{gy}-{\widetilde{\mathrm{\ω}}}_{ibz}^b\cos \mathrm{\γ}\tan \mathrm{\θ}+{\widetilde{\mathrm{\ω}}}_{ibx}^b\sin \mathrm{\γ}\tan \mathrm{\θ}+{\mathrm{\ϵ}}_{gz}\cos \mathrm{\γ}\tan \mathrm{\θ}-{\mathrm{\ϵ}}_{gx}\sin \mathrm{\γ}\tan \mathrm{\θ}-\frac{v_{north}\sin \mathrm{\ψ}}{r_M\cos \mathrm{\θ}}-{\mathrm{\ω}}_{ie}\cos L\frac{\cos \mathrm{\ψ}}{\cos \mathrm{\θ}}-\frac{v_{east}\cos \mathrm{\ψ}}{r_N\cos \mathrm{\θ}}\\ \frac{{\widetilde{\mathrm{\ω}}}_{ibz}^b\cos \mathrm{\γ}}{\cos \mathrm{\θ}}-\frac{v_{east}}{r_N}\tan L-{\mathrm{\ω}}_{ie}\sin L-\frac{{\widetilde{\mathrm{\ω}}}_{ibx}^b\sin \mathrm{\γ}}{\cos \mathrm{\θ}}-\frac{{\mathrm{\ϵ}}_{gz}\cos \mathrm{\γ}}{\cos \mathrm{\θ}}+\frac{{\mathrm{\ϵ}}_{gx}\sin \mathrm{\γ}}{\cos \mathrm{\θ}}+\frac{v_{north}}{r_M}\sin \mathrm{\ψ}\tan \mathrm{\θ}+{\mathrm{\ω}}_{ie}\cos L\cos \mathrm{\ψ}\tan \mathrm{\θ}+\frac{v_{east}}{r_N}\cos \mathrm{\ψ}\tan \mathrm{\θ}\\ 0_{5\×1}\end{array}\right)$

G_p(x_p)为10行10列的矩阵，其中，

$G_{p1}\left(x_p\right)=\left[\begin{array}{cc}\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}\end{array}\right]$

$G_{p2}\left(x_p\right)=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& sin\mathrm{\γ}\\ tan\mathrm{\θ}\sin \mathrm{\γ}& 1& -\cos \mathrm{\γ}tan\mathrm{\θ}\\ -\frac{sin\mathrm{\γ}}{\cos \mathrm{\θ}}& 0& \frac{cos\mathrm{\γ}}{cos\mathrm{\θ}}\end{array}\right]$

w_ax、w_ay分别为载体系中x、y向加速度计测量的随机噪声误差，w_gx、w_gy和w_gz分别为载体系中x、y、z向陀螺仪测量的随机噪声误差；和分别为载体系x、y、z向加速度计的测量值，ω_ie为地球自转角速率，L为当地地理纬度，r_M为子午圈曲率半径，r_N为卯酉圈曲率半径；和为载体系x、y、z向陀螺仪测量值；0_i×j表示元素为0的i行j列矩阵；

反欧拉角直接法的系统方程为：

${\stackrel{\·}{x}}_r=F_r\left(x_r\right)+G_r\left(x_r\right)w$

式中，反欧拉角直接法的系统噪声向量w与正欧拉角直接法的相同；F_r(x_r)为反欧拉角直接法的非线性状态转移函数，G_r(x_r)为反欧拉角直接法的系统噪声输入函数，分别根据下式确定：

$F_r\left(x_r\right)=\left(\begin{array}{c}{\tilde{f}}_{ibz}^b({\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r+{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r{\mathrm{sin\γ}}_r)+({\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)({\▿}_{ay}-{\tilde{f}}_{iby}^b)+v_{north}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})-{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r({\▿}_{ax}-{\tilde{f}}_{ibx}^b)\\ -{\tilde{f}}_{ibz}^b({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r-{\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r{\mathrm{sin\γ}}_r)-({\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)({\▿}_{ay}-{\tilde{f}}_{iby}^b)-v_{east}(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N})-{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r({\▿}_{ax}-{\tilde{f}}_{ibx}^b)\\ {\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\ϵ}}_{gx}+{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r+{\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ϵ}}_{gz}{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ϵ}}_{gy}{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ω}}_{ie}\cos L\frac{{\mathrm{sin\ψ}}_r}{{\mathrm{cos\γ}}_r}+\frac{v_{north}{\mathrm{cos\ψ}}_r}{r_M{\mathrm{cos\γ}}_r}-\frac{v_{east}{\mathrm{sin\ψ}}_r}{r_N{\mathrm{cos\γ}}_r}\\ {\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{cos\θ}}_r-{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{sin\θ}}_r-{\mathrm{\ϵ}}_{gy}{\mathrm{cos\θ}}_r+{\mathrm{\ϵ}}_{gz}{\mathrm{sin\θ}}_r-{\mathrm{\ω}}_{ie}{\mathrm{cosLcos\ψ}}_r-\frac{v_{east}}{r_N}{\mathrm{cos\ψ}}_r-\frac{v_{north}{\mathrm{sin\ψ}}_r}{r_M}\\ \frac{{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{v_{east}}{r_N}\tan L-{\mathrm{\ω}}_{ie}\sin L+\frac{{\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{{\mathrm{\ϵ}}_{gz}{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{{\mathrm{\ϵ}}_{gy}{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}-{\mathrm{\ω}}_{ie}{\mathrm{cosLsin\ψ}}_r{\mathrm{tan\γ}}_r+\frac{v_{north}}{r_M}{\mathrm{cos\ψ}}_r{\mathrm{tan\γ}}_r-\frac{v_{east}}{r_N}{\mathrm{sin\ψ}}_r{\mathrm{tan\γ}}_r\\ 0_{5\×1}\end{array}\right)$

G_r(x_r)为10行10列矩阵，其中，

$G_{r1}\left(x_r\right)=\left[\begin{array}{cc}-{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r& {\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\\ -{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r& -{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\end{array}\right]$

$G_{r2}\left(x_r\right)=\left[\begin{array}{ccc}1& -{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r& -{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r\\ 0& -{\mathrm{cos\θ}}_r& {\mathrm{sin\θ}}_r\\ 0& -\frac{{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}& -\frac{{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}\end{array}\right]$

非线性滤波器采用sigma点非线性滤波，时间更新的具体步骤为：

分解前一步的方差矩阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差矩阵及其平方根阵；

计算状态sigma点：

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

式中，i为sigma点的序号；χ_i,k-1|k-1为t_k-1时刻的系统状态向量的第i个sigma点；为t_k-1时刻的系统状态向量估计值；ξ_i为第i个sigma点，为：

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}{\[0,0,...0\]}^T,i=1\\ \sqrt{3}{\[1\]}_i,i=2,3,...21\end{array}\right.$

式中，每个列向量有10个元素，共有20个列向量；

计算非线性状态函数传播的sigma点：

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

式中，为χ_i,k-1|k-1通过非线性状态转移函数F(χ_i,k-1|k-1)传播的sigma点，通过四阶龙格库塔对正/反欧拉角直接法的系统方程进行数值积分求得，如果Flag＝0，则采用正欧拉角直接法的系统方程；如果Flag＝1，则采用反欧拉角直接法的系统方程；

计算状态的一步预测值：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

式中，为一步预测的t_k时刻系统状态向量；a_i为sigma点ξ_i对应的权重，且m为21；

计算一步预测方差矩阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

式中，Q_k-1为t_k-1时刻的系统噪声方差强度阵，且Q_k＝G(x_k)QG(x_k)^TT_s，G(x_k)为t_k时刻的系统噪声输入函数，如果Flag＝0，则采用正欧拉角直接法的系统噪声输入函数；如果Flag＝1，则采用反欧拉角直接法的系统噪声输入函数；T_s为时间更新的周期，与惯性传感器数据更新周期一致；

若未获得新的速度观测值时，时间更新后，将赋给t_k时刻系统状态向量的估计值将P_k|k-1赋给t_k时刻系统状态向量方差矩阵的估计值P_k|k，进入步骤6；

若获得了新的速度观测值，则时间更新后，进入步骤5；

步骤5、根据标志Flag，利用载体系速度的输出以及正/反欧拉角直接法的量测方程，进行非线性滤波器的量测更新，得到更新后的和P_k|k：如果Flag＝0，则采用正欧拉角直接法的量测方程进行非线性滤波器的量测更新；如果Flag＝1，则采用反欧拉角直接法的量测方程进行非线性滤波器的量测更新；

其中，正/反欧拉角直接法的量测方程具体为：

正欧拉角直接法的量测方程为：

z＝H_p(x_p)+η

式中，z为正欧拉角直接法的量测量，η为正欧拉角直接法的量测噪声向量，假设为高斯白噪声，其方差强度阵为R；H_p(x_p)为正欧拉角直接法的非线性量测函数，分别为：

z＝[v_bx,v_by,v_bz]^T

η＝[η_vbx,η_vby,η_vbz]^T

$H_p\left(x_p\right)=\left[\begin{array}{c}{v}_{east}(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})+v_{north}(\cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ})\\ v_{north}\cos \mathrm{\ψ}\cos \mathrm{\θ}-v_{east}\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ v_{east}(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ})+v_{north}(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ})\end{array}\right]$

v_bx、v_by和v_bz分别为载体系中x、y、z向的速度分量；η_vbx、η_vby、η_vbz分别为载体系中x、y、z向的速度量测噪声；

反欧拉角直接法的量测方程为：

z＝H_r(x_r)+η

式中，反欧拉角直接法的量测量z以及量测噪声向量η与正欧拉角直接法的相同；H_r(x_r)为反欧拉角直接法的非线性量测函数，为

$H_r\left(x_r\right)=\left[\begin{array}{c}{v}_{east}{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r+v_{north}{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r\\ v_{north}({\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)-v_{east}({\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r)\\ v_{east}({\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r+{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r{\mathrm{sin\γ}}_r)-v_{north}({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r-{\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r{\mathrm{sin\γ}}_r)\end{array}\right]$

量测更新的具体步骤为：

分解一步预测方差矩阵：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

式中，S_k|k-1为一步预测的t_k时刻系统状态向量方差矩阵的平方根阵；

计算一步预测sigma点：

${\mathrm{\χ}}_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

式中，χ_i,k|k-1为一步预测的t_k时刻系统状态向量的第i个sigma点；

计算通过非线性量测函数传播的量测sigma点：

Z_i,k|k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|k-1通过非线性量测函数H(χ_i,k|k-1)传播的sigma点，即一步预测的t_k时刻系统量测向量的第i个sigma点；非线性量测函数根据Flag标志选取：如果Flag＝0，则采用正欧拉角直接法的量测方程；如果Flag＝1，则采用反欧拉角直接法的量测方程；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{Z}_{i,k|k-1}{a}_i$

式中，为一步预测的t_k时刻量测向量；

计算一步预测量测方差矩阵：

$P_{zz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的t_k时刻量测方差矩阵；R_k＝R/T_f，R_k为t_k时刻离散系统噪声的方差强度阵；T_f为量测更新的周期，与速度传感器数据更新周期一致；

计算一步预测协方差矩阵：

$P_{xz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

式中，P_xz,k|k-1为一步预测的t_k时刻协方差矩阵；

计算卡尔曼滤波增益：

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

更新状态方差矩阵：

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

步骤6、如果Flag＝0，则将直接输出，如果Flag＝1，则先将中的反欧拉角转换为正欧拉角，然后将得到的正欧拉角及中的速度输出；每次输出后，判断对准时间是否截止，如是，则结束本方法流程，否则进入步骤7；

反欧拉角转换为正欧拉角，采用：

当θ≠±90°时，

步骤7、根据标志Flag，以及以正欧拉角表示的纵摇角θ的大小，每次量测更新后，进行正/反欧拉角直接法的切换，重置标志Flag：

如果Flag＝0，且θ∈[-45°,+45°]，则Flag保持不变，返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝0，且θ∈[-90°,-45°)∪(45°,+90°]，则令Flag＝1，将中的正欧拉角转换为反欧拉角，然后返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝1，且θ∈[-45°,+45°]，则令Flag＝0，将中的反欧拉角转换为正欧拉角，然后返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝1，且θ∈[-90°,-45°)∪(45°,+90°]，则Flag保持不变，返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新。

上述实施例仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。

Claims (4)

1.一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，该方法包括以下步骤：

步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的比力信息，使用基于惯性系重力加速度的凝固解析粗对准算法，求得粗略的以正欧拉角表示的初始纵摇角θ₀、横摇角γ₀和航向角ψ₀；

步骤2、根据所述步骤1得到的初始纵摇角θ₀，设置正/反欧拉角直接法的运行标志Flag：如果θ₀∈[-45°,+45°]，则Flag＝0，采用正欧拉角直接法；如果θ₀∈[-90°,-45°)∪(45°,+90°]，则Flag＝1，采用反欧拉角直接法；

步骤3、正/反欧拉角直接法非线性滤波器的初始化：根据标志Flag，在滤波开始时刻t₀，利用粗对准的结果初始化滤波器的系统状态向量及其方差矩阵P₀；

其中系统状态向量根据Flag标志设定：如果Flag＝0，则采用正欧拉角直接法的状态向量，将粗对准得到的正欧拉角直接赋给系统状态向量如果Flag＝1，则采用反欧拉角直接法的状态向量，先将粗对准获得的正欧拉角转换为反欧拉角，然后将得到的反欧拉角赋给系统状态向量

步骤4、每当获得新的陀螺和加速度计测量值，就进行正/反欧拉角直接法非线性滤波器的时间更新：根据标志Flag，利用陀螺和加速度计的输出以及正/反欧拉角直接法的系统方程，进行非线性滤波器的时间更新，得到t_k时刻系统状态向量的一步预测值及其方差矩阵的一步预测值P_k|k-1，如果Flag＝0，则采用正欧拉角直接法的系统方程进行非线性滤波器的时间更新；如果Flag＝1，则采用反欧拉角直接法的系统方程进行非线性滤波器的时间更新；

未获得新的速度观测值时，时间更新后，将赋给t_k时刻系统状态向量的估计值将P_k|k-1赋给t_k时刻系统状态向量方差矩阵的估计值P_k|k，进入步骤6；

若获得了新的速度观测值，则时间更新后，进入步骤5；

步骤5、根据标志Flag，利用载体系速度的输出以及正/反欧拉角直接法的量测方程，进行非线性滤波器的量测更新，得到更新后的和P_k|k：如果Flag＝0，则采用正欧拉角直接法的量测方程进行非线性滤波器的量测更新；如果Flag＝1，则采用反欧拉角直接法的量测方程进行非线性滤波器的量测更新；

步骤6、如果Flag＝0，则直接输出中的正欧拉角和速度分量，如果Flag＝1，则先将中的反欧拉角转换为正欧拉角，然后输出该正欧拉角和中的速度分量；每次输出后，判断对准时间是否截止，如是，则结束本方法流程，否则进入步骤7；

步骤7、根据标志Flag，以及以正欧拉角表示的纵摇角θ的大小，每次量测更新后，进行正/反欧拉角直接法的切换，重置标志Flag：

如果Flag＝0，且θ∈[-45°,+45°]，则Flag保持不变，返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝0，且θ∈[-90°,-45°)∪(45°,+90°]，则令Flag＝1，将中的正欧拉角转换为反欧拉角，然后返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝1，且θ∈[-45°,+45°]，则令Flag＝0，将中的反欧拉角转换为正欧拉角，然后返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；

如果Flag＝1，且θ∈[-90°,-45°)∪(45°,+90°]，则Flag保持不变，返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新。

2.根据权利要求1所述的一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，所述的步骤4中正/反欧拉角直接法的系统方程具体为：

正欧拉角直接法的系统方程为：

${\stackrel{\·}{x}}_p=F_p\left(x_p\right)+G_p\left(x_p\right)w$

式中，x_p为正欧拉角直接法的状态向量，w为正欧拉角直接法的系统噪声向量，F_p(x_p)为正欧拉角直接法的非线性状态转移函数矩阵，G_p(x_p)为正欧拉角直接法的系统噪声输入函数矩阵，分别根据下式确定：

$x_p={\[v_{east},v_{north},\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{ax},{\▿}_{ay},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

w＝[w_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0]^T

$F_p\left(x_p\right)=\left(\begin{array}{c}{\tilde{f}}_{ibz}^b\left(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}\right)-\left(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}\right)\left({\▿}_{ax}-{\tilde{f}}_{ibx}^b\right)+v_{north}\left(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N}\right)+\cos \mathrm{\θ}\sin \mathrm{\ψ}\left({\▿}_{ay}-{\tilde{f}}_{iby}^b\right)\\ {\tilde{f}}_{ibz}^b\left(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}\right)-\left(\cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}\right)\left({\▿}_{ax}-{\tilde{f}}_{ibx}^b\right)-v_{east}\left(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N}\right)-\cos \mathrm{\ψ}\cos \mathrm{\θ}\left({\▿}_{ay}-{\tilde{f}}_{iby}^b\right)\\ {\widetilde{\mathrm{\ω}}}_{ibx}^b\cos \mathrm{\γ}+{\widetilde{\mathrm{\ω}}}_{ibz}^b\sin \mathrm{\γ}-{\mathrm{\ϵ}}_{gx}\cos \mathrm{\γ}-{\mathrm{\ϵ}}_{gz}\sin \mathrm{\γ}+\frac{v_{north}\cos \mathrm{\ψ}}{r_M}-{\mathrm{\ω}}_{ie}\cos L\sin \mathrm{\ψ}-\frac{v_{east}}{r_N}\sin \mathrm{\ψ}\\ {\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\ϵ}}_{gy}-{\widetilde{\mathrm{\ω}}}_{ibz}^b\cos \mathrm{\γ}\tan \mathrm{\θ}+{\widetilde{\mathrm{\ω}}}_{ibx}^b\sin \mathrm{\γ}\tan \mathrm{\θ}+{\mathrm{\ϵ}}_{gz}\cos \mathrm{\γ}\tan \mathrm{\θ}-{\mathrm{\ϵ}}_{gx}\sin \mathrm{\γ}\tan \mathrm{\θ}-\frac{v_{north}\sin \mathrm{\ψ}}{r_M\cos \mathrm{\θ}}-{\mathrm{\ω}}_{ie}\cos L\frac{\cos \mathrm{\ψ}}{\cos \mathrm{\θ}}-\frac{v_{east}\cos \mathrm{\ψ}}{r_N\cos \mathrm{\θ}}\\ \frac{{\widetilde{\mathrm{\ω}}}_{ibz}^b\cos \mathrm{\γ}}{\cos \mathrm{\θ}}-\frac{v_{east}}{r_N}\tan L-{\mathrm{\ω}}_{ie}\sin L-\frac{{\widetilde{\mathrm{\ω}}}_{ibx}^b\sin \mathrm{\γ}}{\cos \mathrm{\θ}}-\frac{{\mathrm{\ϵ}}_{gz}\cos \mathrm{\γ}}{\cos \mathrm{\θ}}+\frac{{\mathrm{\ϵ}}_{gx}\sin \mathrm{\γ}}{\cos \mathrm{\θ}}+\frac{v_{north}}{r_M}\sin \mathrm{\ψ}\tan \mathrm{\θ}+{\mathrm{\ω}}_{ie}\cos L\cos \mathrm{\ψ}\tan \mathrm{\θ}+\frac{v_{east}}{r_N}\cos \mathrm{\ψ}\tan \mathrm{\θ}\\ 0_{5\×1}\end{array}\right)$

G_p(x_p)为10行10列的矩阵，其中，

$G_{p1}\left(x_p\right)=\left[\begin{array}{cc}\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}\end{array}\right]$

$G_{p2}\left(x_p\right)=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& sin\mathrm{\γ}\\ tan\mathrm{\θ}\sin \mathrm{\γ}& 1& -cos\mathrm{\γ}tan\mathrm{\θ}\\ -\frac{sin\mathrm{\γ}}{\cos \mathrm{\θ}}& 0& \frac{cos\mathrm{\γ}}{cos\mathrm{\θ}}\end{array}\right]$

v_east为东向速度，v_north为北向速度，θ、γ和ψ分别为正欧拉角组合的纵摇角、横摇角和航向角；分别为载体系中x、y向加速度计测量的随机常值偏置，ε_gx、ε_gy和ε_gz分别为载体系中x、y、z向陀螺仪测量的随机常值漂移；w_ax、w_ay分别为载体系中x、y向加速度计测量的随机噪声误差，w_gx、w_gy和w_gz分别为载体系中x、y、z向陀螺仪测量的随机噪声误差；和分别为载体系x、y、z向加速度计的测量值，ω_ie为地球自转角速率，L为当地地理纬度，r_M为子午圈曲率半径，r_N为卯酉圈曲率半径；和为载体系x、y、z向陀螺仪测量值；0_i×j表示元素为0的i行j列矩阵；

反欧拉角直接法的系统方程为：

${\stackrel{\·}{x}}_r=F_r\left(x_r\right)+G_r\left(x_r\right)w$

式中，反欧拉角直接法的系统噪声向量w与正欧拉角直接法的相同；x_r为反欧拉角直接法的状态向量，F_r(x_r)为反欧拉角直接法的非线性状态转移函数矩阵，G_r(x_r)为反欧拉角直接法的系统噪声输入函数矩阵，分别根据下式确定：

$x_r={\[v_{east},v_{north},{\mathrm{\θ}}_r,{\mathrm{\γ}}_r,{\mathrm{\ψ}}_r,{\▿}_{ax},{\▿}_{ay},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

$F_r\left(x_r\right)=\left(\begin{array}{c}{\tilde{f}}_{ibz}^b\left({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r+{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r{\mathrm{sin\γ}}_r\right)-\left({\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\right)\left({\▿}_{ay}-{\tilde{f}}_{iby}^b\right)+v_{north}\left(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N}\right)-{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r\left({\▿}_{ax}-{\tilde{f}}_{ibx}^b\right)\\ -{\tilde{f}}_{ibz}^b\left({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r-{\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r{\mathrm{sin\γ}}_r\right)-\left({\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\right)\left({\▿}_{ay}-{\tilde{f}}_{iby}^b\right)-v_{east}\left(2{\mathrm{\ω}}_{ie}\sin L+\frac{v_{east}\tan L}{r_N}\right)-{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r\left({\▿}_{ax}-{\tilde{f}}_{ibx}^b\right)\\ {\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\ϵ}}_{gx}+{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r+{\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ϵ}}_{gz}{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ϵ}}_{gy}{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r-{\mathrm{\ω}}_{ie}\cos L\frac{{\mathrm{sin\ψ}}_r}{{\mathrm{cos\γ}}_r}+\frac{v_{north}{\mathrm{cos\ψ}}_r}{r_M{\mathrm{cos\γ}}_r}-\frac{v_{east}{\mathrm{sin\ψ}}_r}{r_N{\mathrm{cos\γ}}_r}\\ {\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{cos\θ}}_r-{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{sin\θ}}_r-{\mathrm{\ϵ}}_{gy}{\mathrm{cos\θ}}_r+{\mathrm{\ϵ}}_{gz}{\mathrm{sin\θ}}_r-{\mathrm{\ω}}_{ie}{\mathrm{cosLcos\ψ}}_r-\frac{v_{east}}{r_N}{\mathrm{cos\ψ}}_r-\frac{v_{north}{\mathrm{sin\ψ}}_r}{r_M}\\ \frac{{\widetilde{\mathrm{\ω}}}_{ibz}^b{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{v_{east}}{r_N}\tan L-{\mathrm{\ω}}_{ie}\sin L-\frac{{\widetilde{\mathrm{\ω}}}_{iby}^b{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{{\mathrm{\ϵ}}_{gz}{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}-\frac{{\mathrm{\ϵ}}_{gy}{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}-{\mathrm{\ω}}_{ie}{\mathrm{cosLsin\ψ}}_r{\mathrm{tan\γ}}_r+\frac{v_{north}}{r_M}{\mathrm{cos\ψ}}_r{\mathrm{tan\γ}}_r-\frac{v_{east}}{r_N}{\mathrm{sin\ψ}}_r{\mathrm{tan\γ}}_r\\ 0_{5\×1}\end{array}\right)$

G_r(x_r)为10行10列矩阵，其中，

$G_{r1}\left(x_r\right)=\left[\begin{array}{cc}-{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r& {\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\\ -{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r& -{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\end{array}\right]$

$G_{r2}\left(x_r\right)=\left[\begin{array}{ccc}1& -{\mathrm{sin\θ}}_r{\mathrm{tan\γ}}_r& -{\mathrm{cos\θ}}_r{\mathrm{tan\γ}}_r\\ 0& -{\mathrm{cos\θ}}_r& {\mathrm{sin\θ}}_r\\ 0& -\frac{{\mathrm{sin\θ}}_r}{{\mathrm{cos\γ}}_r}& -\frac{{\mathrm{cos\θ}}_r}{{\mathrm{cos\γ}}_r}\end{array}\right]$

式中：ψ_r、θ_r和γ_r分别为反欧拉角的航向角、纵摇角和横摇角。

3.根据权利要求1所述的一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，所述的步骤5中正/反欧拉角直接法的量测方程具体为：

正欧拉角直接法的量测方程为：

z＝H_p(x_p)+η

式中，z为正欧拉角直接法的量测量，η为正欧拉角直接法的量测噪声向量，H_p(x_p)为正欧拉角直接法的非线性量测函数，分别根据下式确定：

z＝[v_bx,v_by,v_bz]^T

η＝[η_vbx,η_vby,η_vbz]^T

$H_p\left(x_p\right)=\left[\begin{array}{c}{v}_{east}\left(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}\right)+v_{north}\left(\cos \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}\right)\\ v_{north}\cos \mathrm{\ψ}\cos \mathrm{\θ}-v_{east}\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ v_{east}\left(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}\right)+v_{north}\left(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}\right)\end{array}\right].$

v_bx、v_by和v_bz分别为载体系中x、y、z向的速度分量；η_vbx、η_vby、η_vbz分别为载体系中x、y、z向的速度量测噪声；

反欧拉角直接法的量测方程为：

z＝H_r(x_r)+η

式中，反欧拉角直接法的量测量z以及量测噪声向量η与正欧拉角直接法的相同；H_r(x_r)为反欧拉角直接法的非线性量测函数，为

$H_r\left(x_r\right)=\left[\begin{array}{c}{v}_{east}{\mathrm{cos\ψ}}_r{\mathrm{cos\γ}}_r+v_{north}{\mathrm{cos\γ}}_r{\mathrm{sin\ψ}}_r\\ v_{north}\left({\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r+{\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\right)-v_{east}\left({\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r-{\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r{\mathrm{sin\γ}}_r\right)\\ v_{east}\left({\mathrm{sin\ψ}}_r{\mathrm{sin\θ}}_r+{\mathrm{cos\ψ}}_r{\mathrm{cos\θ}}_r{\mathrm{sin\γ}}_r\right)-v_{north}\left({\mathrm{cos\ψ}}_r{\mathrm{sin\θ}}_r-{\mathrm{cos\θ}}_r{\mathrm{sin\ψ}}_r{\mathrm{sin\γ}}_r\right)\end{array}\right].$

4.根据权利要求1、2或3所述的一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，所述非线性滤波器采用sigma点非线性滤波。