本发明公开了一种基于有限步记忆的卡尔曼滤波方法，主要解决现有技术中目标跟踪准确性与稳定性低的问题。其技术方案是：由传统卡尔曼滤波方法得到目标航迹的前N步状态及状态协方差；按当前状态向后回溯N步，得到目标航迹的参考状态；根据参考状态判断目标的机动性，若发生机动，则对上一帧滤波的速度作修正；再根据参考状态判断当前量测的有效性，若无效，则对新息加一个小于1的权值，得到新的新息；根据上一时刻的状态协方差得到一步预测协方差，再计算增益矩阵；根据预测状态和增益矩阵及新的新息更新当前的状态；根据一步预测协方差和增益矩阵更新状态协方差，完成目标跟踪。本发明提高了目标跟踪的准确性与稳定性，可用于雷达数据处理。
1.基于有限步记忆的卡尔曼滤波方法，包括：(1)根据传统卡尔曼滤波方法得到目标航迹的前N步滤波状态 和预测状态 以及状态协方差P(k-1|k-1)，按照目标航迹的当前状态向后回溯N步，得到目标航迹的参考状态 其中，k＝1,2,…,N表示时刻；
(2)根据目标航迹的参考状态 判断目标是否发生机动：
2a)根据参考状态 到当前时刻预测状态 的位移a和当前时刻预测状态 到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：其中，＜a,c＞表示a与c之间的内积；
若 则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤(4)；
若 则表示目标发生了机动，此时对当前量测的速度加一个权值wX和对上一时刻滤波的速度加一个权值wZ：
2c)根据当前量测的速度的权值wX和上一时刻滤波的速度的权值wZ修正上一时刻的滤波速度，得到更新后的滤波 从而得到更新后的预测 再根据当前时刻的预测计算当前时刻预测的量测
(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)FT(k-1)其中，F(k-1)为状态转移矩阵，FT(k-1)为F(k-1)的转置。
5.根据权利要求1所述方法，其中步骤(4)中根据一步预测协方差P(k|k-1)计算当前增益矩阵G(k)，通过下式进行：G(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k))-1其中，H(k)为量测矩阵，HT(k)为H(k)的转置,(H(k)P(k|k-1)HT(k))-1为H(k)P(k|k-1)HT(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)为量测矩阵。
基于有限步记忆的卡尔曼滤波方法
技术领域
[0001] 本发明属于雷达数据处理技术领域，主要涉及慢速运动目标的跟踪滤波技术，可用于对地面与海面目标的稳定跟踪。
背景技术
[0002] 雷达对运动目标的跟踪即是跟踪滤波的过程，这是雷达跟踪的核心内容，它的作用是对目标运动状态作出估计和预测。跟踪的任务是通过相关和滤波处理建立目标的运动轨迹。雷达系统根据在建立目标轨迹过程中对目标运动状态作出的估计和预测来评估目标运动的安全态势和机动性的安全效果。因此，雷达跟踪环节工作性能的优劣直接影响到雷达系统的安全效能。
[0003] 鉴于目标跟踪在增进雷达效能中的重要作用，各国在军用和民用等领域中一直非常重视发展这一雷达技术。机动目标跟踪理论有了很大的发展，尤其是在跟踪算法的研究上，理论更是日趋成熟。在跟踪算法中，主要有线性自回归滤波，两点外推滤波，维纳滤波，加权最小二乘滤波，α-β滤波和卡尔曼滤波，其中卡尔曼滤波算法在目标跟踪理论中占据了主导地位。
[0004] 传统的标准卡尔曼滤波算法是一种叠代最小均方RLS滤波技术，其是把目标的位置，速度和加速度作为目标状态矢量，通过目标的动力学方程来描述目标状态的变化，利用递推的计算方法，估计目标的状态，以建立目标的航迹。卡尔曼滤波算法虽具有收敛速度快、运算量小的特点。但该方法普遍存在的问题就是跟踪误差大，跟踪结果可能不收敛甚至有丢失目标的状况，即它的稳定性不高。
发明内容
[0005] 本发明的目的在于针对传统的标准卡尔曼滤波算法的不足，提出一种基于有限步记忆的卡尔曼滤波方法，以减小跟踪误差，提高目标跟踪的准确性与稳定性。
[0006] 实现本发明目的的技术方案，包括如下：
[0007] (1)根据已知目标航迹的前N步滤波状态 和预测状态 以及状态协方差P(k-1|k-1)，按照目标航迹的当前状态向后回溯N步，得到目标航迹的参考状态其中，k＝1,2,…,N表示时刻；
[0008] (2)根据目标航迹的参考状态 判断目标是否发生机动：
[0009] 2a)根据参考状态 到当前时刻预测状态 的位移a和当前时刻预测状态 到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：
[0011] 2b)根据角度θ判断目标是否发生机动：
[0012] 若 则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤(4)；
[0013] 若 则表示目标发生了机动，此时对当前量测的速度加一个权值wX和对上一时刻滤波的速度加一个权值wZ：
[0014]
[0015] 2c)根据当前量测的速度的权值wX和上一时刻滤波的速度的权值wZ修正上一时刻的滤波，得到更新后的滤波 从而得到更新后的预测 再根据当前时刻的预测计算当前时刻预测的量测
[0016] (3)根据目标航迹的参考状态 判断当前量测Z(k)的可靠性：
[0017] 3a)根据参考状态 到当前时刻预测状态 的位移a和参考状态 到量测状态Z(k)的位移b，计算这两个位移之间的夹角α：
[0019] 3b)根据当前量测Z(k)和当前预测的量测 得到当前新息v(k)：
[0020]
[0021] 3c)根据角度α判断当前量测Z(k)的可靠性：
[0022] 若cosα≥0，则表示当前量测可靠；
[0023] 若cosα<0，则表示当前量测不可靠，对当前新息v(k)加一个权值w，得到新的新息v'(k)，其中，w<1；
[0024] (4)根据上一时刻的状态协方差P(k-1|k-1)得到一步预测协方差P(k|k-1)，进而计算出当前增益矩阵G(k)；
[0025] (5)根据当前预测状态 和增益矩阵G(k)及新的新息v'(k)，更新当前时刻的滤波状态
[0026] (6)根据一步预测协方差P(k|k-1)和增益矩阵G(k)，更新当前状态协方差矩阵P(k|k)。
[0027] 本发明与现有的传统卡尔曼滤波方法相比具有以下优点：
[0028] 1.本发明在目标发生机动时，对上一帧滤波状态的速度进行加权修正，避免了跟踪结果不收敛甚至目标丢失的情况；
[0029] 2.本发明通过将目标航迹向后回溯N步来判断当前量测的可靠性，对于不可靠的量测再通过对当前新息加一个小于1的权值使其满足可靠性的要求，从而大大减小了跟踪误差；
[0030] 3.本发明使用基于有限步记忆的卡尔曼滤波方法，使目标跟踪的准确性与稳定性得以大幅提高。
[0031] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将结合实施例和附图对本发明作进一步描述。
附图说明
[0032] 图1为本发明的实现流程图；
[0033] 图2为本发明中量测可靠性判断示意图；
[0034] 图3为用传统卡尔曼滤波方法处理间断航迹的仿真结果图；
[0035] 图4为用本发明中基于有限步记忆的卡尔曼滤波方法处理间断航迹的仿真结果图；
[0036] 图5为用传统卡尔曼滤波方法处理连续航迹的仿真结果图；
[0037] 图6为用本发明中基于有限步记忆的卡尔曼滤波方法处理连续航迹的仿真结果图；
具体实施方式
[0038] 参照图1，本发明的实现步骤包括如下：
[0039] 步骤一、获取目标航迹的参考状态。
[0040] 由传统卡尔曼滤波方法得到目标航迹的前N步滤波状态 和预测状态以及状态协方差P(k-1|k-1)，其中，k＝1,2,…,N表示时刻；
[0041] 按照航迹的当前状态向后回溯N步，得到的滤波状态称为目标航迹的参考状态[0042] 步骤二、根据目标航迹的参考状态 判断目标是否发生机动。
[0043] 参照图2，本步骤的具体实现如下：
[0044] 2a)根据参考状态 到当前时刻预测状态 的位移a和当前时刻预测状态 到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：
[0046] 2b)根据角度θ判断目标是否发生机动：
[0047] 若 则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤三；
[0048] 若 则表示目标发生了机动，此时分别给定当前量测的速度的权值wZ和上一时刻滤波的速度的权值wX：
[0049]
[0050] 2c)根据当前量测的速度vZ的权值wZ和上一时刻滤波的速度vX的权值wX修正上一时刻滤波的速度vX，得到修正后的速度v'X：
[0051] v′X＝wZ·vZ+wX·vX，
[0052] 用修正后的速度v'X代替修正前的速度vX，得到修正后的滤波 进而更新当前时刻的预测 为：
[0053]
[0054] 其中，F(k-1)为状态转移矩阵；
[0055] 再根据当前时刻的预测 得到当前时刻预测的量测
[0056]
[0057] 其中，H(k)为量测矩阵。
[0058] 步骤三、根据目标航迹的参考状态 判断当前量测Z(k)的可靠性。
[0059] 3a)根据参考状态 到当前时刻预测状态 的位移a和参考状态 到量测状态Z(k)的位移b，计算这两个位移之间的夹角α：
[0061] 3b)根据当前量测Z(k)和当前时刻预测的量测 得到当前新息v(k)：
[0062]
[0063] 3c)根据角度α判断当前量测Z(k)的可靠性：
[0064] 若cosα≥0，则表示当前量测可靠；
[0065] 若cosα<0，则表示当前量测不可靠，则要对当前新息v(k)加一个权值w，得到新的新息v'(k)，其中，w<1。
[0066] 步骤四、计算一步预测协方差矩阵和当前增益矩阵。
[0067] 4a)根据上一时刻的状态协方差矩阵P(k-1|k-1)得到一步预测协方差矩阵P(k|k-1)：
[0068] P(k|k-1)＝F(k-1)P(k-1|k-1)FT(k-1)，
[0069] 其中，FT(k-1)为状态转移矩阵F(k-1)的转置。
[0070] 4b)根据一步预测协方差矩阵P(k|k-1)计算出当前增益矩阵G(k)：
[0071] G(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k))-1，
[0072] 其中，HT(k)为量测矩阵H(k)的转置，(H(k)P(k|k-1)HT(k))-1为H(k)P(k|k-1)HT(k)的逆。
[0073] 步骤五、更新当前时刻的滤波状态。
[0074] 根据当前预测状态 和增益矩阵G(k)及新的新息v'(k)来更新当前时刻的滤波状态
[0075]
[0076] 步骤六、更新当前时刻的状态协方差矩阵。
[0077] 根据一步预测协方差矩阵P(k|k-1)和增益矩阵G(k)来更新当前时刻状态协方差矩阵P(k|k)：
[0078] P(k|k)＝P(k|k-1)-G(k)H(k)P(k|k-1)。
[0079] 本发明的效果可以通过以下对实测数据的处理进一步说明：
[0080] 1.实验环境与条件:
[0081] 数据率：40s/帧
[0082] 目标数：1个
[0083] 测距误差：10000m
[0084] 测速误差：5m/s
[0085] 在仿真中，两组数据的帧数分别为122帧和347帧。
[0086] 2.实验内容与结果：
[0087] 实验1，用传统卡尔曼滤波方法对间断的目标航迹进行滤波处理，结果如图3。
[0088] 实验2，用本发明方法对间断的目标航迹进行滤波处理，结果如图4。
[0089] 比较图3和图4可见，传统卡尔曼滤波方法滤波值随量测值的波动较大，滤波平滑效果较差，尤其在航迹间断处的滤波误差更大。而本发明滤波值随量测值的波动很小，滤波效果得到明显改善，航迹间断处的滤波误差也大大降低。
[0090] 实验3，用传统卡尔曼滤波方法对连续的目标航迹进行滤波处理，结果如图5。
[0091] 实验4，用本发明方法对连续的目标航迹进行滤波处理，结果如图6。
[0092] 比较由图5和图6可见，传统卡尔曼滤波方法滤波值随量测值的波动较大，滤波平滑效果较差。而本发明滤波值随量测值的波动很小，滤波平滑效果得到明显改善。
[0093] 综上，本发明与传统卡尔曼滤波方法相比，大大减小了跟踪误差，提高了跟踪的稳定性。
[0094] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。
