CN109000642A - 一种改进的强跟踪容积卡尔曼滤波组合导航方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及的是一种组合导航方法。

背景技术

单独的导航系统很难满足人类对于精度高、稳定性强的导航系统的需求，而将两种或两种以上的导航系统通过某种方法组合起来构成组合导航系统能使导航的精度得到很大的提升。目前，应用最为广泛的组合导航系统是捷联惯性导航系统(StrapdownInertial Navigation System,SINS)和全球定位系统(Global Position System，GPS)，SINS可以弥补GPS抗干扰性差、动态性能不足的缺点；GPS可以弥补SINS误差随时间积累的缺点。可以很好地展现SINS的自主性、抗干扰能力强和GPS全天候、高精度等优点，所以SINS/GPS组合导航系统的应用前景非常可观。

在实际应用中大多数的组合导航系统都是非线性的，而非线性滤波方法主要有扩展卡尔曼滤波(Extended Kalman Filter，EKE)、无迹卡尔曼滤波(Uncented KalmanFilter，UKF)、容积卡尔曼滤波(Cubature Kalman Filter，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组合导航系统状态变量：

其中，为姿态角误差；δV_x,δV_y,δV_z为速度误差；δ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输出的位置、速度信息相减的差值构成，

位置方程：

L_I,λ_I,h_I分别为SINS输出的经度、纬度、高度方向上的位置信息；L_G,λ_G,h_G分别为GPS输出的经度、纬度、高度方向上的位置信息；H_P(t)为量测驱动阵，H_P(t)＝[0_3×6,I_3×3,0_3×6]；V_P(t)为GPS接收机在导航坐标系下的位置误差噪声，

速度方程：

其中V_IN,V_IE,V_IU分别为SINS东、北、天方向输出的速度；V_GN,V_GE,V_GU分别为GPS接收机东、北、天方向输出的速度；H_V(t)为量测驱动阵，H_V(t)＝[0_3×3,diag{1,1,1},0_3×9]；V_V(t)为GPS接收机在导航坐标系下的速度误差噪声，

将SINS和GPS的位置和速度量测方程合并得到组合导航系统总的量测方程：

2、所述引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法具体包括以下步骤：

时间更新：

(1)计算容积点

其中，m＝2n，n为系统状态维数；协方差阵S_k是P_k的Cholesky分解；[1]表示n维单位向量e＝[1，0，…，0]^T改变元素符号进行全排列产生的点集，[1]_i表集合[1]的第i列；

(2)计算经过状态方程传递后的容积点

f(·)为系统非线性函数；

(3)计算k+1时刻状态预测值和状态误差协方差阵

其中，上标(l)表示没有加入渐消因子时的情况，P_xx，k+1|k为自协方差阵，Q_k为系统噪声方差阵；

(4)计算更新后的状态容积点

(5)经过量测方程传递后的容积点

(6)计算k+1时刻的量测预测值和一步预测互相关协方差阵

(7)引入强跟踪算法，计算改进后的渐消因子λ_k+1，并计算一步预测状态误差方差阵P_k+1|k

λ_k+1＝max(1,λ₀)

式中tr[·]表示对矩阵求迹；λ_k+1为渐消因子，当λ_k+1＝1时，STF就不起作用，转变为标准的容积卡尔曼滤波；P_xx,k+1|k为考虑渐消因子的系统噪声方差阵的预测方差阵；为不考虑渐消因子系统噪声方差阵的状态预测方差阵；为互协方差阵；e_k为新息残差序列，z_k+1为真实量测值，为估计量测值；β′为弱化因子；ρ为遗忘因子，取值为0.95≤ρ≤0.995；R_k+1为量测噪声方差阵；

根据χ²分布表查知，观测维数m与β′的关系为：

P_k+1|k＝λ_k+1P_xx,k+1|k+Q_k

(8)计算加入改进渐消因子λ_k+1后的更新状态容积点

P_k+1|k＝S_k+1|k(S_k+1|k)^T

(9)计算经过量测方程传递后的容积点

量测更新：

(1)计算k+1时刻的量测预测值量测误差协方差阵P_zz,k+1|k和一步预测互相关协方差阵P_xz,k+1|k

(2)计算k+1时刻的滤波增益矩阵K_k+1

K_k+1＝P_xz,k+1|k(P_zz,k+1|k)^-1

(3)计算k+1时刻的状态估计值

(4)计算k+1时刻的状态误差协方差阵P_k+1

本发明提供了一种改进的强跟踪容积卡尔曼滤波组合导航方法，将STF引入到CKF中，利用概率论的知识改进渐消因子的计算方法来减小在正常情况下强跟踪误判概率。根据χ²分布，重新选择渐消因子的参数。将本发明ISTCKF运用到SINS/GPS组合导航系统中，不仅能提高系统的滤波精度，还能提高滤波器对系统状态发生突变时的跟踪能力，从而增强SINS/GPS组合导航系统的鲁棒性。

本发明与现有的技术方法相比存在以下优点：

(1)本发明将改进后的强跟踪滤波与容积卡尔曼滤波相结合，克服了在SINS/GPS系统中因模型不准确或者系统状态发生突变时，CKF滤波出现发散的情况。提高滤波的精度。

(2)本发明ISTCKF滤波方法较于传统的强跟踪滤波方法，大大降低了在系统正常情况下强跟踪误判的概率，并且给出渐消因子固定的参数取值，而非仅凭经验选取。

(3)本发明ISTCKF应用在SINS/GPS组合导航系统中，能够增强系统对状态突变的跟踪能力，增强SINS/GPS组合导航系统的鲁棒性。

附图说明

图1为本发明的改进的强跟踪容积卡尔曼滤波组合导航方法原理图；

图2为本发明CKF与EKF、UKF三种滤波方法方差的对比；

图3为本发明CKF与EKF、UKF三种滤波方法滤波误差的对比；

图4(a)为采用CKF滤波方法姿态角误差，图4(b)为采用CKF滤波方法速度误差，图4(c)为采用CKF滤波方法位置误差，图4(d)为滤波前姿态与滤波后姿态的对比；

图5(a)为本发明ISTCKF滤波方法姿态角误差，图5(b)为采用本发明ISTCKF滤波方法速度误差，图5(c)为采用本发明ISTCKF滤波方法位置误差，图5(d)为滤波前姿态与滤波后姿态的对比。

具体实施方式

下面举例对本发明做更详细的描述。

结合图1，本发明的改进的强跟踪容积卡尔曼滤波组合导航方法包括以下几个步骤：

(1)收集实测SINS系统和GPS系统输出的数据；

(2)选择状态量和观测量，建立SINS/GPS组合导航系统状态空间模型；

(3)SINS/GPS组合导航系统初始化；

(4)在k时刻进行标准CKF的时间更新和量测更新，得到一步状态预测的误差协方差值和一步预测的互协方差值；

(5)利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；

(6)利用校正后的一步状态预测的误差协方差值进行CKF的量测更新，得到k+1时刻的状态估计值和和状态误差协方差，完成对系统的状态估计。

本发明一种改进的强跟踪容积卡尔曼滤波组合导航方法还包括：

所述建立SINS/GPS组合导航系统模型具体包括：

选择SINS/GPS组合导航系统状态变量：

其中，为姿态角误差；δV_x,δV_y,δV_z为速度误差；δ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输出的位置、速度信息相减的差值构成。

位置方程：

L_I,λ_I,h_I分别为SINS输出的经度、纬度、高度方向上的位置信息；L_G,λ_G,h_G分别为GPS输出的经度、纬度、高度方向上的位置信息；H_P(t)为量测驱动阵，H_P(t)＝[0_3×6,I_3×3,0_3×6]；V_P(t)为GPS接收机在导航坐标系下的位置误差噪声。

速度方程：

其中V_IN,V_IE,V_IU分别为SINS东、北、天方向输出的速度；V_GN,V_GE,V_GU分别为GPS接收机东、北、天方向输出的速度；H_V(t)为量测驱动阵，H_V(t)＝[0_3×3,diag{1,1,1},0_3×9]；V_V(t)为GPS接收机在导航坐标系下的速度误差噪声。

将SINS和GPS的位置和速度量测方程合并便可得到组合导航系统总的量测方程：

所述的利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法(ImprovementStrong Tracking Cubature Kalman Filter，ISTCKF)具体步骤包括：

时间更新：

(1)计算容积点

式中，m＝2n，n为系统状态维数；协方差阵S_k是P_k的Cholesky分解；[1]表示n维单位向量e＝[1，0，…，0]^T改变元素符号进行全排列产生的点集，[1]_i表集合[1]的第i列。

(2)计算经过状态方程传递后的容积点

f(·)为系统非线性函数。

(3)计算k+1时刻状态预测值和状态误差协方差阵

式中，上标(l)表示没有加入渐消因子时的情况。P_xx，k+1|k为自协方差阵，Q_k为系统噪声方差阵。

(4)计算更新后的状态容积点

(5)经过量测方程传递后的容积点

(6)计算k+1时刻的量测预测值和一步预测互相关协方差阵

(7)引入强跟踪算法，计算改进后的渐消因子λ_k+1，并计算一步预测状态误差方差阵P_k+1|k

λ_k+1＝max(1,λ₀)

式中tr[·]表示对矩阵求迹；λ_k+1为渐消因子，当λ_k+1＝1时，STF就不起作用，转变为标准的容积卡尔曼滤波；P_xx,k+1|k为考虑渐消因子的系统噪声方差阵的预测方差阵；为不考虑渐消因子系统噪声方差阵的状态预测方差阵；为互协方差阵；e_k为新息残差序列，z_k+1为真实量测值，为估计量测值；β′为弱化因子，可以使滤波效果更加平滑；ρ为遗忘因子，一般取值为0.95≤ρ≤0.995；R_k+1为量测噪声方差阵。

根据χ²分布表查知，观测维数m与β′的关系为：

P_k+1|k＝λ_k+1P_xx,k+1|k+Q_k

(8)计算加入改进渐消因子λ_k+1后的更新状态容积点

P_k+1|k＝S_k+1|k(S_k+1|k)^T

(9)计算经过量测方程传递后的容积点

量测更新：

(1)计算k+1时刻的量测预测值量测误差协方差阵P_zz,k+1|k和一步预测互相关协方差阵P_xz,k+1|k

(2)计算k+1时刻的滤波增益矩阵K_k+1

K_k+1＝P_xz,k+1|k(P_zz,k+1|k)^-1

(3)计算k+1时刻的状态估计值

(4)计算k+1时刻的状态误差协方差阵P_k+1

本发明采用的是CKF滤波算法，针对SINS/GPS系统模型不准确或者系统状态发生突变的情况，利用强跟踪算法，对CKF滤波算法进行改进以增强系统的鲁棒性。并对STCKF滤波算法进行改进，以提升滤波的精度。所以本发明利用MATLAB仿真软件进行仿真实验，将CKF滤波算法与现有的非线性滤波算法EKF和UKF进行比较。

本发明实施例用来解释本发明，而不是对本发明进行限制，在发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。

下面结合具体事例进行仿真分析：

假设物体M在一个二维平面上运动，水平方向(x轴)上做匀加速直线运动，垂直方向(y轴)上也做匀加速直线运动。两个方向上的运动都具有系统噪声W_k。假设坐标位置为(x₀,y₀)的雷达对目标M进行跟踪，则可以得到雷达与M之间的距离r_k和目标M相对于雷达的角度雷达的噪声为V(k)。系统噪声与雷达噪声互不相关，观测次数为50次，采样时间为T＝50s。

系统噪声W(k)方差阵Q_k为：

雷达噪声V(k)方差阵R_k为：

选择某一时刻的位置、速度和加速的为状态变量

运动状态方程为：

X(k+1)＝ΦX(k)+W(k)

式中状态转移矩阵Φ为：

观测方程为：

图2、图3为基于以上系统模型，CKF与EKF、UKF滤波效果对比图；其中图2为EKF、UKF、CKF三种滤波方法方差的对比；图3为EKF、UKF、CKF三种滤波方法滤波误差的对比；以上两幅图表明，EKF滤波效果最差，而CKF滤波效果最好，方差和误差都较小。说明本发明对CKF滤波进行改进是正确的。

图4和图5为基于SINS/GPS组合导航系统采集的实测数据进行的仿真。

仿真所使用的轨迹是利用MATLAB程序产生的机载运动：设置飞机的初始位置经度为126.63°，纬度为45.75°。导航坐标系为当地地理坐标系设SINS系统导航信息的输出频率为100Hz，GPS的输出频率为10Hz，组合导航系统的数据输出频率与SINS系统同步。此外，地球模型取为椭球体，相应的长半轴为R_e＝6378140m，短半轴为R_p＝6356756m，椭圆度为e＝1/298.27；地球自转角速率取为ω_ie＝7.2921158×10^-5rad/s。陀螺仪、加速度计以及GPS的参数设置如下表所示。

表1陀螺仪、加速度计以及GPS的参数

针对以上模型参数的设置，对SINS/GPS组合导航系统进行仿真，仿真时间为1312s。在890s时，系统状态发生突变，分别用CKF、ISTCKF进行MTLAB仿真，仿真结果如图4、图5、所示。图4(a)为采用CKF滤波方法姿态角误差，图4(b)为采用CKF滤波方法速度误差，图4(c)为采用CKF滤波方法位置误差，图4(d)为滤波前姿态与滤波后姿态的对比。图5表示采用本发明的ISTCKF滤波方法进行的仿真。图5(a)为本发明ISTCKF滤波方法姿态角误差，图5(b)为采用本发明ISTCKF滤波方法速度误差，图5(c)为采用本发明ISTCKF滤波方法位置误差，图5(d)为滤波前姿态与滤波后姿态的对比。显然，无论是从姿态、速度还是位置的误差图都可以看出，本发明方法在系统发生突变时，能够及时跟踪系统状态的变化，提高系统的滤波精度。

综上，本发明一种改进的强跟踪容积卡尔曼滤波组合导航方法，导航精度高，且系统鲁棒性强，方法简单，易于实现。

Claims (3)

1.一种改进的强跟踪容积卡尔曼滤波组合导航方法，其特征是：

(1)收集实测SINS系统和GPS系统输出的数据；

(2)选择状态量和观测量，建立SINS/GPS组合导航系统状态空间模型；

(3)SINS/GPS组合导航系统初始化；

(4)在k时刻进行标准CKF的时间更新和量测更新，得到一步状态预测的误差协方差值和一步预测的互协方差值；

(5)利用引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；

(6)利用校正后的一步状态预测的误差协方差值进行CKF的量测更新，得到k+1时刻的状态估计值和和状态误差协方差，完成对系统的状态估计。

2.根据权利要求1所述的改进的强跟踪容积卡尔曼滤波组合导航方法，其特征是所述建立SINS/GPS组合导航系统状态空间模型具体包括：

选择SINS/GPS组合导航系统状态变量：

其中，为姿态角误差；δV_x,δV_y,δV_z为速度误差；δ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输出的位置、速度信息相减的差值构成，

位置方程：

L_I,λ_I,h_I分别为SINS输出的经度、纬度、高度方向上的位置信息；L_G,λ_G,h_G分别为GPS输出的经度、纬度、高度方向上的位置信息；H_P(t)为量测驱动阵，H_P(t)＝[0_3×6,I_3×3,0_3×6]；V_P(t)为GPS接收机在导航坐标系下的位置误差噪声，

速度方程：

其中V_IN,V_IE,V_IU分别为SINS东、北、天方向输出的速度；V_GN,V_GE,V_GU分别为GPS接收机东、北、天方向输出的速度；H_V(t)为量测驱动阵，H_V(t)＝[0_3×3,diag{1,1,1},0_3×9]；V_V(t)为GPS接收机在导航坐标系下的速度误差噪声，

将SINS和GPS的位置和速度量测方程合并得到组合导航系统总的量测方程：

3.根据权利要求1或2所述的改进的强跟踪容积卡尔曼滤波组合导航方法，其特征是所述引入强跟踪算法的改进的强跟踪容积卡尔曼滤波算法具体包括以下步骤：

时间更新：

(1)计算容积点

其中，m＝2n，n为系统状态维数；协方差阵S_k是P_k的Cholesky分解；[1]表示n维单位向量e＝[1，0，…，0]^T改变元素符号进行全排列产生的点集，[1]_i表集合[1]的第i列；

(2)计算经过状态方程传递后的容积点

f(·)为系统非线性函数；

(3)计算k+1时刻状态预测值和状态误差协方差阵

其中，上标(l)表示没有加入渐消因子时的情况，P_xx，k+1|k为自协方差阵，Q_k为系统噪声方差阵；

(4)计算更新后的状态容积点

(5)经过量测方程传递后的容积点

(6)计算k+1时刻的量测预测值和一步预测互相关协方差阵

(7)引入强跟踪算法，计算改进后的渐消因子λ_k+1，并计算一步预测状态误差方差阵P_k+1|k

λ_k+1＝max(1,λ₀)

式中tr[·]表示对矩阵求迹；λ_k+1为渐消因子，当λ_k+1＝1时，STF就不起作用，转变为标准的容积卡尔曼滤波；P_xx,k+1|k为考虑渐消因子的系统噪声方差阵的预测方差阵；为不考虑渐消因子系统噪声方差阵的状态预测方差阵；为互协方差阵；e_k为新息残差序列，z_k+1为真实量测值，为估计量测值；β′为弱化因子；ρ为遗忘因子，取值为0.95≤ρ≤0.995；R_k+1为量测噪声方差阵；

根据χ²分布表查知，观测维数m与β′的关系为：

P_k+1|k＝λ_k+1P_xx,k+1|k+Q_k

(8)计算加入改进渐消因子λ_k+1后的更新状态容积点

P_k+1|k＝S_k+1|k(S_k+1|k)^T

(9)计算经过量测方程传递后的容积点

量测更新：

(1)计算k+1时刻的量测预测值量测误差协方差阵P_zz,k+1|k和一步预测互相关协方差阵P_xz,k+1|k

(2)计算k+1时刻的滤波增益矩阵K_k+1

K_k+1＝P_xz,k+1|k(P_zz,k+1|k)^-1

(3)计算k+1时刻的状态估计值

(4)计算k+1时刻的状态误差协方差阵P_k+1