Multi-target tracking based on improved probability hypothesis density filter
-
摘要: 经典序贯蒙特卡罗概率假设密度(Sequential Mote Carlo Probability Hypothesis Density, SMC-PHD)滤波中, 将目标状态转移密度函数做为建议密度函数, 没有利用当前观测信息, 导致大部分预测粒子状态偏离目标真实状态, 粒子退化严重.针对上述问题, 提出利用均方根容积卡尔曼滤波产生建议密度函数, 对其进行采样得到预测粒子状态, 该方法有严格理论基础, 能有效减轻SMC-PHD滤波中的粒子退化, 且适用性很强.仿真实验对比了该算法、经典SMC-PHD和基于无迹卡尔曼的SMC-PHD算法的跟踪性能, 验证了该方法无论对势估计还是对目标状态估计的精度都优于其他两种算法.
-
关键词:
- 多目标跟踪 /
- 概率假设密度滤波 /
- 序贯蒙特卡罗 /
- 建议密度函数 /
- 均方根容积卡尔曼滤波
Abstract: Due to the most recent observational data being unused, the particles in sequential Mote Carlo probability hypothesis density (SMC-PHD) filter which are drawn from prior transition is far away from the real states and may seriously degenerate. Aiming at these problems, we propose a method named square-rooted cubature Kalman sequential Mote Carlo PHD (SCK-SMC-PHD) filter which uses square-rooted cubature Kalman filter to generate the proposal density function and obtains the present particles states by sampling from the proposal density function. The proposed method which can alleviate particles degradation effectively has rigorous mathematical theoretical basis and strong adaptability. Simulation compares the proposed method with C-SMC-PHD filter and the SMC-PHD based on unscented Kalman filter. The results show that the proposed SCK-SMC-PHD filter has a higher accuracy in estimation of both individual state and target number than the two methods mentioned above. -
下很难得到多目标全局后验概率密度, 因而Mahler提出利用多目标全局后验概率密度的一阶矩代替其本身在多目标贝叶斯递推式中进行传递, 这就是概率假设密度(Probability Hypothesis Density, PHD)滤波[2].PHD滤波通过一阶矩近似, 降低了多目标贝叶斯滤波的计算复杂度, 其序贯蒙特卡罗(Sequential Mote Carlo, SMC)实现形式[3]可在非线性非高斯条件下使用, 但由于其基于序贯重要性采样原理, 因而SMC-PHD滤波具有和粒子滤波(Particle Filter, PF)同样的缺点, 即建议密度函数的选择对算法性能的影响至关重要.
经典SMC-PHD(Classic SMC-PHD, C-SMC-PHD)滤波中将目标状态转移方程作为建议密度函数, 没有利用当前观测量, 导致在运动模型不准时, 大量粒子在迭代过程中权值趋于零, 粒子退化严重.针对SMC-PHD滤波中建议密度函数的选择问题, 许多学者提出改进的SMC-PHD算法, 例如基于辅助粒子滤波的SMC-PHD算法[4]、基于扩展卡尔曼滤波的SMC-PHD(Extent Kalman SMC-PHD, EK-SMC-PHD)算法[5]、基于无迹卡尔曼滤波的SMC-PHD(Unscented Kalman SMC-PHD, UK-SMC-PHD)算法[6]等.在这些改进的算法中, UK-SMC-PHD算法的跟踪性能最优[7-8], 但由于UK-SMC-PHD算法中采用无迹卡尔曼滤波(Unscented Kalman Filter, UKF)产生建议密度函数, 而UKF的性能受目标状态维数的限制, 因而UK-SMC-PHD在目标状态维数较高时算法性能下降很快.
基于均方根容积卡尔曼滤波的SMC-PHD(SCK-SMC-PHD)算法, 利用均方根容积卡尔曼滤波(Square-rooted Cubature Kalman Filter, SCKF)产生建议密度函数, 然后对其进行采样得到预测粒子状态.容积卡尔曼滤波(Cubature Kalman Filter, CKF)和UKF同属于利用数值积分解决高维积分问题的范畴, 但同UKF不同, CKF中采用的容积点是基于球面-径向容积准则, 经严密数学推导得出, 有坚实的理论基础, 且CKF性能不受目标状态维数的限制, 适用性更强.而SCKF是对CKF的进一步改进, 避免了无论在CKF还是在UKF中都需进行的协方差矩阵的开方运算, 进一步放宽了CKF的适用范围.仿真对比试验表明, SCK-SMC-PHD算法无论对目标数目还是对目标状态的估计精度都优于C-SMC-PHD算法和UK-SMC-PHD算法.
1 基于RFS的多目标跟踪
1.1 PHD滤波
RFS是指由数量有限的随机元所组成的集合, PHD滤波是基于RFS理论, 将多目标跟踪中的目标状态集合和观测量集合分别看成两个RFS, 再利用集合积分、集合导数及泛函理论, 在多目标后验概率密度满足泊松分布的前提下, 用全局后验概率密度的一阶矩来代替其本身在多目标贝叶斯滤波公式中传递, 从而简化了多目标贝叶斯滤波.
设k时刻有N(k)个目标状态分别为xk, 1, …, xk, N(k)的目标, 有M(k)个状态分别为zk, 1, …, zk, M(k)的观测量, 基于RFS理论[9], 分别对多目标的目标状态集Xk和观测集Zk建模如下:
{\mathit{\boldsymbol{X}}_k} = \left\{ {{\mathit{\boldsymbol{x}}_{k,1}}, \cdots ,{\mathit{\boldsymbol{x}}_{k,N\left( k \right)}}} \right\} \in \aleph \left( \mathit{\boldsymbol{\chi }} \right), (1) {\mathit{\boldsymbol{Z}}_k} = \left\{ {{\mathit{\boldsymbol{z}}_{k,1}}, \cdots ,{\mathit{\boldsymbol{z}}_{k,M\left( k \right)}}} \right\} \in \aleph \left( \mathit{\boldsymbol{\zeta }} \right). (2) 式中\aleph (χ)、\aleph (ζ)分别为目标状态空间χ⊆Rnx和观测空间ζ⊆Rnz上所有有限子集的集合.
通过以上目标状态和观测量的RFS建模, 可将单目标贝叶斯滤波推广到多目标跟踪中, 得到多目标贝叶斯递推式;
预测:
\begin{array}{l} {p_{k|k - 1}}\left( {{\mathit{\boldsymbol{X}}_k}|{\mathit{\boldsymbol{Z}}_{1:k - 1}}} \right)\\ = \int {{p_{k|k - 1}}\left( {{\mathit{\boldsymbol{X}}_k}|{\mathit{\boldsymbol{X}}_{k - 1}}} \right){p_{k - 1|k - 1}}} \\ \;\;\;\left( {{\mathit{\boldsymbol{X}}_{k - 1}}|{\mathit{\boldsymbol{Z}}_{1:k - 1}}} \right)\delta {\mathit{\boldsymbol{X}}_{k - 1}}; \end{array} (3) 更新:
\begin{array}{l} {p_{k|k}}\left( {{\mathit{\boldsymbol{X}}_k}|{\mathit{\boldsymbol{Z}}_{1:k}}} \right)\\ = \frac{{{g_k}\left( {{\mathit{\boldsymbol{Z}}_k}|{\mathit{\boldsymbol{X}}_k}} \right){p_{k|k - 1}}\left( {{\mathit{\boldsymbol{X}}_k}|{\mathit{\boldsymbol{Z}}_{1:k - 1}}} \right)}}{{\int {{g_k}\left( {{\mathit{\boldsymbol{Z}}_k}|{\mathit{\boldsymbol{X}}_k}} \right){p_{k|k - 1}}\left( {{\mathit{\boldsymbol{X}}_k}|{\mathit{\boldsymbol{Z}}_{1:k - 1}}} \right)\delta {\mathit{\boldsymbol{X}}_k}} }}. \end{array} (4) 式中: gk(·|·)为多目标联合似然函数; pk|k(Xk|Z1:k)为多目标联合后验概率密度; pk|k-1(Xk|Z1:k-1)为多目标联合先验概率密度; pk|k-1(Xk|Xk-1)为多目标状态转移概率密度函数.
将多目标贝叶斯中的pk|k-1(Xk|Z1:k-1)和pk(Xk|Z1:k)分别用其一阶矩Dk|k-1(x)和Dk(x)近似表示, 得到PHD的迭代递推式[4],
预测:
\begin{array}{l} {D_{k|k - 1}}\left( \mathit{\boldsymbol{x}} \right) = {\gamma _k} + \int {\left( {{\beta _{k|k - 1}}\left( {\mathit{\boldsymbol{x|\xi }}} \right) + } \right.} \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\left. {{p_{{\rm{s}},k}}\left( \mathit{\boldsymbol{\xi }} \right){f_{k|k - 1}}\left( {\mathit{\boldsymbol{x|\xi }}} \right)} \right){D_{k - 1}}\left( \mathit{\boldsymbol{\xi }} \right){\rm{d}}\mathit{\boldsymbol{\xi }}; \end{array} (5) 更新:
\begin{array}{l} {D_k}\left( \mathit{\boldsymbol{x}} \right) = \left( {1 - {p_{{\rm{d}},k}}\left( \mathit{\boldsymbol{x}} \right)} \right){D_{k|k - 1}}\left( \mathit{\boldsymbol{x}} \right) + \\ \;\;\;\;\;\sum\limits_{\mathit{\boldsymbol{z}} \in {\mathit{\boldsymbol{Z}}_k}} {\frac{{{p_{{\rm{d}},k}}\left( \mathit{\boldsymbol{x}} \right){g_k}\left( {\mathit{\boldsymbol{z|x}}} \right){D_{k|k - 1}}\left( \mathit{\boldsymbol{x}} \right)}}{{{c_k}{\lambda _k} + \int {{p_{{\rm{d}},k}}\left( \mathit{\boldsymbol{x}} \right){g_k}\left( {\mathit{\boldsymbol{z|x}}} \right){D_{k|k - 1}}\left( \mathit{\boldsymbol{x}} \right){\rm{d}}\mathit{\boldsymbol{x}}} }}} . \end{array} (6) 式中:ps, k(x)为目标存活概率; fk|k-1(·|·)为其单目标的状态转移密度; βk|k-1(·|·)为衍生目标的概率密度函数; γk为k时刻新生目标的密度函数; pd, k为k时刻目标检测概率; ck为杂波概率密度; λk为杂波平均数; gk(·|·)为单目标似然函数.
1.2 SMC-PHD滤波
为将PHD滤波由理论引入到工程实现, B.-N.Vo提出了SMC-PHD滤波.该算法通过一系列带权值的随机样本(加权粒子)来近似表示D(x), 可在非线性非高斯条件下得出PHD滤波的闭合解.假定{xk-1(i), ωk-1(i)}i=1Lk-1为k-1(k≥1)时刻PHD的粒子集, Lk-1是k-1时刻的粒子数, 不考虑衍生目标, SMC-PHD的算法实现步骤如下[3]:
◆预测:分别对存活粒子建议密度函数qk(·|xk-1(i), Zk)和新生粒子建议密度函数pk(·|Zk)采样得到k时刻的粒子状态为
\mathit{\boldsymbol{x}}_k^{\left( i \right)} \sim \left\{ \begin{array}{l} {q_k}\left( { \cdot |\mathit{\boldsymbol{x}}_{k - 1}^{\left( i \right)},{\mathit{\boldsymbol{Z}}_k}} \right),i = 1,2, \cdots ,{L_{k - 1}};\\ {p_k}\left( { \cdot |{\mathit{\boldsymbol{Z}}_k}} \right),i = {L_{k - 1}} + 1, \cdots ,{L_{k - 1}} + {J_k}. \end{array} \right. (7) 由式(8)计算粒子xk(i)对应的权值ωk|k-1(i):
\begin{array}{l} \omega _{k|k - 1}^{\left( i \right)} \sim \\ \left\{ {\begin{array}{*{20}{c}} {\omega _{k - 1}^{\left( i \right)}{p_{s,k}}{f_{k|k - 1}}\left( {\mathit{\boldsymbol{x}}_k^{\left( i \right)}|\mathit{\boldsymbol{x}}_{k - 1}^{\left( i \right)}} \right)/{q_k}\left( { \cdot |\mathit{\boldsymbol{x}}_{k - 1}^{\left( i \right)},{\mathit{\boldsymbol{Z}}_k}} \right),}\\ {i = 1,2, \cdots ,{L_{k - 1}};}\\ {{\mathit{\boldsymbol{\gamma }}_k}\left( {\mathit{\boldsymbol{x}}_k^{\left( i \right)}} \right)/\left( {{J_k}{p_k}\left( {\mathit{\boldsymbol{x}}_k^{\left( i \right)}|{\mathit{\boldsymbol{Z}}_k}} \right)} \right),}\\ {i = {L_{k - 1}} + 1, \cdots ,{L_{k - 1}} + {J_k}.} \end{array}} \right. \end{array} (8) 式中: Jk=ρ {\hat N_\gamma }为新生粒子数, ρ为每个新生目标的采样点数, {\hat N_\gamma }=∫γk(x)dx为新生目标的期望数.
◆更新:根据式(9), 利用观测集Zk更新粒子权值为
\begin{array}{l} \omega _k^{\left( i \right)} = \omega _{k|k - 1}^{\left( i \right)} \times \left[ {1 - {p_{{\rm{d}},k}}\left( {\mathit{\boldsymbol{x}}_k^{\left( i \right)}} \right) + } \right.\\ \;\;\;\;\;\;\;\;\left. {\sum\limits_{\mathit{\boldsymbol{z}} \in {\mathit{\boldsymbol{Z}}_k}} {\frac{{{\psi _{k,\mathit{\boldsymbol{z}}}}\left( {\mathit{\boldsymbol{x}}_k^{\left( i \right)}} \right)}}{{{\lambda _k}{c_k} + \left\langle {{\omega _{k|k - 1}},{\psi _{k,\mathit{\boldsymbol{z}}}}} \right\rangle }}} } \right],\\ \;\;\;\;\;\;\;\;i = 1,2, \cdots ,{L_{k - 1}} + {J_k}. \end{array} 式中: ψk, z(xk(i))=pd, k(xk(i))·g(z|xk(i)),
\left\langle {{\omega _{k|k - 1}},{\psi _{k,\mathit{\boldsymbol{z}}}}} \right\rangle = \sum\limits_{i = 1}^{{L_{k - 1}} + {J_k}} {{\psi _{k,\mathit{\boldsymbol{z}}}}\left( {\mathit{\boldsymbol{x}}_k^{\left( i \right)}} \right) \cdot \omega _{k|k - 1}^{\left( i \right)}} . (9) ◆重采样:计算k时刻的目标数:Nk=[ \sum\limits_{i = 1}^{{L_{k-1}} + {J_k}} {\omega _k^{\left( i \right)}} ], [·]是对“·”进行取整.每一个目标固定采样l个粒子, 对粒子集{xk(i), ωk(i)}i=1Lk-1+Jk进行重采样, 并对权值做归一化处理得到k时刻的粒子集{xk(i), Nk/Lk}i=1Lk, 其中Lk=l·Nk.
◆目标状态提取:采用k-means方法对重采样后得到的粒子集{xk(i), Nk/Lk}i=1Lk进行聚类分析, 得到k时刻的目标状态估计.
SMC-PHD滤波的关键步骤是最优建议密度函数的选择.C-SMC-PHD中将目标状态转移函数做为建议密度函数, 没有利用当前观测信息, 在运动模型不准确时, 会使大量粒子偏离目标真实状态, 在SMC-PHD递推式中, 这些粒子的权值会变的很小, 即这些粒子对后验概率的贡献几乎为零, 而真正有贡献的粒子在迭代过程中会越来越少, 粒子退化严重.在针对上述问题所提出的改进算法中, UK-SMC-PHD的性能卓越, 但UK-SMC-PHD滤波中参数的选择受目标状态维数的影响, 当目标状态维数较高时, 算法性能不稳定, 同时, UK-SMC-PHD算法中需要对协方差矩阵进行开方运算, 一旦在迭代过程中出现协方差矩阵非正定, 算法就会出错.而SCK-SMC-PHD滤波算法, 利用SCKF构建建议密度函数, 既提高了算法的跟踪精度, 同时避免了协方差矩阵开方运算, 且算法性能不受目标状态维数限制, 增强了算法的适用性和稳定性.
2 SCK-SMC-PHD滤波
2.1 SCKF算法
考虑一般的多目标跟踪问题, 在直角坐标系下给出目标离散时间的过程方程和观测方程, 表示为
\left\{ \begin{array}{l} {\mathit{\boldsymbol{x}}_k} = f\left( {{\mathit{\boldsymbol{x}}_{k - 1}}} \right) + {\mathit{\boldsymbol{u}}_{k - 1}},\\ {\mathit{\boldsymbol{z}}_k} = h\left( {{\mathit{\boldsymbol{x}}_k}} \right) + {\mathit{\boldsymbol{v}}_k}. \end{array} \right. (10) 式中: f(·)和h(·)分别为目标的过程模型和观测模型; xk和zk分别为k时刻的目标状态和观测量; uk-1、vk分别为过程噪声和观测噪声,服从均值为0,协方差分别为Qk-1、Rk的高斯分布.
当过程方程或观测方程为非线性时, 贝叶斯滤波的最优解通常无法得到, 利用近似得到贝叶斯滤波的次最优解是常用的方法.同UKF相似, SCKF也是通过数值积分来近似得到高维积分, 但同UKF通过UT变换选取Sigma点的方式不同, SCKF基于三阶球面-径向容积准则选取容积点.
在系统状态和噪声都是高斯分布的条件下, 非线性滤波的问题可转化成求解被积函数为非线性函数×高斯概率密度的积分的问题[10].考虑最简单的形式为
\begin{array}{l} Y\left( f \right) = \int_{{{\bf{R}}^n}} {f\left( \mathit{\boldsymbol{x}} \right)\exp \left( { - {\mathit{\boldsymbol{x}}^{\rm{T}}}\mathit{\boldsymbol{x}}} \right){\rm{d}}\mathit{\boldsymbol{x}}} \\ \;\;\;\;\;\;\;\;\; = \int_{{{\bf{R}}^n}} {f\left( \mathit{\boldsymbol{x}} \right) \cdot N\left( {\mathit{\boldsymbol{x}};\mathit{\boldsymbol{0,I}}} \right){\rm{d}}\mathit{\boldsymbol{x}}} . \end{array} (11) 式中: f(·)为非线性函数; x∈ \mathbb{R}^n, n为x的维度; I为n×n阶单位阵.
将式(11)由直接坐标系转换到球面-径向坐标系下.令x=ry(yyT=1), 则xTx=r2, r∈[0, ∞), 进而式(11)在球面-径向坐标系下有
Y\left( f \right) = \int_0^\infty {\int_{{\mathit{\boldsymbol{U}}_n}} {f\left( {r\mathit{\boldsymbol{y}}} \right){r^{n - 1}}\exp \left( { - {r^2}} \right){\rm{d}}\sigma \left( \mathit{\boldsymbol{y}} \right){\rm{d}}r} } . (12) 式中: Un表示单位超球面; σ(·)表示积分微元.将式(12)进一步化简可写成球面-径向积分形式:
Y\left( f \right) = \int_0^\infty {S\left( r \right){r^{n - 1}}\exp \left( { - {r^2}} \right){\rm{d}}r} ; (13) S\left( r \right) = \int_{{\mathit{\boldsymbol{U}}_n}} {f\left( {ry} \right){\rm{d}}\sigma \left( \mathit{\boldsymbol{y}} \right)} . (14) 式(13)为径向积分, 式(14)为超球面多维积分.分别利用mr个点和ms个点基于高斯-厄米特准则和球形积分准则来近似式(13)和式(14)的积分为
\int_0^\infty {S\left( r \right){r^{n - 1}}\exp \left( { - {r^2}} \right){\rm{d}}r} = \sum\limits_{i = 1}^{{m_r}} {{a_i}S\left( {{r_i}} \right)} ; (15) \int_{{\mathit{\boldsymbol{U}}_n}} {f\left( {r\mathit{\boldsymbol{y}}} \right){\rm{d}}\sigma \left( \mathit{\boldsymbol{y}} \right)} = \sum\limits_{j = 1}^{{m_s}} {{b_j}f\left( {r{\mathit{\boldsymbol{y}}_j}} \right)} , (16) 则式(11)的积分可以表示为
\begin{array}{l} Y\left( f \right) = \int_{{{\bf{R}}^n}} {\mathit{\boldsymbol{f}}\left( \mathit{\boldsymbol{x}} \right)\exp \left( { - {\mathit{\boldsymbol{x}}^{\rm{T}}}\mathit{\boldsymbol{x}}} \right){\rm{d}}\mathit{\boldsymbol{x}}} \\ \;\;\;\;\;\;\;\;\; \approx \sum\limits_{j = 1}^{{m_s}} {\sum\limits_{i = 1}^{{m_r}} {{a_i}{b_j}f\left( {{r_i}{\mathit{\boldsymbol{y}}_j}} \right).} } \end{array} (17) 基于三次幂球面-径向准则, 取mr=1, ms=2n, 可得到
Y\left( f \right) \approx \sum\limits_{i = 1}^{2n} {{\omega _i}f\left( {{\mathit{\boldsymbol{\varphi }}_i}} \right)} . (18) 式中, {ωi, φi}i=12n为一系列的带权值的数值点, 权值ωi= \frac{1}{{2n}}, φi为矩阵φ的第i列,
\mathit{\boldsymbol{\varphi }} = \sqrt n \left\{ {\left( {\begin{array}{*{20}{c}} 1\\ \vdots \\ 0 \end{array}} \right), \cdots ,\left( {\begin{array}{*{20}{c}} 0\\ \vdots \\ 1 \end{array}} \right),\left( {\begin{array}{*{20}{c}} { - 1}\\ \vdots \\ 0 \end{array}} \right), \cdots ,\left( {\begin{array}{*{20}{c}} 0\\ \vdots \\ { - 1} \end{array}} \right)} \right\}. (19) 由以上分析可以看出, SCKF中容积点的个数比UKF中Sigma点的个数要少, 且其对应权值的计算比UKF简单, 因此SCKF的计算复杂度要低于UKF.同时, SCKF的性能不依赖于参数的选择, 且引入了QR分解, 避免了矩阵开方运算, 因而比UKF的稳定性要好.
2.2 SCK-SMC-PHD滤波
鉴于SCKF在处理非线性滤波中的优越性, 利用SCKF构建SMC-PHD中的建议密度函数, 再对其进行采样得到预测粒子状态.本文采用伪代码的形式详细介绍利用SCKF构建建议密度函数的步骤.
已知k-1时刻粒子集{xk-1(i), ωk-1(i), Sk-1(i)}i=1Lk-1, 其中Sk-1(i)为第i个粒子的协方差矩阵的均方根, 则利用SCKF得到建议密度函数, 进而获取k时刻存活粒子状态xk(i)(i=1, 2, …,Lk-1)的步骤如下:
◆获取每个粒子所对应的容积点, 其伪代码为
for i=1, …, Lk-1
for j=1, …, 2n
\mathit{\boldsymbol{\chi }}_{j,k - 1}^{\left( i \right)} = \mathit{\boldsymbol{S}}_{k - 1}^{\left( i \right)}{\mathit{\boldsymbol{\varphi }}_j} + \mathit{\boldsymbol{x}}_{k - 1}^{\left( i \right)},{w_j} = 1/2n, end
end
◆利用状态方程进行时间更新, 其伪代码为
for i=1, …, Lk-1
for j=1, …, 2n
\mathit{\boldsymbol{\chi }}_{j,k|k - 1}^{\left( i \right)*} = f\left( {\mathit{\boldsymbol{\chi }}_{j,k - 1}^{\left( i \right)}} \right), end
\mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)} = \sum\limits_{j = 1}^{2n} {{w_j}\mathit{\boldsymbol{\chi }}_{j,k|k - 1}^{\left( i \right)*}} , \begin{array}{l} \mathit{\boldsymbol{\eta }}_{k|k - 1}^{\left( i \right)*} = \left( {\frac{1}{{\sqrt {2n} }}} \right) \times \\ \left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{\chi }}_{1,k|k - 1}^{\left( i \right)*} - \mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)}}&{\mathit{\boldsymbol{\chi }}_{2,k|k - 1}^{\left( i \right)*} - \mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)}} \end{array}} \right.\\ \left. {\begin{array}{*{20}{c}} \cdots &{\mathit{\boldsymbol{\chi }}_{2n,k|k - 1}^{\left( i \right)*} - \mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)}} \end{array}} \right], \end{array} \mathit{\boldsymbol{S}}_{k|k - 1}^{\left( i \right)} = {\rm{tria}}\left( {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{\eta }}_{k|k - 1}^{\left( i \right)*}}&{{\mathit{\boldsymbol{S}}_{Q,k - 1}}} \end{array}} \right]} \right), end.
SQ, k-1为过程噪声协方差阵Qk-1的均方根, 即Qk-1=SQ, k-1SQ, k-1T; tria(·)表示矩阵的一种三角化运算:设矩阵B为矩阵AT经过QR分解得到的上三角矩阵, 则S=tria(A)代表着S=BT.
◆利用观测量进行量测更新, 得到预测粒子状态的均值及其所对应的状态协方差矩阵均方根, 其伪代码为
for i=1, …, Lk-1
for j=1, …, 2n
\mathit{\boldsymbol{\chi }}_{j,k|k - 1}^{\left( i \right)} = \mathit{\boldsymbol{S}}_{k|k - 1}^{\left( i \right)}{\mathit{\boldsymbol{\varphi }}_j} + \mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)}, \mathit{\boldsymbol{Z}}_{j,k|k - 1}^{\left( i \right)} = h\left( {\mathit{\boldsymbol{\chi }}_{j,k|k - 1}^{\left( i \right)}} \right), end
\mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\left( i \right)} = \sum\limits_{j = 1}^{2n} {{w_j}\mathit{\boldsymbol{Z}}_{j,k|k - 1}^{\left( i \right)}} , \begin{array}{l} \mathit{\boldsymbol{\lambda }}_{z,k|k - 1}^i = \left( {1/\sqrt {2n} } \right) \times \left[ {\mathit{\boldsymbol{Z}}_{1,k|k - 1}^{\left( i \right)} - \mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\left( i \right)}\mathit{\boldsymbol{Z}}_{2,k|k - 1}^{\left( i \right)} - } \right.\\ \;\;\;\;\;\;\;\;\;\;\;\;\;\left. {\mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\left( i \right)} \cdots \mathit{\boldsymbol{Z}}_{2n,k|k - 1}^{\left( i \right)} - \mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\left( i \right)}} \right], \end{array} \mathit{\boldsymbol{S}}_{zz,k|k - 1}^{\left( i \right)} = {\rm{tria}}\left( {\left[ {\mathit{\boldsymbol{\lambda }}_{z,k|k - 1}^{\left( i \right)}{\mathit{\boldsymbol{S}}_{R,k}}} \right]} \right), \begin{array}{l} \mathit{\boldsymbol{\eta }}_{k|k - 1}^{\left( i \right)} = \left( {1/\sqrt {2n} } \right) \times \left[ {\mathit{\boldsymbol{\chi }}_{1,k|k - 1}^{\left( i \right)} - \mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)}\mathit{\boldsymbol{\chi }}_{2,k|k - 1}^{\left( i \right)} - } \right.\\ \;\;\;\;\;\;\;\;\;\;\left. {\mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)} \cdots \mathit{\boldsymbol{\chi }}_{2n,k|k - 1}^{\left( i \right)} - \mathit{\boldsymbol{\bar x}}_{k|k - 1}^{\left( i \right)}} \right], \end{array} \mathit{\boldsymbol{p}}_{xz,k|k - 1}^{\left( i \right)} = \mathit{\boldsymbol{\eta }}_{k|k - 1}^{\left( i \right)}\lambda _{z,k|k - 1}^{\left( i \right){\rm{T}}}, \mathit{\boldsymbol{W}}_k^{\left( i \right)} = \left( {\mathit{\boldsymbol{p}}_{xz,k|k - 1}^{\left( i \right)}/\mathit{\boldsymbol{S}}_{zz,k|k - 1}^{\left( i \right){\rm{T}}}} \right)\mathit{\boldsymbol{S}}_{zz,k|k - 1}^{\left( i \right)}, \mathit{\boldsymbol{\bar x}}_k^{\left( i \right)} = \mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\left( i \right)} + \mathit{\boldsymbol{W}}_k^{\left( i \right)}\left( {\mathit{\boldsymbol{z}}_k^{\left( i \right)} - \mathit{\boldsymbol{z}}_{k|k - 1}^{\left( i \right)}} \right), \mathit{\boldsymbol{S}}_k^{\left( i \right)} = {\rm{tria}}\left( {\left[ {\mathit{\boldsymbol{\eta }}_{k|k - 1}^{\left( i \right)} - \mathit{\boldsymbol{W}}_k^{\left( i \right)}\mathit{\boldsymbol{\lambda }}_{z,k|k - 1}^{\left( i \right)}\mathit{\boldsymbol{W}}_k^{\left( i \right)}{\mathit{\boldsymbol{S}}_{R,k}}} \right]} \right), end
SR, k为过程噪声协方差阵Rk的均方根, 即Rk=SR, kSR, kT.
◆对建议密度函数qk(·|xk-1(i), Zk)采样得到粒子状态xk(i), 其伪代码为
for i=1, …, Lk-1
\mathit{\boldsymbol{p}}_k^{\left( i \right)} = \mathit{\boldsymbol{S}}_k^{\left( i \right)}\mathit{\boldsymbol{S}}_k^{\left( i \right){\rm{T}}}, \mathit{\boldsymbol{x}}_k^{\left( i \right)} \sim {q_k}\left( { \cdot |\mathit{\boldsymbol{x}}_{k - 1}^{\left( i \right)},{\mathit{\boldsymbol{Z}}_k}} \right) = \mathit{\boldsymbol{N}}\left( {\mathit{\boldsymbol{\bar x}}_k^{\left( i \right)},\mathit{\boldsymbol{\bar p}}_k^{\left( i \right)}} \right), end
SCK-SMC-PHD的其他步骤与C-SMC-PHD相同, 如1.2节中所述.
2.3 SCK-SMC-PHD与UK-SMC-PHD对比分析
SCK-SMC-PHD滤波算法与UK-SMC-PHD滤波算法的不同之处在于构建建议密度函数的方式不同, 前者通过SCKF获取建议密度函数, 而后者则是通过UKF获取建议密度函数.SCKF同UKF相同, 都是通过一些数值积分点来近似当前粒子的均值和方差, 但两者选取数值积分点的方式不同(前者选取容积点, 后者选取Sigma点).为方便对比两种算法的性能, 给出UKF通过UT变换获取Sigma点步骤.已知n维随机变量x, 其均值为μ, 协方差矩阵为p, 则x所对应的Sigma点{χi, ωi}i=02n的选取方式为[6]
{\mathit{\boldsymbol{\chi }}_0} = \mu ,{\omega _0} = \kappa /\left( {n + \kappa } \right);\\ {\mathit{\boldsymbol{\chi }}_i} = \mu + {\left( {\sqrt {\left( {n + \kappa } \right)\mathit{\boldsymbol{p}}} } \right)_i},{\omega _i} = 1/\left( {2\left( {n + \kappa } \right)} \right);\\ {\mathit{\boldsymbol{\chi }}_{n + i}} = \mu - {\left( {\sqrt {\left( {n + \kappa } \right)\mathit{\boldsymbol{p}}} } \right)_i},{\omega _{n + i}} = 1/\left( {2\left( {n + \kappa } \right)} \right). (20) 式中, i=1, 2, …, n.
下面我们从以下几个方面对比SCK-SMC-PHD与UK-SMC-PHD的性能:第一, 理论基础.SCKF中容积点的选取是基于三阶球面-径向积分准则经过严格的数学推导得出的, 而UKF中Sigma点是由UT变换得到的, 而UT变换本身没有坚实的理论依据, 其中一些参数的选取还需要依赖经验, 因而, SCK-SMC-PHD与UK-SMC-PHD相比, 具有更强的理论支撑.第二, 计算量.对比表2和表3可以看出, 针对维数为n的随机变量, SCKF选取2n个容积点, 而UKF则需选取2n+1个Sigma点, 因此, SCK-SMC-PHD的算法运行时间要少于UK-SMC-PHD.同时, UKF需要调节参数得到每个Sigma点及其所对应的权值, 而SCKF中容积点及其权值的获取不需要额外的参数, 因而, SCK-SMC-PHD的算法复杂度要低于UK-SMC-PHD.第三, 算法稳定性.由表3可以看出, UKF中Sigma点及其权值的选择依赖于参数κ, 如果κ的选择不合适将会严重影响算法性能, 特别是当目标状态维数大于3时, UKF算法性能及稳定性迅速降低, 而SCKF中容积点的选取只与随机变量均值, 协方差阵和维数有关, 因此SCK-SMC-PHD算法比UK-SMC-PHD算法更稳定.此外, 由于SCKF中避免了UKF中的矩阵开方运算, 进一步提高了SCK-SMC-PHD滤波的稳定性.第四, 跟踪精度.SCKF与UKF都是通过一组数值点来近似多维积分.为比较SCKF与UKF在目标状态维数较高时的滤波精度, 定义稳定因子[10]
{I_\omega } = \sum\limits_i {\left| {{\omega _i}} \right|} /\sum\limits_i {{\omega _i}} . (21) 式中, ωi为数值点的权值(SCKF中为容积点权值, UKF中为Sigma点权值).当Iω>1时, 多维积分的数值估计将引入较大误差.在SCKF中, Iω始终等于1, 而在UKF中, ωi的选取与参数κ有关, 依据经验通常取n+κ=3, 当目标状态维数n小于等于3时, Iω等于1, 当目标状态维数n大于3时, Iω大于1且随着目标维数的增加而增大.由此可见, 当目标状态维数较高时, SCK-SMC-PHD算法的滤波精度要高于UK-SMC-PHD.
3 实验仿真
为提高定位和跟踪精度, 仿真实验在多传感器联合定位[11-12]的背景下, 对比分析C-SMC-PHD、UK-SMC-PHD和SCK-SMC-PHD三种算法的跟踪性能, 传感器布局如图 1所示, 其中每个发射站与接收站构成一对传感器.
仿真实验监测区域为[-30 30] km×[-30 30] km的二维平面.接收站坐标为[0 0] km, 3个发射站坐标分别为[0 20] km、[-17 -10] km、[17 -10] km, 目标状态为xk=[xk {{\dot x}_k} yk {{\dot y}_k}], 由目标位置信息[ xk yk]和目标速度[ {{\dot x}_k} \ \ \ {{\dot y}_k}]组成, 目标初始位置分别为[20 25] km、[15 10] km、[10 30] km和[20 15] km, 目标速度分别为[150 60] m/s、[200 -10] m/s、[0 -180] m/s和[-170 0] m/s.目标的状态方程为
\begin{array}{l} {\mathit{\boldsymbol{x}}_k} = \left[ {\begin{array}{*{20}{c}} 1&T&0&0\\ 0&1&0&0\\ 0&0&1&T\\ 0&0&0&1 \end{array}} \right]{\mathit{\boldsymbol{x}}_{k - 1}} + \\ \;\;\;\;\;\;\;q\left[ {\begin{array}{*{20}{c}} {\frac{{{T^4}}}{4}}&{\frac{{{T^3}}}{2}}&0&0\\ {\frac{{{T^3}}}{2}}&{{T^2}}&0&0\\ 0&0&{\frac{{{T^4}}}{4}}&{\frac{{{T^3}}}{2}}\\ 0&0&{\frac{{{T^3}}}{2}}&{{T^2}} \end{array}} \right]. \end{array} (22) 式中, q=3 m/s2为过程噪声标准差.观测方程为
\begin{array}{l} {\mathit{\boldsymbol{z}}_k} = {\mathit{\boldsymbol{v}}_k} + {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{R}}_k}}&{{{\mathit{\boldsymbol{\dot R}}}_k}} \end{array}} \right]^{\rm{T}}} = \left[ {\begin{array}{*{20}{c}} {{v_{1,k}}}\\ {{v_{2,k}}} \end{array}} \right] + \\ \left[ {\begin{array}{*{20}{c}} {\sqrt {{{\left( {x - {x_t}} \right)}^2} + {{\left( {y - {y_t}} \right)}^2}} + \sqrt {{{\left( {x - {x_r}} \right)}^2} + {{\left( {y - {y_r}} \right)}^2}} }\\ {\frac{{\left( {x - {x_r}} \right)\dot x + \left( {y - {y_r}} \right)\dot y}}{{\sqrt {{{\left( {x - {x_r}} \right)}^2} + {{\left( {y - {y_r}} \right)}^2}} }} + \frac{{\left( {x - {x_t}} \right)\dot x + \left( {y - {y_t}} \right)\dot y}}{{\sqrt {{{\left( {x - {x_t}} \right)}^2} + {{\left( {y - {y_t}} \right)}^2}} }}} \end{array}} \right]. \end{array} (23) 式中: vk为观测噪声; v1, k, v2, k为相互独立的高斯白噪声, 标准差分别为100 m和1 m/s.
设目标出现的时刻分别为初始时刻、10 s、30 s和50 s, 目标消失的时刻分别为60 s、80 s、100 s和100 s, 目标存活概率ps=0.98, 检测概率pd=1, 新生目标出现服从泊松模型, 其密度函数为γk(x)= \sum\limits_{i = 1}^3 {} 0.05N(x; mi, pγ), 其中pγ=diag([200 5 200 5]T)为新生目标的协方差阵.杂波在观测区域内均匀分布, 每桢平均杂波数λ=10, 每时刻每个目标采样1 000个粒子, 每时刻用于搜索新生目标的新生粒子数为1 500.
图 2为目标真实位置与三种滤波方法得到的跟踪轨迹图.由图 2可以看出, C-SMC-PHD的跟踪性能最差.这是由于C-SMC-PHD中建议密度函数的选择缺少观测信息, 严重依赖于模型, 致使由建议密度函数抽样所得到的样本大部分偏离目标真实状态, 导致粒子退化, 滤波性能下降严重.而利用观测信息构建建议密度函数的UK-SMC-PHD算法和SCK-SMC-PHD算法其性能明显优于C-SMC-PHD, 其中SCK-SMC-PHD算法的性能又优于UK-SMC-PHD算法, 由此可见建议密度函数的选择对算法性能至关重要.
文中选取最优子模式分配(Optimal Sub-Pattern Assignment, OSPA)作为多目标跟踪精度评估标准, 设X={x1, …, xm}和Y={y1, …, yn}为两个任意有限集合, m和n分别为X和Y中的元素个数, 若m≤n, 则OSPA距离定义为[13]
\begin{array}{l} \bar d_p^{\left( c \right)}\left( {\mathit{\boldsymbol{X,Y}}} \right) = \left( {\frac{1}{n}\left( {\mathop {\min }\limits_{\pi \in {\Pi _n}} \sum\limits_{i = 1}^m {{d^{\left( c \right)}}{{\left( {{\mathit{\boldsymbol{x}}_i},{\mathit{\boldsymbol{y}}_{\pi \left( i \right)}}} \right)}^p} + } } \right.} \right.\\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{\left. {\left. {{c^p}\left( {n - m} \right)} \right)} \right)^{1/p}}. \end{array} (24) 若m>n, 则dp(c)(X, Y)=dp(c)(Y, X).式中p和c分别为距离误差敏感参数和势误差敏感参数, 仿真中取p=2, c=500.
图 3和图 4分别为100次MoteCarlo仿真后, 三种算法的势估计误差(估计目标数目误差)对比图和OSPA距离误差对比图.与图 2所示一致, 由于引入观测信息, SCK-SMC-PHD和UK-SMC-PHD的滤波精度无论在势估计还是在目标状态估计方面都远远优于C-SMC-PHD算法, 同时, 由图 2和图 3可以更加直观地看出SCK-SMC-PHD的跟踪精度要优于UK-SMC-PHD算法, 这与2.3节中的理论分析一致.
下面验证三种算法对不同杂波环境的适应性.取λ分别为0.001、1、5、10、15、20、30, 在不同密度的杂波环境下, 经过100次Mote Carlo仿真, 三种算法的势估计误差对比图和OSPA距离误差对比图分别如图 5和图 6所示.由图 5和图 6可以看出, 杂波密度相同时, SCK-SMC-PHD算法的跟踪精度要优于C-SMC-PHD算法和UK-SMC-PHD算法.三种算法的性能都随着杂波密度的增大而有所下降, 但三种算法中, SCK-SMC-PHD算法对杂波环境的适应性最强.
4 结论
针对C-SMC-PHD滤波中粒子退化严重的问题, 将SCKF滤波与C-SMC-PHD相结合, 利用SCKF产生建议密度函数, 提出了SCK-SMC-PHD滤波算法.该算法有坚实的理论依据, 能有效抑制C-SMC-PHD中的粒子退化, 与UK-SMC-PHD算法相比, 其计算量更小, 算法稳定性更好, 适用性更强, 且当目标状态维数较高时, 其跟踪精度更高, 仿真结果也证实了上述结论.下一步的工作是将SCKF与势分布PHD滤波[14]和多贝努力滤波[15]相结合, 改善上述两种算法的性能.此外, 通过设定门限等策略提高SCK-SMC-PHD算法的实时性, 也是下一步工作的重点.
-
[1] MAHLER R. Random sets: theory and Applications[M]. New York: Springer-Verlag Press, 1997: 129-164.
[2] MAHLER R Multi-target Bayes filtering via first-order multi-target moments[J]. IEEE transactions on aerospace and electronic systems, 2003, 39(4): 11521178. http://ieeexplore.ieee.org/xpls/icp.jsp?arnumber=1261119
[3] VO B N, SINGH S, DOUCET A. Sequential Monte Carlo methods for multi-target filtering with random finite sets[J]. IEEE transactions on aerospace and electronic systems, 2005, 41(4): 1224-1245. doi: 10.1109/TAES.2005.1561884
[4] WHITELEY N, SINGH S, GODSILL S. Auxiliary particle implementation of probability hypothesis density filter[J]. IEEE rransactions on aerospace and electronic systems, 2010, 46(3): 1437-1454. doi: 10.1109/TAES.2010.5545199
[5] MELZI M, OULDALI A, MESSAOUDI Z, et al. Target tracking using the extended Kalman particle probability hypothesis filter[C]//European Signal Processing Conference. Aalborg, 2010: 1821-1825. https://ieeexplore.ieee.org/document/7096446
[6] TANG X, ZHOU J, HUANG J. Improved particle implementation of the probability hypothesis density filter in resampling[C]//IEEE 12th International Conference on Computer and Information Technology. Chengdu, 2012: 56-61. https://ieeexplore.ieee.org/document/6391874
[7] ZUO J Y, JIA Y N. Adaptive iterated particle filter[J]. Electronics letters, 2013, 49(12): 556-557. http://d.old.wanfangdata.com.cn/NSTLQK/NSTL_QKJJ0230313081/
[8] BHUVANA V P, UNTERRIEDER C, HUEMER M. Battery internal state estimation: a comparative study of non-linear state estimation algorithms[C]//Vehicle Power and Propulsion Conference. Beijing, China, 2013: 5-18. https://ieeexplore.ieee.org/document/6671666
[9] MAHLER R. Multitarget detection and acquisition: a unified approach[C]//Proceedings of SPIE-The International Society for Optical Engineering, 1999: 218-229. https://www.mendeley.com/catalogue/multitarget-detection-acquisition-unified-approach/
[10] 宁夏, 叶春茂, 杨健, 等.容积卡尔曼滤波在空间轨道确定中的应用[J].电波科学学报, 2014, 29(1): 26-34. http://www.cnki.com.cn/Article/CJFDTotal-DBKX201401004.htm NING X, YE C M, YANG J, et al. Cubature Kalman filtering for orbit determination of space targets[J]. Chinese journal of radio science, 2014, 29(1): 26-34.(in Chinese) http://www.cnki.com.cn/Article/CJFDTotal-DBKX201401004.htm
[11] 王俊, 张守宏, 保铮.基于外辐射源的无源相干雷达系统及其关键技术[J].电波科学学报, 2005, 20(3): 381-385. WANG J, ZHANG S H, BAO Z. Study on the external illuminator based passive coherent radar experimental system[J]. Chinese journal of radio science, 2005, 20(3): 381-385. (in Chinese)
[12] 谢锐, 万显荣, 赵志欣, 等.外辐射源天地波雷达定位方法及精度分析[J].电波科学学报, 2014, 29(3): 442-449. http://www.cjors.cn/CN/abstract/abstract562.shtml XIE R, WAN X R, ZHAO Z X, et al. Localization method and accuracy analysis in hybrid sky-surface wave passive radar[J]. Chinese journal of radio science, 2014, 29(3): 442-449. (in Chinese) http://www.cjors.cn/CN/abstract/abstract562.shtml
[13] SCHUHMACHER D, VO B T, VO B N. A consistent metric for performance evaluation of multi-object filters[J]. IEEE transactions on signal processing, 2008, 56(8): 34473457. http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=0e7e06d3c20fd5102d7007d6c826e0e3
[14] VO B T, VO B N, CANTONI A. Analytic implementations of the cardinalized probability hypothesis density filter[J].IEEE transactions on signal processing, 2007, 55(7): 3553-3567. doi: 10.1109/TSP.2007.894241
[15] VO B T, VO B N, CANTONI A. The cardinality balanced multi-target multi-bernulli filter and its implementations[J]. IEEE transactions on signal processing, 2009, 57(2): 409-423. doi: 10.1109/TSP.2008.2007924
-
期刊类型引用(2)
1. 李艳玲,方遒,屠亚杰. 基于IST-RSCKF-MB的雷达多目标跟踪算法. 厦门理工学院学报. 2024(01): 9-16 . 百度学术
2. 徐君妍,崔宗勇,罗远庆,曹宗杰. 复杂场景下的加权粒子滤波行人跟踪方法. 信号处理. 2017(07): 934-942 . 百度学术
其他类型引用(2)