自适应多重渐消IEKF及其在目标跟踪中的应用

目标跟踪应用中,针对迭代扩展卡尔曼滤波(IEKF)在模型失配和噪声时变情况下出现精度下降甚至发散的问题,提出了一种基于多重渐消因子的自适应IEKF算法。该算法首先通过一个基于正态分布的限定记忆新息协方差估值器来计算新息协方差估计值,并根据估计均方误差把多重渐消因子分配给各数据通道;再依照χ 2检验原理判断系统是否异常,仅在系统异常时才引入渐消因子;最后利用目标与观测站间的径向距离和方位角信息,实现了IEKF迭代次数的自适应控制。仿真结果表明:与传统IEKF相比,在系统模型失配时所提算法的位置、速度和加速度平均估计误差分别减少86.97%、33.18%和15.56%;在过程噪声时变时则分别减少60.35%、18.42%和6.02%;在量测噪声时变时则分别减少50.60%、18.78%和5.41%。结果表明,所提算法有效提高了滤波精度,鲁棒性也进一步提升。

Abstract

To overcome the problems of accuracy degradation and divergence of the iterative extended Kalman filter (IEKF) when applying target tracking to model mismatches and noise time-variations, an adaptive IEKF algorithm based on multiple fading factors is proposed. First, a limited memory innovation covariance estimator based on the normal distribution is used to calculate the estimated value of innovation covariance and the multiple fading factors are distributed to each filtering channel according to the estimated covariance. Then, the filtering anomaly according to the χ 2 test principle is determined and the fading factors are introduced only when the system is abnormal. Finally, the radial distance and azimuth between the target and the observing station are used to determine the adaptive control of the IEKF iteration number. The simulation results show that compared with the traditional IEKF, when the system model is mismatched, the mean estimation error of position, velocity, and acceleration of the proposed algorithm is reduced by 86.97%, 33.18%, and 15.56%, respectively. When the process noise is time-varying, it is reduced by 60.35%, 18.42%, and 6.02%, respectively. When the measurement noise is time-varying, it is reduced by 50.60%, 18.78%, and 5.41%, respectively. Therefore, the proposed algorithm effectively improves filtering accuracy and robustness.

1 引言

目标跟踪的性能取决于所用滤波算法的性能,针对机动目标跟踪中的非线性估计问题,学者们先后提出了扩展卡尔曼滤波(EKF)^[1]、无迹卡尔曼滤波(UKF)^[2]和粒子滤波(PF)^[3]等算法。EKF是机动目标跟踪领域里应用最广泛的滤波算法之一,其利用一阶Taylor级数将非线性函数围绕滤波值展开,线性化的同时也容易引入截断误差^[4]。为了克服EKF算法的局限性,文献[ 5]提出一种迭代扩展卡尔曼滤波(IEKF)算法,该算法以EKF为基础,在先验统计特性已知和观测信息较为可靠的情况下,充分利用观测更新后的状态量,经迭代运算求取新的状态估计值,使之与观测量逐渐拟合,从而减小线性化误差的影响。

EKF和IEKF的估计精度都依赖于系统模型和噪声统计特性的准确性^[6],然而在实际情况中,受内外不确定因素的影响,所建立的系统模型往往不能反映真实的物理过程,噪声统计特性也易发生变化。当目标发生机动、系统模型失配或者噪声时变的情况下,滤波算法的精度容易下降甚至发散^[7]。为了提高算法的估计精度,通常有两种做法^[8]:一是建立各种自适应算法,不仅估计状态向量,而且对过程噪声和量测噪声的统计特性也进行估计;二是建立各种对一步预测均方误差阵进行加权的算法,来达到提高滤波估计精度的目的。方法一能实时估计和修正系统状态及噪声统计特性,减小估计误差,但算法的复杂度有所增加,且噪声估值器容易处于临界稳定状态,也会出现滤波发散的情况;方法二引入渐消因子对状态估计的一步预测均方误差阵进行修正,进而扩大滤波增益,等价于增大了新近量测数据的利用权重,降低了陈旧量测数据对状态估计的影响,提升了滤波器的整体性能,算法简单且鲁棒性强。所以本文选择方法二。文献[ 9]在一步预测均方误差阵中引入单渐消因子,在线调节增益矩阵,使得输出残差序列保持正交,进而达到抑制滤波发散的目的。文献[ 10]针对单渐消因子对多变量状态估计跟踪精度不足的问题,提出了多重渐消因子,分别将一步预测均方误差阵中各数据通道结合不同的渐消速率,得到了更精确的估计结果,但该算法在使用时,需要已知系统的先验知识,并人为设定渐消因子间的比例关系,实用性受到一定程度的限制。文献[ 11]利用估计均方误差阵的特征引入多重渐消因子,这是由于估计均方误差阵中的对角元素表示相应状态变量的估计精度,并且每个状态变量的估计精度是不同的,以此特点引入多重渐消因子对各数据通道分别进行加权修正,从而达到自适应调整增益矩阵及提高滤波精度的目的。由于求取渐消因子时需要一个相对准确的新息协方差估计值,新息协方差没有理论上的解析解,只能通过输出新息向量的加权和近似获得^[12]。常规的新息协方差估计值是通过开窗法^[13]来确定的,其对滑动窗口内的历史数据进行算术平均,即对各新息向量赋予相同的权重因子。文献[ 14]根据负指数函数的规律,采用渐消记忆指数加权法计算新息协方差估计值,对陈旧的数据进行遗忘,提升新近数据的利用权重,得到了更准确的新息协方差估计值,进而提高了渐消因子的计算精度。传统的渐消滤波中,判断滤波发散的阈值设置较小,误判滤波发散的概率较大,在系统正常的情况下也易引入渐消因子,文献[ 15]构造了基于新息向量统计特性的卡方检验条件,通过假设检验原理判断系统是否出现异常,仅在系统异常时才引入渐消因子,解决了过度使用渐消因子而导致滤波增益过调节的问题,状态估计也更加平滑。文献[ 16]提出了一种自适应控制迭代次数的IEKF算法,其将状态估计值与量测值之间的相对误差设定为参变量,当参变量小于预先设定的门限值时,停止迭代运算,实现了IEKF算法迭代次数的自适应控制,收敛速度和跟踪精度得到了提高。

