CN109163720A - 基于渐消记忆指数加权的卡尔曼滤波跟踪方法 - Google Patents

Abstract

本发明提供的一种基于渐消记忆指数加权的卡尔曼滤波跟踪方法，包括以下步骤：获取状态误差协方差矩阵P和系统过程噪声矩阵；计算运动目标在第k时刻的预测状态参量估计值计算第k时刻的新息协方差C_0,k；计算第k时刻新息γ_k，计算第k时刻新息协方差的估计值计算第k时刻的加权系数β_k，进一步计算第k时刻的渐消因子λ_k；计算第k时刻的预测状态误差协方差矩阵P_k|k‑1、卡尔曼增益K_k，进一步计算第k时刻的状态估计值以及状态误差协方差矩阵P_k；其中，计算第k时刻新息协方差的估计值的计算方法为其中，加权系数{β_i}呈负指数规律衰减。本发明克服了传统的利用加窗平均的方法计算新息残差矢量估计的精度较差的问题，有效提升新息残差的估计精度，从而使本发明方法具有更高的精确性和鲁棒性。

Description

基于渐消记忆指数加权的卡尔曼滤波跟踪方法

技术领域

本发明涉及卡尔曼滤波技术领域，特别是涉及一种基于渐消记忆指数加权的卡尔曼滤波跟踪方法。

背景技术

卡尔曼滤波(Kalman Filter，KF)方法作为线性、高斯系统的最优递推估计方法，具有精度高、计算复杂度低等优点，从1960年被提出以来，在状态估计、目标跟踪、导航制导、机械控制等领域得到了广泛深入的应用和研究。KF的主要思想是基于当前的估计值和当前的观测信息对下一时刻的状态进行递推估计。传统卡尔曼滤波方法要求构建精确的系统模型，包括系统的状态噪声和观测噪声的统计特性模型，如果噪声统计特性失配，就会导致滤波估计精度的降低，甚至引起滤波的发散。但是在实际的工程应用中，由于外部未知干扰的存在，系统状态噪声和观测噪声的统计特性很难精确获取，而且具有时变特性，因此，降低了卡尔曼滤波方法的性能。

为了抑制卡尔曼滤波在噪声失配情况下精度降低甚至发散的问题，目前常用的方法主要有两种：一种是采用自适应滤波的方法，在估计状态参量的同时，对系统过程噪声和观测噪声的统计特性进行同步估计。虽然该类方法能够有效提升噪声失配情况下的跟踪精度，但是随着状态估计参量个数的增加，导致计算复杂度增加、滤波处理的效率降低；另一种是采用渐消因子对状态一步预测的协方差矩阵P_k|k-1进行修正，通过降低历史信息的影响，增强当前观测信息的修正作用来抑制噪声失配引起的漂移问题。该类方法具有完善的数学理论基础，且基本上没有影响卡尔曼滤波方法的计算复杂性，具有较强的适应性，在动态非线性系统状态估计、捷联惯性导航系统(Strap-down Inertial Navigation System，SINS)中得到了广泛深入的应用。但是该类方法的关键问题是如何精确有效的更新渐消因子。

多渐消因子卡尔曼滤波及其在SINS初始对准中的应用(钱华明等著，中国惯性技术学报，2012，20(3):287-291)通过对残差序列进行正交化处理，实现了渐消因子的基函数计算方法，并给出了详细的理论推导过程，但是该方法只适用于独立同分布的噪声系统，这一点在实际的工程应用中很难满足，因此限制了该方法的应用。基于GPS/INS组合导航的改进自适应渐消卡尔曼滤波算法(马龙等著，科学技术与工程,2013,13(33):9973-9977)将模糊控制的方法引入到KF的反馈系统，通过反馈残差的模糊逻辑实现渐消因子的计算，但是该方法只适应平稳系统，在系统状态突变情况下，渐消因子会出现错误的逻辑值，导致滤波发散。基于新息协方差的自适应渐消卡尔曼滤波器(徐定杰等著，系统工程与电子技术，2011,33(12)：2696-2699)基于新息序列协方差的估计值和理论计算值的残差构建同渐消因子相关的代价函数，通过最小化代价函数进行渐消因子的计算，明显提升了滤波在噪声失配情况下的精度。但是该方法严重依赖于新息协方差的估计精度。当新息协方差估计误差较大的时候，容易导致代价函数收敛较慢、甚至处于震荡状态，无法收敛，严重影响了该算法的应用。

