[发明专利]基于有限步记忆的卡尔曼滤波方法

权利要求书

1.基于有限步记忆的卡尔曼滤波方法，包括：

(1)根据传统卡尔曼滤波方法得到目标航迹的前N步滤波状态和预测状态以及状态协方差P(k-1|k-1)，按照目标航迹的当前状态向后回溯N步，得到目标航迹的参考状态其中，k＝1,2,…,N表示时刻；

(2)根据目标航迹的参考状态判断目标是否发生机动：

2a)根据参考状态到当前时刻预测状态的位移a和当前时刻预测状态到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：

其中，＜a,c＞表示a与c之间的内积；

2b)根据角度θ判断目标是否发生机动：

若则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤(4)；

若则表示目标发生了机动，此时对当前量测的速度加一个权值w_X和对上一时刻滤波的速度加一个权值w_Z：

2c)根据当前量测的速度的权值w_X和上一时刻滤波的速度的权值w_Z修正上一时刻的滤波速度，得到更新后的滤波从而得到更新后的预测再根据当前时刻的预测计算当前时刻预测的量测

v’_X＝w_Z·v_Z+w_X·v_X，

(3)根据目标航迹的参考状态判断当前量测Z(k)的可靠性：

3a)根据参考状态到更新后预测状态的位移a和参考状态到量测状态Z(k)的位移b，计算这两个位移之间的夹角α：

其中，＜a,b＞表示a与b之间的内积；

3b)根据当前量测Z(k)和当前预测的量测得到当前新息v(k)：

3c)根据角度α判断当前量测Z(k)的可靠性：

若cosα≥0，则表示当前量测可靠；

若cosα＜0，则表示当前量测不可靠，对当前新息v(k)加一个权值w，得到新的新息v'(k)，其中，w＜1；

(4)根据上一时刻的状态协方差P(k-1|k-1)得到一步预测协方差P(k|k-1)，进而计算出当前增益矩阵G(k)；

(5)根据当前预测状态和增益矩阵G(k)及新的新息v'(k)，更新当前时刻的滤波状态

(6)根据一步预测协方差P(k|k-1)和增益矩阵G(k)，更新当前状态协方差矩阵P(k|k)。

2.根据权利要求1所述方法，其中，步骤(2)中得到更新后的预测预测通过下式进行：

其中，F(k-1)为状态转移矩阵，为更新后的滤波。

3.根据权利要求1所述方法，其中，步骤(2)中根据当前时刻的预测计算当前时刻预测的量测通过下式进行：

其中，H(k)为量测矩阵。

4.根据权利要求1所述方法，其中，步骤(4)中根据上一时刻的状态协方差P(k-1|k-1)得到一步预测协方差P(k|k-1)，通过下式进行：

P(k|k-1)＝F(k-1)P(k-1|k-1)F^T(k-1)

其中，F(k-1)为状态转移矩阵，F^T(k-1)为F(k-1)的转置。

5.根据权利要求1所述方法，其中步骤(4)中根据一步预测协方差P(k|k-1)计算当前增益矩阵G(k)，通过下式进行：

G(k)＝P(k|k-1)H^T(k)(H(k)P(k|k-1)H^T(k))^-1

其中，H(k)为量测矩阵，H^T(k)为H(k)的转置,(H(k)P(k|k-1)H^T(k))^-1为H(k)P(k|k-1)H^T(k)的逆。

6.根据权利要求1所述方法，其中，步骤(5)中根据当前预测状态和增益矩阵G(k)及新的新息v'(k)，更新当前时刻的滤波状态通过下式进行：

7.根据权利要求1所述方法，其中，步骤(6)中根据一步预测协方差P(k|k-1)和增益矩阵G(k)，更新当前状态协方差矩阵P(k|k)，通过下式进行：

P(k|k)＝P(k|k-1)-G(k)H(k)P(k|k-1)

其中，H(k)为量测矩阵。