据此,本文提出一种自适应多重渐消IEKF算法。所提算法以IEKF算法为框架,首先,通过一个基于正态分布的组合赋权法来计算新息协方差估计值,对记忆长度为N的新息协方差向量分别赋予不同的加权系数,这既满足了一定的公平性原则,也考虑了各数据自身的特殊性,获得了更准确的新息协方差估计值;然后,根据估计均方误差阵中每个数据通道具备不同调节能力这一特性,引入多重渐消因子对各数据通道进行修正,提高滤波器的跟踪精度和鲁棒性;同时基于假设检验原理判断系统是否异常,仅在系统异常时引入多重渐消因子,这解决了过度使用渐消因子而导致滤波器增益过调节的问题;最后利用目标与观测站间的径向距离和方位角信息构造参变量,并根据相对定位误差的容忍度预先设定门限值,当所构参变量小于门限值时,终止迭代,实现了IEKF迭代次数的自适应控制。通过目标跟踪的数值仿真实验,结果表明所提算法在模型失配和噪声时变的情况下,具备较好的滤波精度和鲁棒性。

2 自适应IEKF算法

2.1 IEKF算法

系统的状态方程和量测方程分别为

\begin{array}{l} X_{k} = Φ_{k / k - 1} X_{k - 1} + W_{k - 1}, (1) \\ Z_{k} = h (X_{k}) + V_{k}, (2) \end{array}

式中:X_k∈Rⁿ为状态向量;Z_k∈R^m为量测向量;Φ_k/k_-1∈Rⁿ^×ⁿ为状态转移矩阵;h(·)为关于量测的非线性函数;过程噪声W_k_-1和量测噪声V_k分别是均值为零,协方差为Q_k_-1和R_k的不相关高斯白噪声;k为当前时刻。

标准的IEKF算法 ${^{[}}^{6]}$ 为

\begin{array}{l} {\dot{X}}_{k / k - 1} = Φ_{k / k - 1} {\dot{X}}_{k - 1}, (3) \\ P_{k / k - 1} = Φ_{k / k - 1} P_{k - 1} {Φ^{T}}_{k / k - 1} + Q_{k - 1}, (4) \\ K_{k}^{i} = P_{k / k - 1} (H_{k}^{i})^{T} {[H_{k}^{i} P_{k / k - 1} (H_{k}^{i})^{T} + R_{k}]}^{- 1}, (5) \\ ε_{k}^{i} = Z_{k} - h_{k} ({\dot{X}}_{k}^{i}), (6) \\ {\dot{X}}_{k}^{i + 1} = {\dot{X}}_{k / k - 1} + K_{k}^{i} [ε_{k}^{i} - H_{k}^{i} ({\dot{X}}_{k / k - 1} - {\dot{X}}_{k}^{i})], (7) \\ P_{k}^{i + 1} = (I - K_{k}^{i} H_{k}^{i}) P_{k / k - 1}, (8) \end{array}

式中: ${\dot{X}}_{k / k - 1}$ 为一步预测状态估计;P_k/k_-1为一步预测均方误差阵;K_k为增益矩阵;ε_k为新息向量; ${\dot{X}}_{k}$ 为状态估计值;P_k为估计均方误差阵; $H_{k}^{i}$ = ${\frac{\partial h}{\partial X}|}_{X = {\dot{X}}_{k}^{i}}$ ;i为迭代次数; ${\dot{X}}_{k}^{1}$ = ${\dot{X}}_{k / k - 1}$ ; I为单位矩阵。

IEKF算法的基本思想是,在滤波算法的测量更新环节进行多次迭代,多次对量测方程进行线性化处理,重复这一过程,使得状态估计误差进一步减小,从而达到提高滤波精度的目的。

2.2 自适应IEKF算法

由于传统IEKF算法需要人工设定迭代次数,无法根据工程实际自适应调整迭代次数,且随着迭代次数的增加,IEKF算法的计算负担也会加重。为了解决这个问题,利用目标与观测站之间的径向距离和方位角信息构造参变量α,并预先设定使IEKF算法终止迭代的门限值α₀。在IEKF滤波的每次测量更新后,对比参变量与门限值之间的大小,当参变量α小于门限值α₀时,终止迭代,实现了IEKF算法迭代次数的自适应控制。

IEKF算法的思想是进行多次迭代,使得状态估计值与目标真实状态值之间的偏差逐渐减小,进而提高非线性滤波精度,故参变量α的设置应与状态估计偏差呈正相关。但在实际情况中,目标的真实状态值又难以获取,故转为求解状态估计值与量测值之间的偏差,为此将状态估计值转换为目标与观测站之间的径向距离和方位角信息,即

\begin{array}{l} h (X_{k}) = [\sqrt[]{(x_{k} - x_{0})^{2} + (y_{k} - y_{0})^{2}}, \\ \arctan (\frac{y_{k} - y_{0}}{x_{k} - x_{0}})]^{T} 。 (9) \end{array}

参变量α代数式^[16]为