传统的渐消因子卡尔曼滤波跟踪方法在进行渐消因子计算的时候通过对历史观测信息进行加窗平均的方法来计算新息协方差矩阵的估计值，降低了当前最新观测信息对新息协方差矩阵的修正作用，降低了算法的估计精度，导致渐消因子的计算误差较大。

发明内容

基于此，有必要针对传统的渐消因子卡尔曼滤波跟踪方法渐消因子计算方法降低了当前最新观测信息对新息协方差矩阵的修正作用，降低了算法的估计精度，导致渐消因子的计算误差较大的问题，提供了一种基于渐消记忆指数加权的卡尔曼滤波跟踪方法。

本发明提供的一种基于渐消记忆指数加权的卡尔曼滤波跟踪方法，所述跟踪方法采用卡尔曼滤波器进行误差预测，包括以下步骤：

步骤一：建立运动目标的状态模型以及观测模型，根据所述状态模型获取状态误差协方差矩阵P；

步骤二：获取第k-1时刻的状态参量X_k-1以及状态误差协方差矩阵P_k-1，根据所述第k-1时刻的状态参量X_k-1以及状态误差协方差矩阵P_k-1按照卡尔曼递推公式计算运动目标在第k时刻的预测状态参量估计值进一步计算第k时刻的新息协方差C_0,k；

步骤三：获取第k时刻的目标的观测参量Z_k，根据所述第k时刻的目标的观测参量Z_k计算第k时刻新息γ_k，进一步计算第k时刻新息协方差的估计值

步骤四：计算第k时刻的加权系数β_k，根据所述第k时刻的加权系数β_k、所述第k时刻新息γ_k以及所述第k时刻新息协方差的估计值计算第k时刻的渐消因子λ_k；

步骤五：通过所述第k时刻的渐消因子λ_k计算第k时刻的预测状态误差协方差矩阵P_k|k-1、卡尔曼增益K_k，进一步计算得到第k时刻的状态估计值以及状态误差协方差矩阵P_k；

其中，计算第k时刻新息协方差的估计值的计算方法为

其中，所述加权系数{β_i}呈负指数规律衰减。

在其中一个实施例中，所述加权系数{β_i}的衰减规律为

其中，b为遗忘因子，b∈[0.7,0.95]。

在其中一个实施例中，所述加权系数{β_i}表示为β_i＝d_kb^k-i,i＝1,2,…,k；

其中，

在其中一个实施例中，所述第k时刻新息协方差的估计值的计算方法为

在其中一个实施例中，所述第k时刻新息协方差的估计值的递推计算方法为

在其中一个实施例中，其特征在于，所述遗忘因子b为0.8。

在其中一个实施例中，所述状态模型为X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1；

所述观测模型为Z_k＝H_kX_k+V_k；

其中，X_k∈Rⁿ为状态参量；Z_k∈R^m为观测参量；Φ_k,k-1∈R^n×n为状态的一步转移矩阵；Γ_k-1∈R^n×p为系统过程噪声矩阵；H_k∈R^m×n观测矩阵；W_k为系统过程噪声；V_k为观测噪声。

在其中一个实施例中，所述系统过程噪声W_k以及所述观测噪声V_k满足以下条件：

其中，Q_k表示非负的系统状态噪声方差矩阵；R_k表示正定的观测噪声方差矩阵；δ_kj为狄利克雷函数。

