一种改进的强跟踪容积卡尔曼滤波组合导航方法技术

本发明专利技术是一种改进的强跟踪容积卡尔曼滤波组合导航方法。收集SINS系统和GPS系统输出；选择状态量和观测量，建立SINS/GPS组合导航系统状态空间模型；SINS/GPS组合导航系统初始化；在k时刻进行标准CKF的时间更新和量测更新，得到一步状态预测的误差协方差值和一步预测的互协方差值；利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；利用校正后的一步状态预测的误差协方差值进行CKF的量测更新，得到k+1时刻的状态估计值和和状态误差协方差。本发明专利技术能提高系统的滤波精度，提高滤波器对系统状态发生突变时的跟踪能力，增强SINS/GPS组合导航系统的鲁棒性。

【技术实现步骤摘要】
一种改进的强跟踪容积卡尔曼滤波组合导航方法
本专利技术涉及的是一种组合导航方法。
技术介绍
单独的导航系统很难满足人类对于精度高、稳定性强的导航系统的需求，而将两种或两种以上的导航系统通过某种方法组合起来构成组合导航系统能使导航的精度得到很大的提升。目前，应用最为广泛的组合导航系统是捷联惯性导航系统(StrapdownInertialNavigationSystem,SINS)和全球定位系统(GlobalPositionSystem，GPS)，SINS可以弥补GPS抗干扰性差、动态性能不足的缺点；GPS可以弥补SINS误差随时间积累的缺点。可以很好地展现SINS的自主性、抗干扰能力强和GPS全天候、高精度等优点，所以SINS/GPS组合导航系统的应用前景非常可观。在实际应用中大多数的组合导航系统都是非线性的，而非线性滤波方法主要有扩展卡尔曼滤波(ExtendedKalmanFilter，EKE)、无迹卡尔曼滤波(UncentedKalmanFilter，UKF)、容积卡尔曼滤波(CubatureKalmanFilter，CKF)三种滤波方法。EKF需要对方程进行泰勒级数展开来求解雅可比矩阵，计算量大，且对非线性函数进行线性化近似精度不高。以上缺陷限制了EKF在非线性系统中的应用。UKF基于“对概率分布进行近似要比对非线性函数近似要容易得多”的思想提出了无迹变换，数学理论相对薄弱。对于n维系统，在UT变换的过程中需要计算2n+1个采样点。且中心点权值与其他采样点权值不同，当系统维数较高时，中心点采样点权值为负，会出现协方差非正定的情况，使得滤波难以正常进行。CKF是基于“三阶球面-相径求容积”的规则，经过严密的数学公式推导出来的，对于n维系统，只需计算2n个采样点。相较于UKF，CKF能减小计算量。CKF所有容积点的权值都相同且都为正，因此在计算的过程中不会出现协方差非正定的情况。在组合导航系统中，当建立的数学模型与实际系统模型不匹配或者系统发生突变时，会导致滤波出现发散的现象。针对上述情况，周华东等人基于正交性的原理提出了强跟踪算法。将强跟踪算法引入到卡尔曼滤波器中，在线实时调整增益矩阵，保证滤波过程中有效信息全部提取出来，让滤波器实时跟踪系统的状态，具有较强的鲁棒性。但传统的强跟踪算法中，渐消因子参数的选择通常是根据经验取值；且在系统正常情况下，强跟踪存在一定的错判概率。以上两点影响滤波了的精确性。
技术实现思路
本专利技术的目的在于提供一种能提高滤波精度，提高滤波器对系统状态发生突变时的跟踪能力，增强鲁棒性的改进的强跟踪容积卡尔曼滤波组合导航方法。本专利技术的目的是这样实现的：(1)收集实测SINS系统和GPS系统输出的数据；(2)选择状态量和观测量，建立SINS/GPS组合导航系统状态空间模型；(3)SINS/GPS组合导航系统初始化；(4)在k时刻进行标准CKF的时间更新和量测更新，得到一步状态预测的误差协方差值和一步预测的互协方差值；(5)利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；(6)利用校正后的一步状态预测的误差协方差值进行CKF的量测更新，得到k+1时刻的状态估计值和和状态误差协方差，完成对系统的状态估计。本专利技术还可以包括：1、所述建立SINS/GPS组合导航系统状态空间模型具体包括：选择SINS/GPS组合导航系统状态变量：其中，为姿态角误差；δVx,δVy,δVz为速度误差；δx,δy,δz为位置误差；为陀螺常值漂移；为加速度计常值偏差，状态方程为：f(·)为非线性状态方程；G(t)为系统噪声驱动阵；W(t)表示系统噪声、为高斯白噪声、来源于陀螺仪和加速度计的噪声部分，W(t)表示为：W(t)＝[ωgxωgyωgzωaxωayωaz]T其中ωgx,ωgy,ωgz分别代表陀螺仪三个轴向上的白噪声，ωax,ωay,ωaz分别代表加速度计三个轴向上的白噪声；GPS系统能输出位置、速度信息；SINS/GPS组合导航系统的量测量由SINS输出的位置、速度与GPS输出的位置、速度信息相减的差值构成，位置方程：LI,λI,hI分别为SINS输出的经度、纬度、高度方向上的位置信息；LG,λG,hG分别为GPS输出的经度、纬度、高度方向上的位置信息；HP(t)为量测驱动阵，HP(t)＝[03×6,I3×3,03×6]；VP(t)为GPS接收机在导航坐标系下的位置误差噪声，速度方程：其中VIN,VIE,VIU分别为SINS东、北、天方向输出的速度；VGN,VGE,VGU分别为GPS接收机东、北、天方向输出的速度；HV(t)为量测驱动阵，HV(t)＝[03×3,diag{1,1,1},03×9]；VV(t)为GPS接收机在导航坐标系下的速度误差噪声，将SINS和GPS的位置和速度量测方程合并得到组合导航系统总的量测方程：2、所述引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法具体包括以下步骤：时间更新：(1)计算容积点其中，m＝2n，n为系统状态维数；协方差阵Sk是Pk的Cholesky分解；[1]表示n维单位向量e＝[1，0，…，0]T改变元素符号进行全排列产生的点集，[1]i表集合[1]的第i列；(2)计算经过状态方程传递后的容积点f(·)为系统非线性函数；(3)计算k+1时刻状态预测值和状态误差协方差阵其中，上标(l)表示没有加入渐消因子时的情况，Pxx，k+1|k为自协方差阵，Qk为系统噪声方差阵；(4)计算更新后的状态容积点(5)经过量测方程传递后的容积点(6)计算k+1时刻的量测预测值和一步预测互相关协方差阵(7)引入强跟踪算法，计算改进后的渐消因子λk+1，并计算一步预测状态误差方差阵Pk+1|kλk+1＝max(1,λ0)式中tr[·]表示对矩阵求迹；λk+1为渐消因子，当λk+1＝1时，STF就不起作用，转变为标准的容积卡尔曼滤波；Pxx,k+1|k为考虑渐消因子的系统噪声方差阵的预测方差阵；为不考虑渐消因子系统噪声方差阵的状态预测方差阵；为互协方差阵；ek为新息残差序列，zk+1为真实量测值，为估计量测值；β′为弱化因子；ρ为遗忘因子，取值为0.95≤ρ≤0.995；Rk+1为量测噪声方差阵；根据χ2分布表查知，观测维数m与β′的关系为：Pk+1|k＝λk+1Pxx,k+1|k+Qk(8)计算加入改进渐消因子λk+1后的更新状态容积点Pk+1|k＝Sk+1|k(Sk+1|k)T(9)计算经过量测方程传递后的容积点量测更新：(1)计算k+1时刻的量测预测值量测误差协方差阵Pzz,k+1|k和一步预测互相关协方差阵Pxz,k+1|k(2)计算k+1时刻的滤波增益矩阵Kk+1Kk+1＝Pxz,k+1|k(Pzz,k+1|k)-1(3)计算k+1时刻的状态估计值(4)计算k+1时刻的状态误差协方差阵Pk+1本专利技术提供了一种改进的强跟踪容积卡尔曼滤波组合导航方法，将STF引入到CKF中，利用概率论的知识改进渐消因子的计算方法来减小在正常情况下强跟踪误判概率。根据χ2分布，重新选择渐消因子的参数。将本专利技术ISTCKF运用到SINS/GPS组合导航系统中，不仅能提高系统的滤波精度，还能提高滤波器对系统状态发生突变时的跟踪能力，从而增本文档来自技高网...

