本发明属于导航技术领域，尤其涉及一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法。
背景技术：
对准精度和对准实时性是惯导系统进行初始对准时的两项重要技术指标。初始对准精度影响SINS的性能，初始对准实时性标志着系统的快速反应能力。因此，在保证初始对准精度的同时提高初始对准的实时性，才能提高捷联惯导系统的性能。
针对捷联惯导初始对准非线性、非高斯噪声的特点，EKF(Extended Kalman Filter)和UKF(Unscented Kalman Filter)等基于模型近似线性化和噪声为高斯条件下的滤波方法，受到线性卡尔曼滤波算法的条件制约，若仍简单地采用均值和方差表征状态概率分布，将导致滤波性能变差。PF(particle filter)不需要对状态变量的概率密度作过多的约束，其不受模型非线性及高斯假设的限制，适用于任何非线性非高斯的随机系统，从这个意义上讲，相比于EKF和UKF，PF是非高斯非线性系统状态估计的“最优”滤波器，可以应用于SINS大方位失准角初始对准。
粒子滤波方法存在重要性分布函数难以选取、粒子退化、粒子易贫化、计算量大等缺陷。其改进主要围绕构建更合理的重要性分布函数、改进重采样法则、改变粒子数以减少计算量等。针对这一问题，已有学者将UPF(Unscented Particle Filtering)滤波方法应用到非线性初始对准中，其重要性概率密度通过UKF算法获得，从非线性状态向量的概率分布角度出发，选取Sigma采样点表示状态变量，并根据非线性模型计算后验分布的均值和方差来实现对状态向量后验分布的近似，能够得到比较逼近真实后验概率密度的重要性概率密度。捷联惯导误差模型维数较高，直接应用粒子滤波会带来维数灾难，计算量会急剧增加，初始对准的实时性严重降低，惯导系统的性能就会受到严重影响。在大方位失准角条件下，强非线性、非高斯噪声条件下SINS静基座下的初始对准，由于初始对准需要采样的状态变量较多，利用现有滤波算法，虽然能够保证对准精度，但是存在计算量随着状态维数增加而急剧增大的问题，造成初始对准时间长，实时性差。
技术实现要素：
发明目的：本发明提出一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法，能有效地降低滤波过程的计算复杂度，增强滤波算法的实时性，同时提高初始对准在模型误差、噪声、干扰等不确定因素下的滤波精度和鲁棒性。
技术方案：一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法，将大失准角下非线性初始对准模型分解为线性与非线性部分，分别采用KF和强跟踪的UPF完成状态估计，具体包括如下步骤：
S1：建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型
根据初始对准捷联惯导系统中速度、姿态角误差方程，在大方位失准角条件下，假设水平失准角φE，φN是小量，方位失准角φU为大角度时，忽略二阶及二阶以上的高阶项等，建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型；
S2：建立大方位失准角静基座初始对准下线性部分和非线性部分滤波模型
将步骤S1得到的捷联惯导系统大方位失准角静基座初始对准的状态空间模型分解为线性部分和非线性部分；取东向、北向速度误差δvE，δvN作为线性部分的观测量，建立捷联惯导系统大方位失准角静基座初始对准线性滤波模型离散化的状态方程和量测方程；结合线性部分的状态方程和量测方程，得到非线性部分的观测量，进而建立初始对准非线性滤波模型离散化的状态方程和量测方程；
S3：采用KF和STUPF组合滤波算法完成状态估计
对步骤S2中得到的大方位失准角静基座初始对准下线性部分滤波模型，采用KF算法进行处理；大方位失准角静基座初始对准下非线性部分的滤波模型，采用强跟踪的STUPF算法进行处理，基于KF和STUPF组合滤波算法，完成状态估计。
进一步的，所述步骤S1中建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型的方法如下：
静基座初始对准时，失准角定义为φ＝[φE φN φU]T，φE，φNφU分别为东向、北向、天向的失准角；参考的速度vn＝0，地理位置准确已知且不变，L为当地的纬度；假设陀螺仪测量误差主要为常值漂移εb和随机漂移(视为零均值高斯白噪声)，下标x、y、z为载体轴向；加速度计的测量误差δfb主要为常值偏置和随机噪声(视为零均值高斯白噪声)，下标x、y、z为载体轴向；忽略重力加速度误差项δgn，忽略二阶及二阶以上的高阶项，则静基座大方位失准角下初始对准的速度、姿态角误差方程可以简化为如下：
其中，i，e，b，n，n'分别代表惯性、地球、载体、理想和计算的导航坐标系，理想的导航坐标系为东北天地理坐标系，载体坐标系为右前上坐标系；其中，统一定义符号表示k系相对于j的角速度矢量在l系上的投影，δvn为vn计算误差，为地球自转的角速率，为的计算误差，fb为载体坐标系下加速度计的输出，是n系到n'的变换矩阵。δvE，δvN分别为东向，北向的速度误差，RM，RN分别为子午圈和卯酉圈的主曲率半径，h为地理高度，矩阵和从n系到n'的变换矩阵可以近似为：
进一步的，所述步骤S2中建立大方位失准角静基座初始对准下线性部分和非线性部分滤波模型的方法如下：
a.线性部分的滤波模型
取东向、北向速度误差作为线性部分的观测量，即Z＝[δvE δvN]T，线性部分离散化的状态方程和量测方程如下:
其中，为k+1时刻的状态向量，为k+1时刻的量测值，为k时刻的系统噪声，为k+1时刻的量测噪声序列，为k时刻到k+1时刻的一步状态转移矩阵，是系统噪声输入矩阵，为输入控制矩阵，当作控制信号处理，量测矩阵系统噪声量测噪声
b.非线性部分的滤波模型
非线性部分的观测方程及其观测量无法直观得到。将线性部分东向、北向速度误差的微分方程离散化可得：
k+1时刻线性部分的观测量如下式：
得到非线性部分在k时刻的观测量，也就是说，对于非线性部分，k+1时刻估计的是k时刻的非线性部分的状态变量
可以得到非线性部分的状态方程和量测方程为：
其中，为k时刻的状态向量，为k时刻的量测值，为k-1时刻的系统噪声，为k+1时刻的测量噪声序列，为非线性离散函数，是系统噪声输入矩阵，为输入控制矩阵，当作控制信号处理，是量测矩阵，系统噪声量测噪声
进一步的，所述步骤S3中所述KF和STUPF组合滤波算法如下：
a.滤波初始化
在初始时刻即第k＝0时刻，对线性部分的状态变量初始化，假设已知；对非线性部分的状态变量进行初始化，即对初始状态的先验概率密度分布进行采样，生成N个服从概率分布的粒子i＝1,...,N，其均值和方差分别满足：
并选取i＝1,...,N，初始状态可得：
下面的步骤按照k＝1,2,3,...执行
b.KF处理
在第k时刻,已知线性部分的量测量对线性部分的滤波模型采用卡尔曼滤波处理，包括时间更新和量测更新，可以得到线性部分在k时刻的状态估计
c.STUPF处理
在第k+1时刻,已知线性部分的量测量根据非线性部分的量测方程，可以将量测量转化为k时刻非线性部分的观测量，对非线性部分的滤波模型采用强跟踪的UPF算法滤波处理，可以得到k时刻非线性部分的状态估计值由于量测矩阵为线性矩阵，所以使用UPF算法进行滤波；
d.再次采用KF处理
根据第k时刻所得到的全部状态变量与k+1时刻线性部分的观测量再次采用卡尔曼滤波处理，可以得到k+1时刻线性部分的状态变量
e.令k＝k+1，返回到步骤c，循环运算，就可以得到捷联惯导系统大方位失准角初始对准过程中各个时刻失准角的估计值。
有益效果：相对于现有技术，本发明克服了捷联惯导大方位失准角静基座初始对准利用现有滤波方法计算量大，初始对准实时性差的不足，在保证初始对准精度的同时提高初始对准的实时性，从而提高捷联惯导系统的性能。综合在不同运行环境下的适应能力、滤波精度和时间耗费，基于KF和STUPF组合滤波算法可以充分利用卡尔曼滤波和粒子滤波的各自优点，既避免了单纯采用kalman滤波带来的非线性估计误差，同时也降低了纯粹使用粒子滤波造成的巨大计算量，是一种兼顾系统估计精度和算法实时性的滤波，具有实用价值。同时，由于在模型误差、噪声、干扰等不确定因素比较大的情况下，初始对准滤波精度和鲁棒性会严重降低，本发明将强跟踪理论与UPF相结合，从而提高UPF的滤波精度和鲁棒性。
附图说明
图1为本发明的流程图。
具体实施方式
下面将结合附图，对本发明的实施案例进行详细的描述；
如图1所示，本发明利用KF(Kalman Filter，卡尔曼滤波)和STUPF(Strong Tracing Unscented Particle Filter，无迹粒子滤波)组合滤波方法来解决静基座下捷联惯导大方位失准角初始对准问题，将大失准角下非线性初始对准模型分解为线性与非线性部分，分别采用KF和强跟踪的UPF完成状态估计，具体实现步骤如下：
1、建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型
根据初始对准捷联惯导系统中速度、姿态角误差方程，在大方位失准角条件下，假设水平失准角φE，φN是小量，方位失准角φU为大角度时，忽略二阶及二阶以上的高阶项等，建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型；
在捷联惯导系统中,初始对准的速度、姿态角误差方程分别为：
其中，i，e，b，n，n'分别代表惯性、地球、载体、理想和计算的导航坐标系，理想的导航坐标系为东北天地理坐标系，载体坐标系为右前上坐标系；其中，统一定义符号表示k系相对于j的角速度矢量在l系上的投影，为速度vn实际计算值，δvn为vn计算误差；为的实际计算值，为地球自转的角速率，为的计算误差；为的实际计算值，为的计算误差；为加速度计比力输出，fb为载体坐标系下加速度计的输出，δfb为加速度计量测误差，为陀螺仪量测误差，gn为当地的重力加速度，δgn为重力加速度误差项。失准角定义为φ＝[φE φN φU]T，φE，φNφU分别为东向、北向、天向的失准角。在大方位失准角条件下，假设水平失准角φE，φN是小量，方位失准角φU为大角度时，则矩阵和从n系到n'的变换矩阵可以近似为：
静基座初始对准时，参考的速度为vn＝0，δvE，δvN分别为东向，北向的速度误差，RM，RN分别为子午圈和卯酉圈的主曲率半径，h为地理高度，地理位置准确已知且不变，L为当地的纬度，假设陀螺仪测量误差主要为常值漂移εb和随机漂移(视为零均值高斯白噪声)，下标x、y、z为载体轴向；加速度计的测量误差δfb主要为常值偏置和随机噪声(视为零均值高斯白噪声)，下标x、y、z为载体轴向；忽略重力加速度误差项δgn，忽略二阶及二阶以上的高阶项，则静基座大方位失准角下初始对准的速度、姿态角误差方程可以简化为：
2、建立捷联惯导系统大方位失准角静基座初始对准的线性和非线性滤波模型
将步骤1得到的捷联惯导系统大方位失准角静基座初始对准的状态空间模型分解为线性部分和非线性部分；取东向、北向速度误差δvE，δvN作为线性部分的观测量，建立捷联惯导系统大方位失准角静基座初始对准线性滤波模型离散化的状态方程和量测方程；结合线性部分的状态方程和量测方程，得到非线性部分的观测量，进而建立初始对准非线性滤波模型离散化的状态方程和量测方程；
忽略高度通道的影响，选择线性部分的状态变量噪声向量非线性部分的状态变量Xn＝[φE φN φU]T，噪声向量
a.线性部分的滤波模型
由式(4)可得线性部分的状态方程如式(5)：
其中，cij是转换矩阵的第i行，第j列的元素，i＝1,...,3,j＝1,...,3；ωie是地球自转的角速度，L为当地的地理纬度。取东向、北向速度误差作为线性部分的观测量，即Z＝[δvE δvN]T，线性部分离散化的状态方程和量测方程如式
(6):
其中，，为k+1时刻的状态向量，为k+1时刻的量测值，为k时刻的系统噪声，为k+1时刻的量测噪声序列，为k时刻到k+1时刻的一步状态转移矩阵，是系统噪声输入矩阵，为输入控制矩阵，当作控制信号处理，量测矩阵系统噪声量测噪声
b.非线性部分的滤波模型
由式(4)可得线性部分的状态方程如式(7)：
非线性部分的观测方程及其观测量无法直观得到。将式(5)东向、北向速度误差的微分方程离散化：
其中，Ts为滤波周期，k+1时刻线性部分的观测量如式(9)：
利用k+1时刻线性部分的观测量结合式(8)可以得到非线性部分在k时刻的观测量，也就是说，对于非线性部分，k+1时刻估计的是k时刻的非线性部分的状态变量
结合式(8)、式(10)和离散化的式(7)，可得到非线性部分的状态方程和量测方程为：
其中，为k时刻的状态向量，为k时刻的量测值，为k-1时刻的系统噪声，为k+1时刻的测量噪声序列，为非线性离散函数，是系统噪声输入矩阵，为输入控制矩阵，当作控制信号处理，是量测矩阵，系统噪声量测噪声
3、采用KF和STUPF组合滤波算法完成状态估计
对步骤2中得到的大方位失准角静基座初始对准下线性部分滤波模型，采用KF算法进行处理；大方位失准角静基座初始对准下非线性部分的滤波模型，采用强跟踪的STUPF算法进行处理，基于KF和STUPF组合滤波算法，完成状态估计。
a.滤波初始化
在初始时刻即第k＝0时刻，对线性部分的状态变量初始化，假设已知；对非线性部分的状态变量进行初始化，即对初始状态的先验概率密度分布进行采样，生成N个服从概率分布的粒子i＝1,...,N，其均值和方差分别满足式(12)：
并选取i＝1,...,N，初始状态计算可得如式(13)所示：
下面的步骤按照k＝1,2,3,...执行
b.KF处理
在第k时刻,已知线性部分的量测量对线性部分的滤波模型用卡尔曼滤波处理：
1).卡尔曼滤波的时间更新
2).卡尔曼滤波的量测更新
c.STUPF处理
在第k+1时刻,已知线性部分的量测量根据式(11)的分析可知，可以将量测量可以转化为k时刻非线性部分的观测量，对非线性部分的滤波模型采用STUPF处理，可以得到k时刻非线性部分的状态估计值由于量测矩阵为线性矩阵，所以可以使用简化的UPF算法进行滤波。
1).STUKF算法
其中表示每个列向量均为的一个含L列的矩阵，L是状态的维数，λ＝α2(L+κ)-L，α用于确定Sigma点在均值附近的分布情况(往往取为一个小的正值，如e-4≤α≤1)，κ是一比例因子(一般在状态估计时设为0，在参数估计时取为3-L)，i＝1,...,N。
本文改进的强跟踪UPF算法在量测预测协方差和互协方差时均引入了渐消因子其具体的定义为：
定义残差为残差的协方差阵，可以通过式(19)估算
其中，β为弱化因子，0＜ρ≤1为遗忘因子。
2).选取重要推荐密度
并由该推荐概率密度生成N个粒子作为二次采样的原始粒子。
3).计算粒子的权重系数
其中根据概率密度计算，根据概率密度计算，根据概率密度
4).权值归一化
5).计算有效粒子的尺寸Neff
如果Neff小于门限值Nth，说明粒子的多样性降低，将转步骤6)进行重采样，否则转入步骤7)进行初始对准状态估计。
6)重采样
重采样的目的是消除权值较小的粒子，增加权值较大的粒子，使重采样后的样本集的分布符合后验密度p(Xk/Zk)。采用SIR或者残差二次采用法对原始的粒子作二次采样，生成二次采样的的粒子采样用的每个粒子的权值重新设置为1/N。
7)初始对准状态估计
第k时刻初始对准状态估计值和估计误差方差阵分别为:
d.再次采用KF处理
根据第k时刻所得到的全部状态变量与k+1时刻线性部分的观测量再次采用卡尔曼滤波处理，可以的到k+1时刻线性部分的状态变量
e.令k＝k+1，返回到步骤c，循环运算。就可以得到捷联惯导系统大方位失准角初始对准过程中各个时刻失准角的估计值。
本发明将大失准角下非线性初始对准滤波模型分解为线性与非线性两部分，推导了线性滤波模型离散化的状态方程和量测方程，采用卡尔曼滤波进行处理；结合线性部分的状态方程和量测方程，得到非线性部分的观测量，进而建立初始对准非线性部分离散化的滤波模型，采用强跟踪的UPF滤波进行状态估计，得到失准角的估计值。本发明基于KF和STUPF组合滤波算法，既保证了初始对准精度，又降低了滤波器状态维数，避免了维数灾难，克服了计算量大、实时性差的不足，在保证初始对准精度的同时提高初始对准的实时性，具有实用价值。本发明既避免了单纯采用kalman滤波带来的非线性估计误差，同时也降低了纯粹使用粒子滤波造成的巨大计算量，需要进行UPF滤波的状态维数由原来的10维降为3维，增强了滤波算法的实时性；通过引入渐消因子有效增强当前信息残差对系统修正作用，以提高初始对准在模型误差、噪声、干扰等不确定因素下的滤波精度和鲁棒性。充分利用了卡尔曼滤波和粒子滤波的各自优点，在保证初始对准精度的同时提高了初始对准的实时性。