上述基于渐消记忆指数加权的卡尔曼滤波跟踪方法，采用渐消记忆指数加权的方法实时估计新息残差矢量，克服了传统的利用加窗平均的方法计算新息残差矢量估计的精度较差的问题，有效提升新息残差的估计精度，从而使本发明方法具有更高的精确性和鲁棒性。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。

图1本发明实施例一的滤波估计结果的位置估计值；

图2本发明实施例一的滤波估计结果的位置估计误差；

图3本发明实施例二的在状态噪声失配10倍的情况下，滤波估计结果的位置估计值；

图4本发明实施例二的在状态噪声失配10倍的情况下，滤波估计结果的位置估计误差；

图5本发明实施例二的在状态噪声失配50倍的情况下，滤波估计结果的位置估计值；

图6本发明实施例二的在状态噪声失配50倍的情况下，滤波估计结果的位置估计误差。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下通过实施例，并结合附图，对本发明的基于分类进化重采样的粒子滤波方法进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明一实施例的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，跟踪方法采用卡尔曼滤波器进行误差预测，包括以下步骤：

步骤一：建立运动目标的状态模型以及观测模型，根据状态模型获取状态误差协方差矩阵P；

步骤二：获取第k-1时刻的状态参量X_k-1以及状态误差协方差矩阵P_k-1，根据第k-1时刻的状态参量X_k-1以及状态误差协方差矩阵P_k-1按照卡尔曼递推公式计算运动目标在第k时刻的预测状态参量估计值进一步计算第k时刻的新息协方差C_0,k；

步骤三：获取第k时刻的目标的观测参量Z_k，根据第k时刻的目标的观测参量Z_k计算第k时刻新息γ_k，进一步计算第k时刻新息协方差的估计值

步骤四：计算第k时刻的加权系数β_k，根据第k时刻的加权系数β_k、第k时刻新息γ_k以及第k时刻新息协方差的估计值计算第k时刻的渐消因子λ_k；

步骤五：通过第k时刻的渐消因子λ_k计算第k时刻的预测状态误差协方差矩阵P_k|k-1、卡尔曼增益K_k，进一步计算得到第k时刻的状态估计值以及状态误差协方差矩阵P_k；

其中，计算第k时刻新息协方差的估计值的计算方法为

其中，加权系数{β_i}呈负指数规律衰减。

上述基于渐消记忆指数加权的卡尔曼滤波跟踪方法，采用渐消记忆指数加权的方法实时估计新息残差矢量，克服了传统的利用加窗平均的方法计算新息残差矢量估计的精度较差的问题，有效提升新息残差的估计精度，从而使本发明方法具有更高的精确性和鲁棒性。

在本发明中，建立的运动目标的状态模型和观测模型表示为式(1)所示：

式(1)中，X_k∈Rⁿ为状态参量；Z_k∈R^m为观测参量；Φ_k,k-1∈R^n×n为状态的一步转移矩阵；Γ_k-1∈R^n×p为系统过程噪声矩阵；H_k∈R^m×n观测矩阵；W_k为系统过程噪声；V_k为观测噪声。

进一步地，系统过程噪声W_k以及观测噪声V_k满足如式(2)所示的条件：

其中，Q_k表示非负的系统状态噪声协方差矩阵；R_k表示正定的观测噪声协方差矩阵；δ_kj为狄利克雷函数。

传统的标准卡尔曼滤波方法的递推估计过程可以描述为式(3)至式(8)所示：

P_k＝(I-K_kH_k)P_k|k-1 (8)

其中，由式(7)和式(8)分别得到第k时刻的状态估计值以及状态误差协方差矩阵P_k。

式(6)中，γ_k表示当前k时刻最新观测信息引入的新息(联测残差)，由此，新息协方差矩阵C_0,k的理论值计算如式(9)所示：

