CN109919233B - 一种基于数据融合的跟踪滤波方法 - Google Patents

Abstract

本发明提供了一种基于数据融合的跟踪滤波方法，首先计算各个Sigma采样点和相应的权值，每个Sigma采样点通过非线性函数传播，得到观测向量的均值估计和协方差估计，将非线性系统分解为具有多个子系统的非线性滤波器，基于不敏卡尔曼滤波方法的局部滤波进行状态估计，计算各局部滤波器的权值后，得到基于数据融合算法的卡尔曼滤波器状态估计。本发明得到的滤波估计值不仅可以使滤波算法的估计精度有所提高，还增加了滤波系统的可靠性。

Description

一种基于数据融合的跟踪滤波方法

技术领域

本发明属于定向定位领域，涉及一种跟踪滤波方法。

背景技术

卡尔曼滤波方法是被动跟踪中一种非常重要的手段，已广泛应用于军事和民用领域。传统的卡尔曼滤波方法滤波过程中容易产生误差积累，并且在状态初值和初始协方差误差较大时，很容易导致滤波发散。

不敏卡尔曼滤波方法不需要对非线性系统进行线性化，并可以很容易应用于非线性系统的状态估计。但是，不敏卡尔曼滤波方法存在一些缺点，比如容易受初始状态误差和可观测性等方面的影响，这时因为不敏卡尔曼滤波方法是用高斯分布来逼近系统状态的后验概率分布，在系统状态的后验概率分布为非高斯情况下，滤波结果将有极大的误差。

发明内容

为了克服现有技术的不足，本发明提供一种基于数据融合的跟踪方法，在跟踪滤波系统受到初始状态误差和可观测性影响情况下，确保跟踪滤波性能不下降。

本发明解决其技术问题所采用的技术方案包括以下步骤：

a)计算2N+1个Sigma采样点χ_i和相应的权值w_i，

其中，参数λ＝α²(N+η)-N，N表示状态向量x的维度，

表示x的均值，P_x表示x的协方差，λ为设定的比例因子，η取0，α表示采样点对均值的离散程度，β表示状态向量的先验信息，

和

分别表示第一个采样点的均值和协方差的权值，

和

分别表示第i个采样点χ_i的均值和协方差的权值；

b)每个Sigma采样点通过非线性函数传播，得到观测向量z＝h(χ_i)，其中，h(χ_i)表示非线性状态方程；

z的均值估计和协方差估计分别为

和

假设有多个传感器，基于数据融合的卡尔曼滤波方法状态方程和观测方程分别为x(l+1)＝f(x(l+1),v(m))和z_j(l)＝h(x(l),n(l))，其中j＝1,2,…,L，L表示传感器的个数，状态向量为x(l)，第j个传感器的观测量为z_j(l)，v(m)和n(l)分别是过程噪声和观测噪声，且均为均值0的高斯白噪声，其协方差矩阵为E[v(l)v^H(l)]＝Q(l),E[n(l)n^H(l)]＝R(l)；

c)假设非线性系统有多个传感器组成，将其分解为具有多个子系统的非线性滤波器，则第j个传感器的增益矩阵

其中，

表示状态值与观测值的互协方差矩阵的一步预测，

表示观测值自相关矩阵的一步预测，

状态估计误差自相关矩阵

d)各局部滤波器的权值

其中tr(P_j(l+1|l+1))表示矩阵P_j(l+1|l+1)的迹；

e)基于数据融合算法的卡尔曼滤波器状态估计为

本发明的有益效果是：得到的滤波估计值不仅可以使滤波算法的估计精度有所提高，还增加了滤波系统的可靠性。

附图说明

图1是基于数据融合的跟踪滤波方法流程图；

图2是滤波方法框图；

图3是在传感器数目为2，采样周期为1s，两个传感器的量测噪声方差分别是

时位置均方根误差图；

图4是在传感器数目为2，采样周期为1s，两个传感器的量测噪声方差分别是

时X方向位置均方根误差图；

图5是在传感器数目为2，采样周期为1s，两个传感器的量测噪声方差分别是

时Y方向位置均方根误差图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明包括以下步骤：

a)首先计算(2N+1)个Sigma采样点χ_i和相应的权值w_i：

其中，参数λ＝α²(N+η)-N，N表示状态向量x的维度，

表示x的均值，P_x表示x的协方差，λ为设定的比例因子，η一般取0，α表示采样点对均值的离散程度，取较小的值，如0.001，β表示状态向量的先验信息，β的最佳值是2，

和

分别表示第一个采样点的均值和协防差的权值，

和

分别表示第i个采样点χ_i的均值和协方差的权值。

b)每个Sigma采样点通过非线性函数传播，得到观测向量

z＝h(χ_i) (3)

其中，h(χ_i)表示非线性状态方程；

z的均值估计和协方差估计为：

假设有多个传感器，基于数据融合的卡尔曼滤波方法状态方程和观测方程如下：

x(l+1)＝f(x(l+1),v(m)) (6)

z_j(l)＝h(x(l),n(l)) (7)

其中j＝1,2,…,L，L表示传感器的个数，状态向量为x(l)，第j个传感器的观测量为z_j(l)，v(m)和n(l)分别是过程噪声和观测噪声，且均为均值0的高斯白噪声，其协方差矩阵为E[v(l)v^H(l)]＝Q(l),E[n(l)n^H(l)]＝R(l)。

c)假设非线性系统有多个传感器组成，可以将其分解为具有多个子系统的非线性滤波器，那么对于第j个传感器来说，基于不敏卡尔曼滤波方法的局部滤波的状态估计包括：

