一种克服雷达扩展卡尔曼航迹滤波发散的方法

技术领域

本发明涉及卡尔曼航迹滤波数据处理技术领域，具体涉及一种克服雷达扩展卡尔曼航迹滤波发散的方法。

背景技术

航迹滤波是雷达数据处理算法的重要组成部分，能将雷达探测点迹滤波形成连续航迹输出，有效提高雷达目标探测跟踪精度。扩展卡尔曼滤波是最为常用的一种航迹滤波算法，常规的扩展卡尔曼滤波算法通过建立系统状态方程，结合对系统的实际观测数据，实现对系统状态的最优估计。

目前技术存在的问题及缺点：

1)在雷达对空探测领域，尤其是军用场景下，探测目标往往是高速、高机动目标，常规的目标运动模型（如常速度模型、常加速度模型）无法在长时间段内准确描述目标的运动规律，导致卡尔曼滤波算法失去最优性，严重时甚至产生较大偏差出现滤波发散的情况。

2)常规扩展卡尔曼航迹滤波的精度很大程度上依赖于对目标运动过程建模的逼真程度，为此，有人提出了更加复杂的目标运动方程模型，甚至通过不同运动模型的组合来建立更逼真的运动模型。然而，复杂运动模型在工程实现上是困难的，而且，即使是复杂的运动模型，也不能够完全准确描述目标的所有运动过程，尤其是在目标进行高速大机动运动时。当存在多个运动目标时，该问题会更加严重。

3)也有将常规扩展卡尔曼滤波修改为平方根滤波的形式，即采用平方根扩展卡尔曼滤波方法，来克服航迹滤波发散的问题。但是这种方法仅适用于以前硬件计算能力弱，计算精度低的条件下滤波运算过程中可能出现奇异矩阵的情况，对系统方程无法准确描述真实系统过程的问题起不到任何作用。如今随着硬件能力的提升，高精度浮点计算已不是问题，平方根扩展卡尔曼滤波使用场景将进一步得到限制。

公开号为CN110455287A的发明专利自适应无迹卡尔曼粒子滤波方法提供了偏粒子滤波流程，也能够抑制滤波发散的问题，但其应用场合为测量值出现错误时候的一种修正手段，并不适用于测量值本身是正确的，而由于系统建模原因导致预测值不准，航迹会出现发散的情况。

发明内容

发明目的：为了解决雷达航迹滤波中目标运动方程模型无法长时间准确描述目标运动特性从而导致目标滤波航迹发散的问题，本发明提供一种克服雷达扩展卡尔曼航迹滤波发散的方法，以修正滤波输出趋势，提高滤波结果精度，克服航迹滤波中可能出现的发散情况。

技术方案：本发明所述克服雷达扩展卡尔曼航迹滤波发散的方法，包括如下步骤：

（1）按照常规扩展卡尔曼滤波方法进行系统建模，建立雷达探测目标系统的状态方程和测量方程；

（2）假设雷达探测目标系统航迹滤波上一时刻为k，则k时刻的状态为X(k|k)、协方差矩阵为P(k|k)；

（3）对k时刻状态进行一步预测，得到k+1时刻的预测状态X(k+1|k)和预测协方差矩阵P(k+1|k)；

（4）采用修正矩阵M(k)或修正系数对所述预测协方差矩阵进行防发散处理；所述修正矩阵M(k)为对角矩阵，且对角线上取值对应为雷达探测目标系统中每个分量方差的修正值，所述修正后预测协方差矩阵Pm(k+1|k)=P(k+1|k)+M(k)；采用修正系数对所述预测协方差矩阵进行防发散处理具体操作为以下任意一种：将P(k+1|k)矩阵中对角线元素乘以同一放大系数，将P(k+1|k)矩阵中对角线元素乘以不同放大系数，将P(k+1|k)矩阵中全体元素乘以同一放大系数，将P(k+1|k)矩阵中全体元素乘以不同放大系数；

（5）通过雷达探测目标系统k+1时刻的测量值Z(k+1)对预测状态进行更新，计算新息向量Y(k+1)、新息矩阵S(k+1)和卡尔曼增益矩阵K(k+1)，计算过程中用修正后的预测协方差矩阵Pm(k+1|k)代替原始预测协方差矩阵P(k+1|k)；

（6）计算k+1时刻的目标状态X(k+1|k+1)和协方差矩阵P(k+1|k+1)。

进一步完善上述技术方案，所述雷达探测目标系统采用常加速度模型，根据公式

得到：转移矩阵

进一步地，所述步骤（3）中k+1时刻的预测状态

预测协方差矩阵

其中，F(k)为转移矩阵，Q(k)为过程激励噪声矩阵，取为常数阵。

进一步地，所述雷达探测目标系统中的分量包括目标位置（x、y、z）、姿态角（航向角az、俯仰角el、横滚角ro）、三相速度（v

其中，

进一步地，所述步骤（5）中预测的测量值为

所述新息矩阵为：

所述新息向量为：

所述卡尔曼增益矩阵为：

进一步地，所述步骤（6）中计算所得状态值为：

计算所得协方差矩阵为：

进一步地，所述步骤（5）中H(k)是采用雅克比矩阵。

有益效果：与现有技术相比，本发明的优点在于：

1)通过对传统协方差矩阵进行优化，修正了滤波输出趋势，使滤波结果精度有所提高，有效克服了航迹滤波中出现的发散情况；

2)无需修改扩展卡尔曼滤波器现有系统状态模型，不增加系统复杂度；

3)由于核心步骤是在卡尔曼滤波的一步预测与状态更新之间插入协方差矩阵修正处理，因此与原始滤波流程兼容度高，代码修改量小；