由式(3)至式(8)所示的推倒过程可以看出，卡尔曼滤波方法具有无限递推的记忆特性，当系统过程模型或者是噪声统计特性失配的情况下，将会导致卡尔曼滤波方法的最优估计结果产生偏差，严重时将会引起滤波的发散，影响滤波器的正常使用。

在传统的渐消因子卡尔曼滤波跟踪方法渐消因子计算方法中，与传统的标准卡尔曼滤波的不同之处在于，在如式(5)所示的预测状态误差协方差矩阵P_k|k-1中引入渐消因子λ_k，则预测状态误差协方差矩阵P_k|k-1如式(10)所示：

采用如式(10)所示的预测状态误差协方差矩阵P_k|k-1替换式(5)所示的预测状态误差协方差矩阵P_k|k-1即可得到传统的渐消因子卡尔曼滤波跟踪方法的推倒过程。基于新息序列方差的估计值和理论计算值的残差构建同渐消因子相关的代价函数，通过最小化代价函数进行渐消因子的计算，能够明显提升滤波在噪声失配情况下的精度。但是该方法严重依赖于新息协方差的估计精度。当新息协方差估计误差较大的时候，容易导致代价函数收敛较慢、甚至处于震荡状态，无法收敛，严重影响了该算法的应用。

在传统的渐消因子卡尔曼滤波跟踪方法的渐消因子计算方法中，渐消因子λ_k计算方法如式(11)所示：

在式(11)中，tr(·)表示求矩阵的迹运算，N_k和M_k的计算方法如式(12)所示：

从式(12)中可以看出，渐消因子λ_k的计算主要取决于新息矢量γ_k的新息协方差矩阵的估计值传统的渐消因子卡尔曼滤波的渐消因子计算过程中主要是采用加窗平均的方法，计算方法如式(13)所示：

同时，式(13)获取最优估计值的条件是新息矢量γ_k的元素满足相互正交的特性，即要求新息矢量γ_k满足如式(14)所示的条件：

进一步地，式(14)所示的条件可等价为式(15)所示的条件：

即传统的渐消因子卡尔曼滤波主要通过不断地更新渐消因子λ_k来在线的调整卡尔曼增益K_k，尽量满足γ_k元素正交的特性，从而达到一致滤波发散、提升系统在模型和噪声统计特性失配情况下的滤波估计性能。从式(11)到(15)可以看出，渐消因子的计算主要依赖于新息协方差矩阵估计值的计算，但是从式(13)中可以看出，在计算新息协方差矩阵的时候，其样本数据的每一项加权系数均为1/k-1，同等对待了历史和最新观测信息，因此导致计算精度的降低，并消弱了最新观测信息的修正作用。

在本发明中的发明构思中，主要通过降低历史信息对计算的新息协方差矩阵的影响，同时增加最新观测信息对新息协方差矩阵的修正作用。在本发明中通过使计算第k时刻新息协方差的估计值的计算方法为并且使加权系数{β_i}呈负指数规律衰减，从而基于加权系数系数{β_i}的负指数衰减规律降低了历史信息对计算的新息协方差矩阵的影响，同时增加最新观测信息对新息协方差矩阵的修正作用。

进一步可选地，加权系数{β_i}的衰减规律如式(16)所示：

其中，b为遗忘因子，b∈[0.7,0.95]。

本发明通过大量的研究以及分析发现，当遗忘因子b∈[0.7,0.95]，能够使本发明基于渐消记忆指数加权的卡尔曼滤波跟踪方法的新息残差的估计精度更高，从而使本发明方法具有更高的精确性和鲁棒性。

在本实施例中，以遗忘因子b＝0.8为例，对本发明的新息协方差的估计值的计算方法作进一步的说明。

为了使加权系数{β_i}能够满足如式(16)所示的衰减规律，令：

因为

则可将本发明的加权系数{β_i}表示为如式(19)所示：

