一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法与流程

本发明涉及一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法，属于目标跟踪领域。

背景技术：

目标跟踪中常使用卡尔曼滤波对目标状态进行估计，通过卡尔曼滤波可以将传感器数据中的量测噪声消除，以达到对目标精准跟踪的效果。然而传统的卡尔曼滤波是建立在系统模型准确并且是线性的、过程噪声和量测噪声服从均值为零、方差为固定值的高斯分布且其统计特性已知的情况下。然而，在实际目标跟踪中，大多数系统模型是非线性的，系统噪声的统计特性也是不完全已知甚至是未知的，这时再用卡尔曼滤波对其进行估计将造成估计结果不准确甚至发散。

为了解决噪声统计特性不准确的问题，提出了sage-husa自适应噪声估值器对噪声进行实时估计。然而sage-husa自适应滤波只能在对单一过程噪声不准确或者量测噪声不准确时有较好的估计效果，当同时存在过程噪声与量测噪声统计特性不准确时，sage-husa自适应滤波算法将不再适用。因此，对于过程噪声与量测噪声统计特性同时不准确的情况，本发明将采用改进的自适应滤波算法，用sage-husa自适应噪声估值器对过程噪声统计特性估计，同时采用变分贝叶斯对量测噪声进行迭代估计，此方法可同时估计出过程噪声和量测噪声统计特性。

由于实际工程环境日益复杂，传感器工作时常会受到扰动的影响，导致测量数据常出现一些明显偏离实际值的异常数据，也称野值，在本发明中将这种情况称作故障。故障的出现将会影响自适应滤波对量测噪声统计特性的估计，进而造成滤波精度降低。因此，本发明采用卡方检验的方法对故障进行检测，如若发生故障则对其进行处理，降低故障对滤波的影响，以此来提高系统的容错性。然而常规新息卡方检验法对硬故障检测效果好，当传感器出现缓变故障时，由于缓变故障开始很小，不易检测出来，有故障的传感器的输出将污染滤波器输出，使其跟踪故障输出，所以本发明采用一种能检测缓变故障的故障检测方法，提高了故障检测的灵敏度，大大提高了滤波精度。

技术实现要素：

为了应对上面提到的过程噪声和量测噪声统计特性不准确以及量测存在故障等情况，本发明基于sage-husa自适应估计和变分贝叶斯估计提出一种新的自适应滤波方法，通过卡方检验对故障进行检测，并对其进行处理，得到自适应容错容积卡尔曼滤波方法。

本发明大体包括三部分内容。第一部分根据实际运动目标进行系统建模；第二部分，根据改进自适应滤波方法对噪声统计特性进行估计；第三部分，根据卡方检验对故障进行检测，并对其处理。

利用本发明不仅可以应对噪声统计特性不准确的情况，而且能够处理故障，提高滤波精度，包括以下步骤：

步骤1.系统建模，考虑离散时间非线性系统模型，其跟踪目标的状态方程和量测方程如下：

式中：xk+1∈rⁿ是k+1时刻的系统状态向量，其是由x方向位移和速度以及y方向位移和速度构成，f和h为已知函数，zk+1∈r^m是k+1时刻系统的量测向量，分别由k+1时刻传感器测得目标运动距离和角度构成，其中σ为随机向量，用来描述异常值大小，当ρk+1＝0时表示没有出现异常量测，当ρk+1＝1时，表示出现异常量测，wk是均值为0方差为qk的过程噪声向量，vk+1是均值为0方差为rk+1的量测噪声向量，且wk和vk+1互不相关，即：

式中，δkj为kronecher-δ函数；

步骤2给出自适应容错容积卡尔曼滤波方波，本算法使用sage-husa滤波方法对过程噪声估计，变分贝叶斯方法对未知量测噪声估计，容积卡尔曼滤波处理非线性问题，并采用卡方检验法对异常量测进行检测，以下简称故障诊断，并进行容错处理，具体如下：

步骤2.1给出初始化状态量和估计误差方差矩阵：

式中：为状态估计的初始值，x0为初始状态向量，q0,r0分别为过程噪声矩阵和量测噪声矩阵的初始值，为量测噪声协方差阵，αi,0,βi,0为逆伽马分布参数，i＝1,2,…,d，d是量测向量的维数；

步骤2.2给出由k时刻的状态对k+1时刻的状态进行预测：

式中pk为估计误差方差矩阵，sk为cholesky分解值，χi,k为第i个cubature点，为经过状态方程传播的第i个cubature点，为状态预测均值，为系统过程噪声均值，pk+1|k为预测误差协方差矩阵，0＜ρ＜1为衰减系数，用来描述当前时刻量测噪声因子与前一刻噪声因子的关系，ξi是第i个cubature点，l＝2n，n是状态向量的维数，而[1]∈rⁿ，[1]i表示第i个列向量，且

步骤2.3给出上步中过程噪声均值和方差的计算，根据sage-husa自适应估计对其进行计算，其公式如下：

式中为遗忘因子，为量测残差；

步骤2.4给出量测预测：

式中sk+1|k为由预测误差协方差矩阵pk+1|k经过cholesky分解得到，和都为k+1时刻第i个cubature点，为k+1时刻的量测预测值，pxz,k+1|k为互协方差矩阵。

步骤2.5给出故障诊断步骤为：

式中：rk+1|k为预测量测噪声方差矩阵，pzz,k+1|k为预测新息协方差矩阵，λk+1为故障检测函数，td为预先设定的门限值，可由预先设定的误警率确定，f(k+1)为加权因子。