α_{k}^{i} = 20 π \times \frac{{(h_{k} ({\dot{X}}_{k}^{i}) - Z_{k})}_{2}}{{(Z_{k})}_{2}}, (10)

式中: $α_{k}^{i}$ 为第k次观测,迭代i次得到的参变量; ${(\cdot)}_{2}$ 为矩阵的二范数。门限值α₀的设置与可容忍的相对定位偏差有关,其相对偏差如表1所示^[16]。

表 1. 相对定位偏差与门限值的对应关系^[16]

Table 1. Correspondence between relative positioning error and threshold^[16]

Relative positioning error /%	0.5	0.4	0.3	0.2	0.1
Threshold	0.31	0.25	0.19	0.13	0.06

查看所有表

首先进行常规IEKF滤波并实时计算参变量 $α_{k}^{i}$ ,当 $α_{k}^{i}$ 小于设定的门限值α₀时,终止迭代,由此实现了IEKF迭代次数的自适应控制。

3 基于多重渐消因子的自适应IEKF算法

IEKF算法迭代过程中,状态估计值 ${\dot{X}}_{k}$ 的每次修正都是通过量测值来完成的,收敛精度必然依赖于观测精度,所以IEKF算法对量测误差敏感。而在实际情况中,噪声的先验统计特性也是不准确的,这可能会降低跟踪精度。为了解决这个问题,在IEKF的基础上引入多重渐消因子,以增强滤波器对过程参数变化的鲁棒性和跟踪能力。

3.1 单渐消因子滤波

传统渐消滤波在(4)式处,即在一步预测均方误差阵中加入单渐消因子λ_k,得到^[10]

P_{k / k - 1} = λ_{k} Φ_{k / k - 1} P_{k - 1} {Φ^{T}}_{k / k - 1} + Q_{k - 1} 。 (11)

由(11)式可知,当λ_k>1时,P_k/k_-1增大,进而由(5)式知,增益K_k亦增大。这等价于当前量测值的利用率增加,陈旧量测值的利用率相对降低,即降低了陈旧量测值对状态估计 ${\dot{X}}_{k}$ 的影响。

渐消因子λ_k由基于新息向量的统计特性确定。当滤波处于最优状态时,新息向量ε_k为高斯白噪声,且服从N $(0, H_{k} P_{k / k - 1} {H^{T}}_{k} + R_{k})$ 分布。则k时刻的新息协方差理论值为

C_{k} = E (ε_{k} {ε^{T}}_{k}) = H_{k} P_{k / k - 1} {H^{T}}_{k} + R_{k} 。 (12)

将(11)式代入(12)式,可得

\begin{array}{l} C_{k} = H_{k} (λ_{k} Φ_{k / k - 1} P_{k - 1} {Φ^{T}}_{k / k - 1} + Q_{k - 1}) {H^{T}}_{k} + R_{k} = \\ H_{k} λ_{k} Φ_{k / k - 1} P_{k - 1} {Φ^{T}}_{k / k - 1} {H^{T}}_{k} + H_{k} Q_{k - 1} {H^{T}}_{k} + R_{k} 。 (13) \end{array}

但需要说明的是,新息协方差C_k没有理论上的解析解,只能通过输出新息向量的加权和近似获得^[12]。通常由开窗法得到^[13]

{\dot{C}}_{k} = \frac{1}{N} \overset{N - 1}{\sum_{u = 0}} ε_{k - u} {ε^{T}}_{k - u} 。 (14)

结合(11)~(14)式,即构成单渐消因子λ_k的求取方法,即