β_i＝d_kb^k-i,i＝1,2,…,k (19)

将如式(19)所示的加权系数{β_i}代入如式(13)所示的新息协方差矩阵估计值的计算方法中，取代原加权系数1/k-1，则可以得到本发明的第k时刻基于渐消记忆指数加权新息协方差矩阵估计值的计算方法：

联立式(18)与式(20)，则有

将式(21)代入式(20)则本发明的第k时刻新息协方差的估计值的计算方法如式(22)所示：

进一步地，本发明第k-1时刻新息协方差的估计值的计算方法如式(23)所示：

联立式(22)和式(23)，则可以将本发明提出的基于渐消记忆指数加权的新息协方差估计值递推方法表示为如式(24)所示：

在实施例的实验的初始化阶段，令

以下通过计算机仿真实验，通过将本发明的基于误差修正的卡尔曼滤波方法(实施例一、二种简称为本文方法)与传统的卡尔曼滤波方法(实施例一、二中简称为KF)、传统的渐消因子卡尔曼滤波方法(实施例一、二中简称为渐消因子KF)对比分析以验证本发明方法的有效性，以及在噪声统计特性失配情况下的优越性。实施例一针对噪声统计特性匹配的情况下进行有效性分析，验证本发明方法的滤波有效性。实施例二针对噪声统计特性失配情况下进行优越性分析，验证本发明方法的优越性。

为了进行计算机仿真实验，建立了如式(25)到(28)所示的运动系统，该运动系统描述的是一个简单的振荡运动，该运动系统的状态模型和观测模型如式(25)所示：

在该运动系统进行滤波估计预测第k时刻的状态估计值的过程中，始终将观测噪声方差矩阵和状态噪声协方差矩阵设置为如式(26)所示：

R_k＝Q_k＝diag[0.01 0.01 0.01 0.01] (26)

为了便于比较，在本发明的仿真实验中，统一初始变量为X₀＝[1 0 1 0]，状态输入/输出矩阵分别如式(27)和式(28)所示：

实施例一有效性分析

为分析本文方法的有效性，首先针对噪声统计特性匹配的情况下进行了滤波估计，仿真实验中数据设置为R_k＝Q_k＝diag[0.01 0.01 0.01 0.01]，且仿真过程中也同样认为满足该设置，滤波跟踪结果如图1和图2所示。从图1和图2中可以看出，在噪声匹配的情况下，三种方法都能实现很好的估计，三种方法的整体误差均控制在0.04以内，均能满足高精度的估计需求，验证了本文方法的修正是有效的。进一步地，从图2中可以看出，本文方法和渐消因子KF方法的估计精度要略好于KF方法的估计精度。

实施例二优越性分析

为分析失配情况下本文方法估计的优越性，将实际数据的噪声参量分别放大10倍和50倍，但是实际的算法参量中还是认为噪声特性满足如式(26)的标准，噪声算法同实际环境的噪声统计特性相差10倍，其滤波跟踪结果如图3至图图6所示。

从图3和图4中可以看出，在状态噪声失配10倍的情况下，渐消因子KF方法和本文方法均保持了较高的估计精度，且估计误差绝对值均控制在0.04m以内，而KF的估计误差绝对值超出了0.04m。说明本文方法以及渐消因子HF方法能够有效改善传统KF方法因状态噪声统计特性失配问题引起的估计漂移问题。

随着状态噪声失配度的增加，达到失配50倍的情况下，从图5和图6中可以看出，传统KF方法和渐消因子KF方法均产生了较大的估计误差，最主要原因是因为这两种方法采用了相同的加窗平均的方法对新息残差矩阵进行估计，无法通过最新的观测信息对协方差估计值进行快速的修正，导致了估计结果的偏差，而本文方法因为引入了渐消记忆指数加权的新息协方差递推估计方法，可以快速的凸显最新观测信息对协方差估计的修正作用，进而能够快速的更新渐消因子λ_k，因此，能够保持较高的估计精度。