若没有发生故障，则过程噪声均值和方差由saga-husa估计得出，若发生故障，则将公式(9)的过程噪声均值和公式(10)的过程噪声方差矩阵分别替换为qk+1＝qkqk+1＝qk。

步骤2.6给出量测更新，对以下部分进行变分贝叶斯迭代：

设置迭代初始值：

式中：l为迭代次数。分别为迭代初始估计状态向量和估计误差方差矩阵。

式中：为k+1时刻第l次迭代新息协方差矩阵，为k+1时刻第l次迭代卡尔曼滤波增益，为k+1时刻第l次迭代状态估计值，为k+1时刻第l次迭代估计误差协方差矩阵。

以下步骤为计算k+1时刻第l次迭代的量测噪声逆伽马分布参数

式中：都为k+1时刻第l次迭代的第i个容积点，为k+1时刻第l次迭代的量测估计值，中的i代表该向量的第i个元素，代表该矩阵中第i行i列上的元素。

步骤2.7迭代结束后对量测噪声逆伽马分布参数、量测噪声、状态估计值以及状态估计误差方差矩阵进行更新。

步骤3给出缓变故障检测方法，具体算法如下：

步骤3.1令和分别为系统在k-m时刻的伪状态估计值和伪估计误差方差阵，即和

步骤3.2在k-m+1时刻，根据公式(4)-(8)进行状态更新，得到状态预测值和预测误差方差矩阵

步骤3.3将和作为k-m+1时刻的伪状态估计值和伪估计误差协方差阵，继续进行时间更新，以此类推得到k时刻的状态预测值和预测误差协方差阵再由公式(12)-(15)和(18)得到量测预测值和新息协方差阵pzz,k|k-m。得到伪新息向量为：并构造出故障检测函数用于步骤2.5的故障检测。

本发明的有益效果：本发明既能同时对过程噪声协方差和量测噪声协方差动态估计，又能应对雷达所测数据发生故障时的情况，实现了对目标的有效跟踪。

附图说明：

图1：本发明方法的流程框图。

具体实施方法

本发明提出的一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法，首先根据实际目标跟踪的运动状态建立系统模型，其次给出自适应容错容积卡尔曼滤波方法的步骤，最后介绍缓变故障的检测算法，其流程框图如图1所示，包括以下几个步骤：

步骤1：系统建模，考虑离散时间非线性系统模型，其跟踪目标的状态方程和量测方程如下：

式中：xk+1∈rⁿ是k+1时刻的系统状态向量，其是由x方向位移和速度以及y方向位移和速度构成，f和h为已知函数，zk+1∈r^m是k+1时刻系统的量测向量，分别由k+1时刻传感器测得目标运动距离和角度构成，其中σ为随机向量，用来描述异常值大小，当ρk+1＝0时表示没有出现异常量测，当ρk+1＝1时，表示出现异常量测，wk是均值为0方差为qk的过程噪声向量，vk+1是均值为0方差为rk+1的量测噪声向量，且wk和vk+1互不相关，即：

式中，δkj为kronecher-δ函数.

步骤2：设置状态向量估计初始值和估计误差方差矩阵初始值分别为p0，过程噪声协方差矩阵和量测噪声协方差矩阵分别设置为q0,r0。且αi,0,βi,0为逆伽马分布参数，i＝1,2,…,d，d是量测向量的维数，设置遗忘因子b，故障检测阈值td。

步骤3：根据sage-husa自适应估计公式(10)和(11)计算出k时刻的过程噪声均值和过程噪声协方差并用于下步中k+1时刻的状态预测。

步骤4：根据公式(4)～(9)进行状态预测，得到状态预测值和预测误差协方差矩阵pk+1|k。

步骤5：根据公式(12)～(16)进行量测预测，得到量测预测值和互协方差矩阵pxz,k+1|k。

步骤6：本发明对故障检测方法采用缓变故障卡方检测方法，将在步骤8中介绍。根据公式(17)计算得到预测量测噪声协方差矩阵rk+1|k，并将其用于计算新息协方差矩阵pzz,k+1|k。根据缓变故障检测方法，检测当前时刻新息与理论新息协方差的不一致程度，判断是否存在发生故障；若没有发生故障，则k+1时刻的过程噪声均值和方差按公式(10)和公式(11)由saga-husa估计得出；若发生故障，则将过程噪声均值和过程噪声方差矩阵分别替换为qk+1＝qk，qk+1＝qk；

步骤7：在步骤6中，若没有检测出故障，根据公式(21)～(34)进行量测更新，此时的加权因子f(k+1)＝1，采用变分贝叶斯迭代估计得出状态估计值和状态估计误差方差矩阵pk+1。若检测出故障，则进行第一次变分贝叶斯迭代估计时采用公式(26)，剩下次数的迭代将公式(26)替换为

步骤8：根据权利要求步骤3中的缓变故障检测方法，得到伪量测预测值和伪息协方差阵pzz,k|k-m，计算出伪新息向量为：构造出故障检测函数用于步骤6中的故障检测。

本发明所述自适应容错容积卡尔曼滤波方法能够实现同时估计过程噪声协方差和量测噪声协方差，并提出容错的思想，使得通过雷达所测得的数据出现异常时，降低异常数据对滤波的估计精度的影响。通过缓变故障卡方检验方法检测故障，可以有效检测出缓变故障，有效地实现了目标跟踪功能。