\{\begin{array}{l} N_{k} = H_{k} Q_{k - 1} {H^{T}}_{k} + R_{k} \\ M_{k} = H_{k} Φ_{k / k - 1} P_{k - 1} {Φ^{T}}_{k / k - 1} {H^{T}}_{k} \\ λ_{k} = \max \{1, tr ({\dot{C}}_{k} - N_{k}) / tr (M_{k})\} \end{array}, (15)

式中:tr(·)为矩阵求迹运算; ${\dot{C}}_{k}$ 为新息协方差估计值。λ_k的下界为1,是从收敛性方面考虑的。

由(15)式可知,求取渐消因子需要一个相对准确的新息协方差估计值 ${\dot{C}}_{k}$ ,从统计观点来看,传统的开窗法得到的 ${\dot{C}}_{k}$ 是对滑动窗口内的所有数据进行算术平均得到的,虽然有一定的公平性,但没有很好地体现出各数据自身的特殊性。

3.2 基于正态分布的限定记忆新息协方差估值器

正态分布是统计学和概率论中最重要的分布之一,且在现实世界里,有许多物理量的分布规律服从正态分布或近似正态分布,文献[ 17]甚至指出实际问题中几乎所有的连续变量都大似呈正态分布。因此,为了综合利用专家经验和客观数据规律,改善新息协方差估计值的求取精度,提出了一种基于正态分布的组合赋权法来计算新息协方差估计值,在形式上表现为对记忆长度为N的新息协方差向量进行加权平均,即近N个时刻内的新息协方差向量分别乘以不同的加权系数,从而得到当前时刻的新息协方差估计值,这样既满足一定的公平性原则,也体现各数据自身的特殊性。

定义1 若随机变量x服从一个均值为μ、标准差为σ的概率分布,其概率密度函数^[18]为

f (x) = \frac{1}{\sqrt[]{2 π σ^{2}}} \cdot \exp [\frac{- {(x - μ)}^{2}}{2 σ^{2}}], (16)

则这个随机变量服从正态分布。

考察限定记忆长度为N的新息协方差向量,并对近N个时刻内的新息协方差向量分别赋予不同的加权系数w_g(g=1,2,…,N)。

依据(16)式可得

\begin{array}{l} η_{g} = \frac{1}{\sqrt[]{2 π σ^{2}}} \cdot \exp [\frac{- {(g - μ)}^{2}}{2 σ^{2}}], (17) \\ \{\begin{array}{l} μ = (1 + N) / 2 \\ σ = \sqrt[]{\frac{1}{N} \overset{N}{\sum_{g = 1}} {(g - μ)}^{2}} \end{array} 。 (18) \end{array}

由于加权系数w_g∈[0,1],且 $\overset{N}{\sum_{g = 1}} w_{g} = 1$ ,对(17)式进行单位化处理,便得到加权系数w_g的表达式:

w_{g} = η_{g} / \overset{N}{\sum_{g = 1}} η_{g} 。 (19)

那么,k时刻的新息协方差估计值 ${\dot{C}}_{k}$ 可表示为

{\dot{C}}_{k} = w_{1} ε_{k} {ε^{T}}_{k} + w_{2} ε_{k - 1} {ε^{T}}_{k - 1} + \dots + w_{N} ε_{k - N + 1} {ε^{T}}_{k - N + 1} 。 (20)

将(20)式代入(15)式,由一个基于正态分布的限定记忆新息协方差估值器来计算渐消因子,方法简单,便于计算机实现。

3.3 多重渐消因子滤波

单渐消因子λ_k对P_k/k_-1各数据通道的调节能力相同,若在(4)式中引入多重渐消因子,各数据通道以不同速率渐消,则能得到更高性能的滤波器。由于估计均方误差阵P_k中的对角元素表示相应状态变量的估计精度,并且每个状态变量的估计精度不同,所以根据均方误差阵的这一特性引入多重渐消因子S_k对各数据通道进行修正,使每个数据通道具备不同的渐消能力,最终提高滤波器的跟踪精度和鲁棒性。则(4)式可写为

P_{k / k - 1} = S_{k} Φ_{k / k - 1} P_{k - 1} {Φ^{T}}_{k / k - 1} + Q_{k - 1}, (21)

式中:S_k=diag $\{s_{1} s_{2} \dots s_{j} \dots s_{n}\}$ ,s_j表示第j个状态变量对应的渐消因子,j=1,2,…,n。s_j满足

s_{j} = c_{j} \cdot λ_{j}, (22)

式中:λ_j为(15)式得到的渐消因子。c_j为给渐消因子λ_j分配的权重因子,满足^[11]

c_{j} = \{\begin{array}{l} 1, & λ_{j} = 1 \\ \max [1, P_{k} (j, j) / \frac{1}{n} \overset{n}{\sum_{j = 1}} P_{k} (j, j)], & λ_{j} > 1 \end{array}, (23)

式中:P_k(j,j)为估计均方误差阵的第j个对角线元素,亦表示第j个状态变量的估计精度。

3.4 基于χ²检验的渐消滤波

文献[ 12]证明,传统的强跟踪滤波中,判断滤波发散的阈值设置过小,即便系统处于正常工作状态,也有远大于16%的概率判定系统异常,此时引入渐消因子会导致对滤波增益过度调节,也会使状态估计不够平滑。

针对传统算法对滤波发散的判断不够准确,从而导致渐消因子的引入时机不够合理的问题,文献[ 15]提出了一种基于新息向量统计特性的滤波状态χ²检验条件,只有在判定系统异常时,才引入渐消因子。

当滤波处于最优状态时,新息向量的统计特性满足^[15]

\{\begin{array}{l} ε_{k} ~N (0, H_{k} P_{k / k - 1} {H^{T}}_{k} + R_{k}) \\ γ_{k} = {ε^{T}}_{k} {(H_{k} P_{k / k - 1} {H^{T}}_{k} + R_{k})}^{- 1} ε_{k} ~ χ^{2} (m) \end{array}, (24)

式中:γ_k为服从自由度为m的χ²分布;m为量测维数。所以可利用χ²分布上侧分位点的性质来判定系统是否异常,当量测维数m=2时,可选取分位点ζ=9.210,即^[19]

P \{χ^{2} (2) > ζ\} = 1 % 。 (25)

(25)式表明,在滤波正常工作的情况下,γ_k>ζ的概率只有1%。由假设检验原理可知,若γ_k>ζ,则在99%的置信度下可判定为系统异常,故以此作为渐消因子的引入条件。首先进行常规IEKF滤波并实时计算γ_k,若满足γ_k>ζ的检验条件,则引入渐消因子。

3.5 算法流程

至此,基于多重渐消因子的自适应IEKF算法流程为

①初始化状态估计值 ${\dot{X}}_{0}$ 和估计均方误差阵P₀;

②由(3)、(4)式完成时间更新环节,求得一步预测状态估计值 ${\dot{X}}_{k / k - 1}$ 和一步预测均方误差阵P_k/k_-1;

③按照(24)、(25)式判断系统是否异常,若系统异常则引入多重渐消因子,若系统正常,则直接进行步骤⑥;

④由(20)式计算新息协方差估计值;

⑤由(15)~(23)式得到多重渐消因子,并代入到P_k/k_-1中;

⑥由(5)~(8)式完成测量更新环节,求得状态估计值 ${\dot{X}}_{k}^{i + 1}$ 和估计均方误差阵 $P_{k}^{i + 1}$ ;

⑦按照(9)、(10)式进行IEKF算法迭代次数判据,若参变量 $α_{k}^{i}$ 大于门限值α₀,则返回步骤③~⑥再次进行测量更新,若 $α_{k}^{i}$ 小于α₀,则终止迭代。

4 数值仿真及分析

为验证所提算法的有效性,根据文献[ 20]来设定实验中的数据,这些数据是完整可靠的,具备一定的说服力,并对文献[ 20]中的算例分三种情况进行仿真。一是模型失配情况下的仿真,二是过程噪声协方差不匹配情况下的仿真,三是量测噪声协方差不匹配情况下的仿真。

设定坐标位置为(x₀,y₀)的雷达对目标M进行跟踪,目标M在某一时刻k的位置、速度和加速度用矢量X_k= $[x_{k}, y_{k}, {\overset{..}{x}}_{k}, {\overset{..}{y}}_{k}, {\overset{...}{x}}_{k}, {\overset{...}{y}}_{k}]^{T}$ 表示,同时假设目标M在x-y两个方向上运动都有加性过程噪声W_k,目标运动状态方程如(1)式所示,式中Φ_k/k_-1= $[\begin{array}{l} I_{2 \times 2} & Δ t I_{2 \times 2} & 0.5 Δ t^{2} I_{2 \times 2} \\ 0_{2 \times 2} & I_{2 \times 2} & Δ t I_{2 \times 2} \\ 0_{2 \times 2} & 0_{2 \times 2} & I_{2 \times 2} \end{array}]$ ,采样周期Δt=0.5 s;过程噪声W_k服从N(0,Q_k)分布,且过程噪声协方差Q_k= $[\begin{array}{l} I_{2 \times 2} & 0_{2 \times 2} & 0_{2 \times 2} \\ 0_{2 \times 2} & 0 . 1^{2} I_{2 \times 2} & 0_{2 \times 2} \\ 0_{2 \times 2} & 0_{2 \times 2} & 0 . 01^{2} I_{2 \times 2} \end{array}]$ 。观测量为目标与雷达站的径向距离和方位角,实际测量中雷达具有加性量测噪声V_k~N(0,R_k),且量测噪声协方差R_k= $[\begin{array}{l} 5^{2} & 0 \\ 0 & 0 . 001^{2} \end{array}]$ 。以雷达为中心建立笛卡儿坐标系,其观测方程如(2)式所示。目标初始状态值X₀=[100 m, 50 m, 5 m/s, 2 m/s, 0.2 m/s², 0.1 m/s²]^T;估计均方误差阵P₀= $[\begin{array}{l} 5 I_{2 \times 2} & 0_{2 \times 2} & 0_{2 \times 2} \\ 0_{2 \times 2} & I_{2 \times 2} & 0_{2 \times 2} \\ 0_{2 \times 2} & 0_{2 \times 2} & 0.1 I_{2 \times 2} \end{array}]$ ;参变量门限值α₀取0.25;滑动窗口长度N取5;仿真进行100次蒙特卡罗实验,结果取平均值。

目标跟踪的有效性通过算法的均方根误差(RMSE)和采样时间内平均估计误差(ME)来衡量,RMSE越小,表明状态估计值和真实值越接近,滤波算法的跟踪精度越高。

\begin{array}{l} E_{RMSE, k} = {\{\frac{1}{\overset{̅}{M}} \overset{\overset{̅}{M}}{\sum_{l = 1}} {(X_{r, k}^{l} - {\dot{X}}_{r, k}^{l})}_{2}^{2}\}}^{1 / 2}, (26) \\ E_{ME} = \frac{1}{T} \overset{T}{\sum_{k = 1}} E_{RMSE, k}, (27) \end{array}

式中: $X_{r, k}^{l}$ 和 ${\dot{X}}_{r, k}^{l}$ 分别为第l次蒙特卡罗仿真过程中第k时刻目标状态向量的第r个分量的真实值和估计值; $\overset{̅}{M}$ 为蒙特卡罗仿真次数;T为仿真步数。

4.1 模型失配情况下的跟踪结果分析

设计滤波器时误以为目标高度不变,因而建立模型失配情况下的状态方程和量测方程:

\begin{array}{l} X_{k} = F_{k / k - 1} X_{k - 1} + W_{k - 1}, (28) \\ Z_{k} = h (X_{k}) + V_{k}, (29) \end{array}

式中:F_k/k_-1= $[\begin{array}{l} 1 & 0 & Δ t & 0 & 0.5 Δ t^{2} & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & Δ t & 0 \\ 0 & 0 & 0 & 1 & 0 & Δ t \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array}]$ 。

在模型失配时,分别采用传统IEKF算法、文献[ 16]提出的自适应IEKF算法(AIEKF)、文献[ 11]提出的多重渐消因子的IEKF算法(MFIEKF)、所提算法进行仿真对比实验,各种滤波算法的位置、速度和加速度RMSE对比如图1所示。

图 1. 模型失配时的跟踪结果。(a)位置RMSE;(b)速度RMSE;(c)加速度RMSE

Fig. 1. Tracking results under model mismatch. (a) RMSE of position; (b) RMSE of velocity; (c) RMSE of acceleration

下载图片查看所有图片

由图1(a)可知,当系统模型失配时,传统IEKF的滤波结果是发散的,而AIEKF、MFIEKF和所提算法在跟踪过程中表现出较好的平稳性,且所提算法在跟踪过程中的波动幅度最小,能保持良好的滤波效果;由图1(b)可知,在滤波初始阶段,各算法的速度RMSE均逐渐增大,其中所提算法的速度RMSE增幅最小,10 s后各算法的速度RMSE逐渐减小,并在0.6 m/s左右波动,最终所提算法的速度RMSE小于其他算法;由图1(c)可知,在滤波初始阶段,各算法的加速度RMSE逐渐增大,8 s后各算法的加速度RMSE逐渐减小,所提算法在跟踪过程中出现了最小的增幅且跟踪较平稳。这是因为当模型失配时,传统IEKF无法自适应调整迭代次数,也不具备强跟踪能力,故其估计精度最低;AIEKF在模型失配时能自适应调整迭代次数,并通过多次迭代运算,使状态估计值与量测值逐渐吻合,所以得到了较好的滤波结果;MFIEKF具备强跟踪能力,引入了多重渐消因子对预测均方误差阵进行修正,对过程参数的变化具备一定的鲁棒性,所以能在滤波过程中保持较好的平稳性;所提算法通过基于正态分布的限定记忆新息协方差估值器提高了渐消因子的求取精度,并根据χ²检验条件使渐消因子的引入时机更加合理,也能自适应调整IEKF算法的迭代次数,因而在模型失配的情况下状态估计误差最小,跟踪较平稳。另外,由于本实验设定为误以为目标高度不变,因此传统IEKF在位置估计上是发散的,又因为滤波算法对各个状态向量是独立估计的,且本实验设定的模型在速度和加速度方面是准确的,所以传统IEKF在速度和加速度估计上最终能表现出和其他滤波算法相当的精度。表2为4种算法的ME和运行时间对比。

表 2. 模型失配时不同算法的ME和运行时间

Table 2. ME and runtime of different algorithms under model mismatch

Algorithm	ME			Runtime /s
Algorithm	Position /m	Velocity /(m·s^-1)	Acceleration /(m·s^-2)	Runtime /s
IEKF	22.5528	0.8117	0.0726	0.0154
AIEKF	4.2763	0.6776	0.0672	0.1105
MFIEKF	5.4990	0.7488	0.0704	0.0781
Proposed algorithm	2.9384	0.5424	0.0613	0.0942

查看所有表

由表2可知:在系统模型失配时,传统IEKF的误差最大,说明传统IEKF不具备应对模型失配的能力;AIEKF、MFIEKF和所提算法都表现出较好的鲁棒性;相比于AIEKF,所提算法在位置、速度和加速度ME上分别减小了31.29%、19.95%和8.78%,平均耗时减少14.75%;相比于MFIEKF,所提算法在位置、速度和加速度ME上分别减小了46.56%、27.56%和13.35%,平均耗时增加20.61%;所提算法的平均耗时虽然比MFIEKF有所增加,但估计精度更高,具备良好的自适应能力和滤波精度。

4.2 过程噪声协方差不匹配情况下的跟踪结果分析

过程噪声协方差不匹配的情况一般由元器件的误差产生,而元器件误差容易受到温度和湿度等外部条件的影响,因而过程噪声协方差是时变的。本实验设定真实的过程噪声协方差阵在20~22.5 s扩大5倍,即为5Q,而滤波算法中设定的过程噪声协方差不变,以检验各滤波算法在过程噪声协方差不匹配时的跟踪能力。仿真结果如图2所示。

图 2. 过程噪声协方差不匹配时的跟踪结果。(a)位置RMSE;(b)速度RMSE;(c)加速度RMSE

Fig. 2. Tracking results under process noise covariance mismatch. (a) RMSE of position; (b) RMSE of velocity; (c) RMSE of acceleration

下载图片查看所有图片

由图2(a)可知:4种滤波算法都收敛,在整个滤波过程中,传统IEKF的估计精度最低,MFIEKF次之,AIEKF稍好,所提算法最好;当过程噪声协方差不匹配时,传统IEKF出现大幅度的波动,AIEKF和MFIEKF出现较大幅度的波动,所提算法的峰值误差最小。最终,所提算法收敛精度更高,具有良好的自适应能力和鲁棒性。

由图2(b)可知:传统IEKF波动最大,且在噪声发生变化时,速度RMSE迅速增大,这是因为传统IEKF不具备强跟踪能力,其应对噪声突变的鲁棒性较差;AIEKF在过程噪声发生变化时,也出现了明显的波动,但收敛速度较快,这是由于AIEKF能根据量测信息对状态估计进行迭代调整,故滤波精度优于传统IEKF;MFIEKF在20 s之前,滤波精度略优于传统IEKF,当过程噪声协方差发生变化时,MFIEKF能比传统IEKF更快收敛,且收敛精度更高,这是由于MFIEKF在传统IEKF的基础上引入了多重渐消因子,其对噪声变化有着更好的自适应能力;在整个滤波过程中,所提算法的估计精度较高于其他算法,且在过程噪声发生变化时,出现较小的峰值误差,并较快收敛,这是由于所提算法通过一个基于正态分布的限定记忆新息协方差估值器提升了多重渐消因子的计算精度,且渐消因子的引入时机更加合理。

由图2(c)可知:在初始阶段各滤波算法的加速度RMSE相差不大;10 s过后,各滤波算法的加速度RMSE开始减小;到20~22.5 s,由于过程噪声协方差发生突变,各滤波算法加速度RMSE再次增大,所提算法的波动幅度较小,且收敛较快;25 s过后,各滤波算法的加速度RMSE逐渐减小。表3为4种算法的ME和运行时间对比。

表 3. 过程噪声协方差不匹配时不同算法的ME及运行时间

Table 3. ME and runtime of different algorithms under process noise covariance mismatch

Algorithm	ME			Runtime /s
Algorithm	Position /m	Velocity /(m·s^-1)	Acceleration /(m·s^-2)	Runtime /s
IEKF	4.0043	0.7507	0.0698	0.0623
AIEKF	2.0518	0.6592	0.0690	0.1711
MFIEKF	2.5182	0.7087	0.0694	0.0931
Proposed algorithm	1.5876	0.6124	0.0656	0.1732

查看所有表

由表3可知:AIEKF、MFIEKF和所提算法在估计精度方面都较传统IEKF有所提高;所提算法在位置、速度和加速度上的估计精度均优于其他对比算法,平均耗时上虽然高于其他算法,但较大提高了滤波精度,故以运行时间为代价换取滤波精度的提高是可取的。

4.3 量测噪声协方差不匹配情况下的跟踪结果分析

量测噪声由于易受外界因素的干扰,其统计特性常常不是定值,而随时间发生变化,因而量测噪声的统计特性也是时变的。本实验设定真实的量测噪声协方差阵在20~22.5 s扩大10倍,即为10R,而滤波算法中设定的量测噪声协方差不变,以检验各滤波算法在量测噪声统计特性不匹配时的跟踪能力。仿真结果如图3所示。

图 3. 量测噪声协方差不匹配时的跟踪结果。(a)位置RMSE;(b)速度RMSE;(c)加速度RMSE

Fig. 3. Tracking results under measurement noise covariance mismatch. (a) RMSE of position; (b) RMSE of velocity; (c) RMSE of acceleration

下载图片查看所有图片

由图3(a)可知:在整个滤波过程中,所提算法的位置RMSE最小,且在量测噪声发生变化时,所提算法的峰值误差小于其他算法,并能较快收敛,证明了基于多重渐消因子的自适应IEKF算法在抗干扰能力上的优势;与传统IEKF和AIEKF相比,所提算法滤波精度更高,这是由于引入了多重渐消因子,增强了算法的抗干扰能力,也提高了滤波精度;与MFIEKF相比,所提算法通过一个基于正态分布的限定记忆新息协方差估值器计算新息协方差估计值,得到了更准确的渐消因子,同时通过χ²检验进行系统稳定性检测,在更合理的时机引入渐消因子,避免了对增益过矫正,所以滤波精度有所提升。由图3(b)可知:在滤波初始阶段,各滤波算法的速度RMSE相差不大;10 s以后,所提算法的速度RMSE逐渐小于其他3种算法;至20~22.5 s时,由于量测噪声发生变化,各滤波算法发生不同幅度的波动,其中所提算法出现较小的波动且过渡时间短。结果表明,所提算法能较好地应对突变状态,具有较好的跟踪能力。由图3(c)可知:在滤波初始阶段,各滤波算法的加速度RMSE相差不大;12 s之后,各滤波算法的加速度RMSE逐渐减小;到20~22.5 s,由于量测噪声协方差发生突变,各滤波算法出现不同幅度大小的波动,其中所提算法的波动幅度最小,之后各滤波算法的加速度RMSE逐渐减小。表4为4种算法的ME和运行时间对比。

表 4. 量测噪声协方差不匹配时不同算法的ME及运行时间

Table 4. ME and runtime of different algorithms under measurement noise covariance mismatch

Algorithm	ME			Runtime /s
Algorithm	Position /m	Velocity /(m·s^-1)	Acceleration /(m·s^-2)	Runtime /s
IEKF	3.2746	0.6443	0.0629	0.0648
AIEKF	2.2893	0.5758	0.0617	0.1561
MFIEKF	2.6930	0.6045	0.0622	0.0943
Proposed algorithm	1.6175	0.5233	0.0595	0.1557

查看所有表

由表4可知:所提算法在位置、速度和加速度上的估计精度均优于其他算法,平均运行时间虽高于传统IEKF和MFIEKF,但位置估计精度得到明显提高,且平均运行时间略小于AIEKF。所提算法在AIEKF的基础上引入多重渐消因子,提高了估计精度,算法效率符合预期,体现了较好的滤波精度和鲁棒性,这在目标跟踪中尤为重要。

5 结论

为提高IEKF滤波器在模型失配和噪声时变情况下的滤波精度和鲁棒性,提出了一种基于多重渐消因子的自适应IEKF滤波算法。该算法通过一个基于正态分布的限定记忆新息协方差估值器获得更准确的新息协方差估计值,进而提高了多重渐消因子的求取精度,又利用径向距离和方位角信息设定参变量,对IEKF算法的迭代次数实现自适应控制。

将所提算法应用到目标跟踪的数值仿真实验。实验结果表明,所提算法的位置、速度和加速度平均估计误差在系统模型失配时分别为2.94 m、0.54 m/s和0.06 m/s²;在过程噪声时变时分别为1.59 m、0.61 m/s和0.07 m/s²;在量测噪声时变时分别为1.62 m、0.52 m/s和0.06 m/s²,具备较好的滤波精度。

参考文献

[1] 穆绍硕, 乔川. 小型机载光电平台中基于扩展卡尔曼滤波的地面目标定位算法[J]. 光学学报, 2019, 39(5): 0528001.

Mu S S, Qiao C. Ground-target geo-location method based on extended Kalman filtering for small-scale airborne electro-optical platform[J]. Acta Optica Sinica, 2019, 39(5): 0528001.

[2] 李珂, 李醒飞, 杨帆. IMM-UKF算法在两坐标雷达-光电融合跟踪系统中的改进与应用[J]. 激光与光电子学进展, 2016, 53(12): 122501.

Li K, Li X F, Yang F. Improvement and application of IMM-UKF algorithm in two coordinate radar-optoelectronic fusion tracking system[J]. Laser & Optoelectronics Progress, 2016, 53(12): 122501.

[3] 郭力仁, 胡以华, 王云鹏, 等. 基于粒子滤波的高阶运动目标激光探测微动参数估计[J]. 光学学报, 2018, 38(9): 0912006.

Guo L R, Hu Y H, Wang Y P, et al. Laser micro-motion parameter estimation of high-order moving target based on improved particle filter[J]. Acta Optica Sinica, 2018, 38(9): 0912006.

[4] 张天喜, 周军, 廖华丽, 等. 基于三维激光的图优化即时定位与建图策略[J]. 激光与光电子学进展, 2019, 56(20): 201502.

Zhang T X, Zhou J, Liao H L, et al. Simultaneous localization and mapping strategy of graph optimization based on three-dimensional laser[J]. Laser & Optoelectronics Progress, 2019, 56(20): 201502.

[5] Li LQ, Ji HB, Luo JH. The iterated extended Kalman particle filter[C] //IEEE International Symposium on Communicationsand Information Technology, October 12-14, 2005, Beijing, China.New York: IEEE Press, 2005: 1213- 1216.

[6] Xu Y, Chen X, Li Q. Adaptive iterated extended Kalman filter and its application to autonomous integrated navigation for indoor robot[J]. The Scientific World Journal, 2014, 2014: 138548.

[7] Xu D J, Wu Z M, Huang Y L. A new adaptive Kalman filter with inaccurate noise statistics[J]. Circuits, Systems, and Signal Processing, 2019, 38(9): 4380-4404.

[8] Huang Y L, Zhang Y G, Wu Z M, et al. A novel adaptive Kalman filter with inaccurate process and measurement noise covariance matrices[J]. IEEE Transactions on Automatic Control, 2018, 63(2): 594-601.

[9] Liang Y P, Dai J H, Wang K Q, et al. A strong tracking SLAM algorithm based on the suboptimal fading factor[J]. Journal of Sensors, 2018, 2018: 1-14.

[10] 周东华, 席裕庚, 张钟俊. 一种带多重次优渐消因子的扩展卡尔曼滤波器[J]. 自动化学报, 1991, 17(6): 689-695, 758.

Zhou D H, Xi Y G, Zhang Z J. A suboptimal multiple fading extended Kalman filter[J]. Acta Automatica Sinica, 1991, 17(6): 689-695, 758.

[11] 高伟, 李敬春, 奔粤阳, 等. 基于多重渐消因子的自适应卡尔曼滤波器[J]. 系统工程与电子技术, 2014, 36(7): 1405-1409.

Gao W, Li J C, Ben Y Y, et al. Adaptive Kalman filter based on multiple fading factors[J]. Systems Engineering and Electronics, 2014, 36(7): 1405-1409.

[12] 钱华明, 葛磊, 黄蔚. 一种改进的强跟踪滤波算法[J]. 应用科学学报, 2015, 33(1): 32-40.

Qian H M, Ge L, Huang W. An improved strong tracking filtering algorithm[J]. Journal of Applied Sciences, 2015, 33(1): 32-40.

[13] Gao W, Li J C, Zhou G T, et al. Adaptive Kalman filtering with recursive noise estimator for integrated SINS/DVL systems[J]. Journal of Navigation, 2015, 68(1): 142-161.

[14] 薛海建, 郭晓松, 周召发. 基于自适应多重渐消因子卡尔曼滤波的SINS初始对准方法[J]. 系统工程与电子技术, 2017, 39(3): 620-626.

Xue H J, Guo X S, Zhou Z F. SINS initial alignment method based on adaptive multiple fading factors Kalman filter[J]. Systems Engineering and Electronics, 2017, 39(3): 620-626.

[15] 郭士荦, 吴苗, 许江宁, 等. 自适应渐消卡尔曼滤波及其在SINS初始对准中的应用[J]. 武汉大学学报·信息科学版, 2018, 43(11): 1667-1672, 1680.

Guo S L, Wu M, Xu J N, et al. Adaptive fading Kalman filter and its application in SINS initial alignment[J]. Geomatics and Information Science of Wuhan University, 2018, 43(11): 1667-1672, 1680.

[16] 关欣, 吕政君, 许小丰. 基于外辐射源的AIEKF定位算法[J]. 火力与指挥控制, 2018, 43(6): 37-40, 46.

Guan X, Lü Z J, Xu X F. Location algorithm of AIEKF based on non-cooperative illuminator[J]. Fire Control & Command Control, 2018, 43(6): 37-40, 46.

[17] Jøsang A. A logic for uncertain probabilities[J]. International Journal of Uncertainty, Fuzziness and Knowledge-Based Systems, 2001, 9(3): 279-311.

[18] Picinbono B. Second-order complex random vectors and normal distributions[J]. IEEE Transactions on Signal Processing, 1996, 44(10): 2637-2640.

[19] Chang G B. Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion[J]. Journal of Geodesy, 2014, 88(4): 391-401.

[20] 黄小平, 王岩. 卡尔曼滤波原理及应用[M]. 北京: 电子工业出版社, 2015: 77- 118.

Huang XP, WangY. Kalman filtering principle and application[M]. Beijing: Publishing House of Electronics Industry, 2015: 77- 118.

3.2 基于正态分布的限定记忆新息协方差估值器

4.2 过程噪声协方差不匹配情况下的跟踪结果分析

4.3 量测噪声协方差不匹配情况下的跟踪结果分析

5 结论

严春满, 吴松伦, 胡志斌. 自适应多重渐消IEKF及其在目标跟踪中的应用[J]. 激光与光电子学进展, 2021, 58(6): 0628005. Yan Chunman, Wu Songlun, Hu Zhibin. Adaptive Multiple Fading IEKF and its Application in Target Tracking[J]. Laser & Optoelectronics Progress, 2021, 58(6): 0628005.