本方法针对噪声统计特性失配情况下卡尔曼滤波器精度降低及发散问题，提出了新息协方差矩阵估计值的预测方法。由于传统渐消因子卡尔曼滤波方法计算过程中主要是采用数据加窗的加权求和方法进行计算，同等对待历史信息和当前信息，然而由于在噪声匹配情况下需要加强最新观测信息的修正作用，其降低了算法的估计精度，导致渐消因子的计算误差较大。

本发明提出的新息协方差矩阵估计值的预测过程中，引入了渐消记忆指数加权的方法，通过采用负指数特性进行权重赋值，消弱历史信息的影响，增强当前观测信息的修正作用，通过实施一和实施例二验证了本发明具有较高的精度和鲁棒性，在噪声失配50倍的情况下仍然能够保持了0.04m以内的估计误差，能很好地应用在实际的工程应用中。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (8)

1.一种基于渐消记忆指数加权的卡尔曼滤波跟踪方法，所述跟踪方法采用卡尔曼滤波器进行误差预测，其特征在于，包括以下步骤：

步骤一：建立运动目标的状态模型以及观测模型，根据所述状态模型获取状态误差协方差矩阵P；

步骤二：获取第k-1时刻的状态参量X_k-1以及状态误差协方差矩阵P_k-1，根据所述第k-1时刻的状态参量X_k-1以及状态误差协方差矩阵P_k-1按照卡尔曼递推公式计算运动目标在第k时刻的预测状态参量估计值进一步计算第k时刻的新息协方差C_0,k；

步骤三：获取第k时刻的目标的观测参量Z_k，根据所述第k时刻的目标的观测参量Z_k计算第k时刻新息γ_k，进一步计算第k时刻新息协方差的估计值

步骤四：计算第k时刻的加权系数β_k，根据所述第k时刻的加权系数β_k、所述第k时刻新息γ_k以及所述第k时刻新息协方差的估计值计算第k时刻的渐消因子λ_k；

步骤五：通过所述第k时刻的渐消因子λ_k计算第k时刻的预测状态误差协方差矩阵P_k|k-1、卡尔曼增益K_k，进一步计算得到第k时刻的状态估计值以及状态误差协方差矩阵P_k；

其中，计算第k时刻新息协方差的估计值的计算方法为

其中，所述加权系数{β_i}呈负指数规律衰减。

2.根据权利要求1所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，所述加权系数{β_i}的衰减规律为

其中，b为遗忘因子，b∈[0.7,0.95]。

3.根据权利要求2所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，其特征在于，所述加权系数{β_i}表示为β_i＝d_kb^k-i,i＝1,2,…,k；

其中，

4.根据权利要求3所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，其特征在于，所述第k时刻新息协方差的估计值的计算方法为

5.根据权利要求4所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，其特征在于，所述第k时刻新息协方差的估计值的递推计算方法为

6.根据权利要求2至5任意一项所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，其特征在于，所述遗忘因子b为0.8。

7.根据权利要求6所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，其特征在于，所述状态模型为X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1；

所述观测模型为Z_k＝H_kX_k+V_k；

其中，X_k∈Rⁿ为状态参量；Z_k∈R^m为观测参量；Φ_k,k-1∈R^n×n为状态的一步转移矩阵；Γ_k-1∈R^n×p为系统过程噪声矩阵；H_k∈R^m×n观测矩阵；W_k为系统过程噪声；V_k为观测噪声。

8.根据权利要求7所述的基于渐消记忆指数加权的卡尔曼滤波跟踪方法，其特征在于，所述系统过程噪声W_k以及所述观测噪声V_k满足以下条件：

其中，Q_k表示非负的系统状态噪声方差矩阵；R_k表示正定的观测噪声方差矩阵；δ_kj为狄利克雷函数。