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

权利要求书

1.一种克服雷达扩展卡尔曼航迹滤波发散的方法，包括如下步骤：

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

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

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

（4）对预测协方差矩阵P(k+1|k)进行防发散处理得到修正后的预测协方差矩阵Pm(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)；

其特征在于：所述步骤（4）中采用修正矩阵M(k)对所述预测协方差矩阵进行防发散处理；所述雷达探测目标系统中的分量包括目标位置（x、y、z）、姿态角（航向角az、俯仰角el、横滚角ro）、三向速度（v_x、v_y、v_z），所述修正矩阵M(k)对角线上的取值对应系统状态中每个分量方差的修正值，，

其中， 分别为位置坐标、姿态角、三向速度的方差修正值，所述修正后预测协方差矩阵Pm(k+1|k)=P(k+1|k)+M(k)。

2.根据权利要求1所述的克服雷达扩展卡尔曼航迹滤波发散的方法，其特征在于：所述雷达探测目标系统采用常加速度模型，根据公式 ，，

得到：转移矩阵，噪声矩阵。

3.根据权利要求2所述的克服雷达扩展卡尔曼航迹滤波发散的方法，其特征在于：所述步骤（3）中k+1时刻的预测状态，

预测协方差矩阵，

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

4.根据权利要求3所述的克服雷达扩展卡尔曼航迹滤波发散的方法，其特征在于：所述步骤（5）中预测的测量值为，H(k)是量测转换矩阵；

所述新息矩阵为：，其中， R(k+1)为观测噪声矩阵，采用对角阵且值为各分量的测量误差值；

所述新息向量为：；

所述卡尔曼增益矩阵为：。

5.根据权利要求4所述的克服雷达扩展卡尔曼航迹滤波发散的方法，其特征在于：所述步骤（6）中计算所得状态值为：；

计算所得协方差矩阵为：，I为单位矩阵。

6.根据权利要求4所述的克服雷达扩展卡尔曼航迹滤波发散的方法，其特征在于：所述步骤（5）中H(k)采用雅克比矩阵。