增益矩阵为：

其中，

表示状态值与观测值的互协方差矩阵的一步预测，

表示观测值自相关矩阵的一步预测，

状态估计误差自相关矩阵为：

d)各局部滤波器的权值为：

其中tr(P_j(l+1|l+1))表示矩阵P_j(l+1|l+1)的迹。

e)基于数据融合算法的卡尔曼滤波器状态估计为：

本发明提供一种基于数据融合的跟踪滤波方法，图1为该方法流程图，图2是滤波方法框图，本实例中假设目标做匀速直线运动，观测平台做匀加速直线运动，目标与观测平台在同一水平高度Z＝1000m的平面运动，执行以下步骤：

步骤一：由图2建立跟踪滤波模型，观测平台从第4秒到第80秒对目标进行跟踪，传感器数目为2，采样周期T＝1s，待估计目标的初始位置X_T＝[-3000,2500]^T，单位为米，速度V_T＝[30,0]^T，单位为米/秒，观测平台的初始位置是X⁽¹⁾＝[-2000,1000]^T，速度为V⁽¹⁾＝[0,35]^T，加速度a＝[0,3]^T，过程噪声的自相关矩阵是Q＝2.4I_6×6，两个传感器的量测噪声方差分别是σ₁ ²＝0.0007,σ₂ ²＝0.0004；计算(2N+1)个Sigma采样点χ_i和相应的权值w_i；

步骤二：计算z的均值估计和协方差估计：

根据各局部滤波器的数据计算基于数据融合的卡尔曼滤波方法状态方程和观测方程

x(l+1)＝f(x(l+1),v(m))

z_j(l)＝h(x(l),n(l))

步骤三：计算系统增益矩阵：

状态估计误差自相关矩阵

步骤四：计算各局部滤波器的权值为：

步骤五：计算基于数据融合算法的卡尔曼滤波器状态估计：

图3是待估计目标的初始位置X_T＝[-3000,2500]^T，速度V_T＝[30,0]^T，观测平台的初始位置是X⁽¹⁾＝[-2000,1000]^T，速度为V⁽¹⁾＝[0,35]^T，加速度a＝[0,3]^T时，各局部滤波器和基于数据融合的滤波器位置的均方根误差。从图中可以看出，基于数据融合的不敏卡尔曼滤波器要比各个单独的不敏卡尔曼滤波器的滤波性能更好，在30秒之前两种滤波算法对位置估计的均方根误差波动比较大，其原因是在滤波初期样本数据较少导致滤波不稳定，随着时间的推移，位置估计的均方根误差慢慢收敛并趋于稳定。

图4是待估计目标的初始位置X_T＝[-3000,2500]^T，速度V_T＝[30,0]^T，观测平台的初始位置是X⁽¹⁾＝[-2000,1000]^T，速度为V⁽¹⁾＝[0,35]^T，加速度a＝[0,3]^T时，各局部滤波器和基于数据融合的滤波器位置的均方根误差。从图中可以看出，在X方向上基于数据融合的不敏卡尔曼滤波器要比各个单独的不敏卡尔曼滤波器的滤波性能更好。

图5是待估计目标的初始位置X_T＝[-3000,2500]^T，速度V_T＝[30,0]^T，观测平台的初始位置是X⁽¹⁾＝[-2000,1000]^T，速度为V⁽¹⁾＝[0,35]^T，加速度a＝[0,3]^T时，各局部滤波器和基于数据融合的滤波器位置的均方根误差。从图中可以看出，在Y方向上基于数据融合的不敏卡尔曼滤波器要比各个单独的不敏卡尔曼滤波器的滤波性能更好。

Claims (1)

1.一种基于数据融合的跟踪滤波方法，其特征在于包括下述步骤：

a)计算2N+1个Sigma采样点χ_i和相应的权值w_i，

其中，参数λ＝α²(N+η)-N，N表示状态向量x的维度，

表示x的均值，P_x表示x的协方差，λ为设定的比例因子，η取0，α表示采样点对均值的离散程度，β表示状态向量的先验信息，

和

分别表示第一个采样点的均值和协方差的权值，

和

分别表示第i个采样点χ_i的均值和协方差的权值；

b)每个Sigma采样点通过非线性函数传播，得到观测向量z＝h(χ_i)，其中，h(χ_i)表示非线性状态方程；

z的均值估计和协方差估计分别为

和

假设有多个传感器，基于数据融合的卡尔曼滤波方法状态方程和观测方程分别为x(l+1)＝f(x(l+1),v(m))和z_j(l)＝h(x(l),n(l))，其中j＝1,2,…,L，L表示传感器的个数，状态向量为x(l)，第j个传感器的观测量为z_j(l)，v(m)和n(l)分别是过程噪声和观测噪声，且均为均值0的高斯白噪声，其协方差矩阵为E[v(l)v^H(l)]＝Q(l),E[n(l)n^H(l)]＝R(l)；

c)假设非线性系统有多个传感器组成，将其分解为具有多个子系统的非线性滤波器，则第j个传感器的增益矩阵

其中，

表示状态值与观测值的互协方差矩阵的一步预测，

表示观测值自相关矩阵的一步预测，

状态估计误差自相关矩阵

d)各局部滤波器的权值

其中tr(P_j(l+1|l+1))表示矩阵P_j(l+1|l+1)的迹；

e)基于数据融合算法的卡尔曼滤波器状态估计为

Images