4)协方差矩阵修正代码运行耗时低，对原始滤波代码的时效性几乎无影响。

附图说明

图1是本发明克服雷达扩展卡尔曼航迹滤波发散的处理流程；

图2是常规扩展卡尔曼理论滤波结果示意图；

图3是本发明滤波结果示意图。

具体实施方式

下面通过附图对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

本发明提供的克服雷达扩展卡尔曼航迹滤波发散的方法，通过对协方差矩阵的修正，解决航迹滤波中的发散问题。卡尔曼递推公式主要分为预测和更新两部分，在预测阶段，使用系统上一状态的估计值，做出对当前系统状态的预测并得到预测的观测值；在更新阶段，利用对当前状态的观测值修正在上一阶段获得的预测值，以获得一个更接近真实值的新估计值。

如图1所示的处理流程及方法：

（1）按照常规扩展卡尔曼滤波方法进行系统建模，建立雷达探测目标系统的状态方程和测量方程；

（2）假设雷达探测目标系统航迹滤波上一时刻为k，则k时刻的状态为X(k|k)、状态协方差矩阵为P(k|k)；

（3）为获取目标k+1时刻的滤波状态，首先对k时刻状态进行一步预测，得到k+1时刻的预测状态X(k+1|k)和预测协方差矩阵P(k+1|k)；

（4）为克服滤波过程中可能出现的发散问题，对一步预测得到的协方差矩阵P(k+1|k) 进行防发散处理，得到修正后预测协方差矩阵Pm(k+1|k)，一种简单的修正方法是基于滤波器应用场景，将修正矩阵M(k) 取为一对角阵，对角线上每个元素对应系统状态中每个分量方差的修正值；

（5）通过雷达探测目标系统k+1时刻的测量值Z(k+1)对预测状态进行更新，计算新息向量Y(k+1)、新息矩阵S(k+1)和卡尔曼增益矩阵K(k+1)，计算过程中用修正后的预测协方差矩阵Pm(k+1|k)代替原始预测协方差矩阵P(k+1|k)；

（6）计算k+1时刻的目标状态X(k+1|k+1)和协方差矩阵P(k+1|k+1)。

下面提供一种具体实施方式：

（1）系统建模：按照常规扩展卡尔曼滤波方法进行系统建模，设定雷达探测目标的运动方程和雷达系统测量方程，本系统运动模型采用常加速度模型，根据公式

得到：转移矩阵

（2）状态预测：在雷达探测目标系统中，状态元素一般有位置坐标、姿态角、三向速度，假设雷达航迹滤波上一时刻为k，且k时刻的滤波系统状态为X(k|k)、协方差矩阵为P(k|k)，状态转移函数为F(k)，则可得到k+1时刻状态预测值X(k+1|k)，即

（3）协方差预测：由于系统发生转移，因此要对协方差进行预测，得到协方差矩阵P(k+1|k) ，即

其中， Q(k)为过程激励噪声矩阵，可取为常数阵。

（4）协方差修正：为克服滤波过程中可能出现的发散问题，在一步预测与状态更新之间对一步预测得到的状态协方差矩阵P(k+1|k) 进行防发散处理，得到修正后的矩阵为Pm(k+1|k) ,即Pm(k+1|k)=P(k+1|k)+M(k)

通常假设系统状态各维之间是相互独立的，因此，修正矩阵M(k) 设置为对角阵即可，对角线元素大小直接影响滤波效果。由于雷达航迹滤波主要对目标位置（x、y、z）、姿态角（航向角az、俯仰角el、横滚角ro）、三相速度（v

（5）增益矩阵计算：卡尔曼滤波最终的滤波结果是通过预测和观测的加权进行合成，因此需要计算新息矩阵S(k+1)和卡尔曼增益矩阵K(k+1)，

预测的测量值：

其中，H(k)是量测转换矩阵，对于多数雷达来说，由于目标的状态和观测坐标系不同从而导致转换存在非线性关系，以至于在计算H矩阵时可能存在非线性偏差，因此，首先对非线性系统进行线性化处理，利用非线性方式求出H矩阵，本发明使用的非线性工具为雅克比矩阵；

新息矩阵：

其中，R(k+1)表示观测噪声矩阵，本系统中取其为对角阵，且值为各分量的测量误差值（标准差），并用方差表示；

新息向量：

卡尔曼增益矩阵：

1) 滤波估值计算：滤波估值就是卡尔曼滤波输出的状态值X(k+1|k+1)，

2) 滤波估值协方差计算：状态值更新后，需对协方差矩阵进行更新，得到P(k+1|k+1)，以进行下一次的递推过程，其中I为单位矩阵，即

另一种具体实施方式的主要区别在于步骤4中防发散处理的方法，由步骤4可知为M(k)对角阵，对P(k+1|k)矩阵的影响主要在于其对角线元素值的大小。因此，我们可以不叠加修正矩阵M(k)，而将P(k+1|k)矩阵的对角线元素（或者全体元素）乘上某一放大系数（或不同的放大系数）来提高一步预测所对应的不确定度，具体方式可以因应用场景的不同选取。

如图2所示采用传统扩展卡尔曼理论滤波结果示意图，直线为真实航迹，波动线为滤波航迹，图中显示运动模型不能完全反映目标运动特性，导致滤波结果发散。

如图3所示采用本发明提供的方法滤波结果示意图，直线为真实航迹，波动线为滤波航迹。通过图2、图3的对比能够看出，本发明修正了滤波输出趋势，使滤波结果精度有所提高，有效克服了航迹滤波中可能出现的发散情况。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。