【技术保护点】
1.一种改进的强跟踪容积卡尔曼滤波组合导航方法，其特征是：(1)收集实测SINS系统和GPS系统输出的数据；(2)选择状态量和观测量，建立SINS/GPS组合导航系统状态空间模型；(3)SINS/GPS组合导航系统初始化；(4)在k时刻进行标准CKF的时间更新和量测更新，得到一步状态预测的误差协方差值和一步预测的互协方差值；(5)利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；(6)利用校正后的一步状态预测的误差协方差值进行CKF的量测更新，得到k+1时刻的状态估计值和和状态误差协方差，完成对系统的状态估计。

【技术特征摘要】
1.一种改进的强跟踪容积卡尔曼滤波组合导航方法，其特征是：(1)收集实测SINS系统和GPS系统输出的数据；(2)选择状态量和观测量，建立SINS/GPS组合导航系统状态空间模型；(3)SINS/GPS组合导航系统初始化；(4)在k时刻进行标准CKF的时间更新和量测更新，得到一步状态预测的误差协方差值和一步预测的互协方差值；(5)利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；(6)利用校正后的一步状态预测的误差协方差值进行CKF的量测更新，得到k+1时刻的状态估计值和和状态误差协方差，完成对系统的状态估计。2.根据权利要求1所述的改进的强跟踪容积卡尔曼滤波组合导航方法，其特征是所述建立SINS/GPS组合导航系统状态空间模型具体包括：选择SINS/GPS组合导航系统状态变量：其中，为姿态角误差；δVx,δVy,δVz为速度误差；δx,δy,δz为位置误差；为陀螺常值漂移；为加速度计常值偏差，状态方程为：f(·)为非线性状态方程；G(t)为系统噪声驱动阵；W(t)表示系统噪声、为高斯白噪声、来源于陀螺仪和加速度计的噪声部分，W(t)表示为：W(t)＝[ωgxωgyωgzωaxωayωaz]T其中ωgx,ωgy,ωgz分别代表陀螺仪三个轴向上的白噪声，ωax,ωay,ωaz分别代表加速度计三个轴向上的白噪声；GPS系统能输出位置、速度信息；SINS/GPS组合导航系统的量测量由SINS输出的位置、速度与GPS输出的位置、速度信息相减的差值构成，位置方程：LI,λI,hI分别为SINS输出的经度、纬度、高度方向上的位置信息；LG,λG,hG分别为GPS输出的经度、纬度、高度方向上的位置信息；HP(t)为量测驱动阵，HP(t)＝[03×6,I3×3,03×6]；VP(t)为GPS接收机在导航坐标系下的位置误差噪声，速度方程：其中VIN,VIE,VIU分别为SINS东、北、天方向输出的速度；VGN,VGE,VGU分别为GPS接收机东、北、天方向输出的速度；HV(t)为量测驱动阵，HV(t)＝[03×3,diag{1,1,1},03×9]；...

【专利技术属性】
技术研发人员：黄平，孙婷婷，张灏楠，胡心达，曹雨佳，孙延伟，贾通，黄俊杰，吴闻起，杨光，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23