一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法技术

本发明专利技术公开了一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法,将大失准角下非线性初始对准滤波模型分解为线性与非线性两部分，建立了线性滤波模型离散化的状态方程和量测方程，采用卡尔曼滤波进行处理；结合线性部分的状态方程和量测方程，得到非线性部分的观测量，进而建立初始对准非线性部分离散化的滤波模型，采用强跟踪的UPF滤波进行状态估计，得到失准角的估计值。本发明专利技术基于KF和STUPF组合滤波算法，既保证了初始对准精度，又降低了滤波器状态维数，避免了维数灾难，克服了计算量大、实时性差的不足，在保证初始对准精度的同时提高初始对准的实时性，具有实用价值。

【技术实现步骤摘要】
一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法
本专利技术属于导航
，尤其涉及一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法。
技术介绍
对准精度和对准实时性是惯导系统进行初始对准时的两项重要技术指标。初始对准精度影响SINS的性能，初始对准实时性标志着系统的快速反应能力。因此，在保证初始对准精度的同时提高初始对准的实时性，才能提高捷联惯导系统的性能。针对捷联惯导初始对准非线性、非高斯噪声的特点，EKF(ExtendedKalmanFilter)和UKF(UnscentedKalmanFilter)等基于模型近似线性化和噪声为高斯条件下的滤波方法，受到线性卡尔曼滤波算法的条件制约，若仍简单地采用均值和方差表征状态概率分布，将导致滤波性能变差。PF(particlefilter)不需要对状态变量的概率密度作过多的约束，其不受模型非线性及高斯假设的限制，适用于任何非线性非高斯的随机系统，从这个意义上讲，相比于EKF和UKF，PF是非高斯非线性系统状态估计的“最优”滤波器，可以应用于SINS大方位失准角初始对准。粒子滤波方法存在重要性分布函数难以选取、粒子退化、粒子易贫化、计算量大等缺陷。其改进主要围绕构建更合理的重要性分布函数、改进重采样法则、改变粒子数以减少计算量等。针对这一问题，已有学者将UPF(UnscentedParticleFiltering)滤波方法应用到非线性初始对准中，其重要性概率密度通过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时刻所得到的全部状本文档来自技高网...

【技术保护点】
一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法，其特征在于，将大失准角下非线性初始对准模型分解为线性与非线性部分，分别采用KF和强跟踪的UPF完成状态估计，具体包括如下步骤：S1：建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型根据初始对准捷联惯导系统中速度、姿态角误差方程，在大方位失准角条件下，假设水平失准角φ

【技术特征摘要】
1.一种基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法，其特征在于，将大失准角下非线性初始对准模型分解为线性与非线性部分，分别采用KF和强跟踪的UPF完成状态估计，具体包括如下步骤：S1：建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型根据初始对准捷联惯导系统中速度、姿态角误差方程，在大方位失准角条件下，假设水平失准角φE，φN是小量，方位失准角φU为大角度时，忽略二阶及二阶以上的高阶项等，建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型；S2：建立大方位失准角静基座初始对准下线性部分和非线性部分滤波模型将步骤S1得到的捷联惯导系统大方位失准角静基座初始对准的状态空间模型分解为线性部分和非线性部分；取东向、北向速度误差δvE，δvN作为线性部分的观测量，建立捷联惯导系统大方位失准角静基座初始对准线性滤波模型离散化的状态方程和量测方程；结合线性部分的状态方程和量测方程，得到非线性部分的观测量，进而建立初始对准非线性滤波模型离散化的状态方程和量测方程；S3：采用KF和STUPF组合滤波算法完成状态估计对步骤S2中得到的大方位失准角静基座初始对准下线性部分滤波模型，采用KF算法进行处理；大方位失准角静基座初始对准下非线性部分的滤波模型，采用强跟踪的STUPF算法进行处理，基于KF和STUPF组合滤波算法，完成状态估计。2.根据权利要求1所述的基于KF和STUPF组合滤波的SINS大方位失准角初始对准方法，其特征在于，所述步骤S1中建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型的方法如下：静基座初始对准时，失准角定义为φ＝[φEφNφU]T，φE，φNφU分别为东向、北向、天向的失准角；参考的速度vn＝0，地理位置准确已知且不变，L为当地的纬度；假设陀螺仪测量误差主要为常值漂移εb和随机漂移(视为零均值高斯白噪声)，下标x、y、z为载体轴向；加速度计的测量误差δfb主要为常值偏置▽b和随机噪声(视为零均值高斯白噪声)，下标x、y、z为载体轴向；忽略重力加速度误差项δgn，忽略二阶及二阶以上的高阶项，则静基座大方位失准角下初始对准的速度、姿态角误差方程可以简化为如下：其中，i，e，b，n，n'分别代表惯性、地球、载体、理想和计算的导航坐标系，理想的导航坐标系为东北天地理坐标系，载体坐标系为右前上坐标系；其中，统一定义符号表示k系相对于j的角速度矢量在l系上的投影，δvn为vn计算误差，为地球自转的角速率，为的计算误差，fb为载体坐标系下加速度计的输出，是n系到n'的变换矩阵。δvE，δvN分别为东向，北向的速度误差，RM，RN分别为子午圈和卯酉圈的主曲率半径，h为地理高度，矩阵和从n系到n'的变换矩阵可以近似为：3.根据权利要求1所述的基于KF和STUPF组合滤波的SINS大...

【专利技术属性】
技术研发人员：王健，朱永云，颜亚雄，杨书天，胡贺庆，王自强，陈浩，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32