智能车能利用不同类型的车载传感器，如激光雷达、毫米波雷达、摄像机^[1-2]等，收集周边环境信息. 通过分析这些信息，智能车能区分可驾驶区域和障碍物，从而实现准确的环境感知^[3]. 目标跟踪是自动驾驶系统环境感知的一项关键技术，其本质是动态系统的状态估计问题，通过传感器的噪声测量值估计动态目标的真实状态^[4]. 卡尔曼滤波器^[5](Kalman filter，KF)是用于传感器融合与目标跟踪的标准模型，过程噪声和测量噪声均假设为统计特性已知的高斯分布. KF可被视为更通用的贝叶斯滤波器^[6]的特例. 随着系统模型越来越复杂，概率模型中的后验分布一般难以直接计算. 为了解决该问题，Beal^[7]提出变分贝叶斯推断(variational Bayesian inference, VBI)方法，基于平均场理论近似求解隐变量的后验分布. Särkkä等^[8]提出基于VBI的自适应卡尔曼滤波，可以对目标状态和噪声参数的联合后验分布进行近似计算. 沈忱等^[9]提出基于层次贝叶斯结构的状态空间模型，将测量噪声的均值、方差和系统状态一起作为随机变量，并利用VBI进行解耦. 余东平等^[10]提出基于VBI的多目标无源定位算法. Zhu等^[11]应用学生t-分布作为测量噪声模型进行状态估计. Huang等^[12]提出基于学生t-分布的分层高斯状态空间模型.

基于车载传感器的目标跟踪易受传感器感知精度、感知范围、周边目标之间相互遮挡等因素的影响. 为了解决以上问题，智能车能利用各种无线通信技术^[13]，如专用短距离通信(dedicated short range communications，DSRC)无线电或4G/5G技术^[14-15]，与其他邻近的智能车交换感知数据，包括目标的位置、速度、航向角等. 主车将车载传感器感知数据与接收的数据进行融合^[16]，能提升目标状态估计的准确性.

在协同目标跟踪中，协同车定位信息准确性越高，不同车体坐标系之间的坐标转换越准确，数据融合的误差就越小. 目前，基于卫星的导航定位系统，如全球定位系统和北斗系统，易受多路径效应、大气扰动和遮挡物干扰等因素影响，定位噪声具有重尾分布，导致定位数据中频繁出现野值^[17]. 因此，根据定位数据计算出的车辆间相对位置不够准确^[18].

针对定位数据易受野值干扰而影响协同目标跟踪性能的问题，本研究提出适用于重尾非高斯定位噪声的协同目标跟踪方法，将定位噪声建模为多元学生t-分布，构建目标状态和未知定位噪声参数的联合概率推断模型. 应用变分贝叶斯推断原理将后验分布估计问题转化为最优化问题，以交替优化的方式实现变分后验分布参数的有效计算，降低野值对目标状态估计的影响.

在参数估计问题上，学生t-分布具有重尾特性，在野值数据处理方面比高斯模型具有更好的鲁棒性. 因此，利用该分布刻画重尾非高斯定位噪声 $ {\boldsymbol{v}} $的分布， $ d $维学生t-分布的概率密度函数可表示为

(1) $ S\left( {{\boldsymbol{v}};{\boldsymbol{\mu}} ,{\boldsymbol{\varLambda}} ,\lambda } \right) \propto {\left[ {1 + \frac{1}{\lambda }{{\left( {{\boldsymbol{v}} - {\boldsymbol{\mu}} } \right)}^{\text{T}}}{\boldsymbol{\varLambda}} \left( {{\boldsymbol{v}} - {\boldsymbol{\mu}} } \right)} \right]^{ - {{(d + \lambda) }}/{2}}} .$

式中： ${\boldsymbol{ \mu}} \in {{{{{\rm{R}}}}}^d}$为均值， ${\boldsymbol{\varLambda}} \in {{{{{\rm{R}}}}}^{d \times d}}$为精确度， $ \lambda $为自由度，学生t-分布尾部的厚度由参数 $ \lambda $决定. 随着 $ \lambda $减少，尾部衰减较慢，当 $ \lambda = 1 $时，退化成柯西分布. 当 $ \lambda \to \infty $时，逼近高斯分布. 不同 $ \lambda $值的学生t-分布的概率密度函数如图1所示. 图中，v为随机变量.

学生t-分布的极大似然估计难以获得闭式解. 定义均值为 $ {\boldsymbol{\mu}} $和逆协方差矩阵为 $ {{\boldsymbol{\varSigma}} ^{ - 1}} $的多元高斯分布 $N\left( {{\boldsymbol{x}};{\boldsymbol{\mu}} ,{{\boldsymbol{\varSigma}} ^{ - 1}}} \right) = {\left( {2{\text{π }}} \right)^{ - {d}/{2}}}{\left| {\boldsymbol{\varSigma }} \right|^{{1}/{2}}}{\text{exp}}\;\left( - \dfrac{1}{2}{\text{Tr}}\;\Big( {\boldsymbol{\varSigma}} \left( {\boldsymbol{x} -\boldsymbol{ \mu} } \right) \times $ $ {{\left( {{\boldsymbol{x}} - {\boldsymbol{\mu}} } \right)}^{\text{T}}} \Big) \right)$( $ {\text{Tr}}\;\left( \cdot \right) $表示矩阵迹运算)，定义形状参数为 $ a $、逆尺度参数为 $ b $的伽马分布 $G\left( {\tau ;a,b} \right) = \dfrac{{{b^a}}}{{{{\varGamma}} \left( a \right)}}{\tau ^{a - 1}}{{\text{e}}^{ - b\tau }}$，可以将学生t-分布表示为具有相同均值和不同精确度的多元尺度高斯分布的无限混合^[18]：

(2) $ S\left({\boldsymbol{v}};{\boldsymbol{\mu}} ,{\boldsymbol{\varLambda}} ,\lambda \right)=\displaystyle {\int }_{0}^{\infty }N\left({\boldsymbol{v}};{\boldsymbol{\mu}} ,{\left(u{\boldsymbol{\varLambda}} \right)}^{-1}\right)G\left(u;{\lambda }/{2},{\lambda }/{2}\right)\text{d}u .$

协同目标跟踪场景包括主车(host vehicle，HV)、协同车(cooperative vehicle，CV)和目标车(target vehicle，TV)，其中，主车和协同车配备毫米波雷达、GPS定位系统和V2V通信设备等. 主车和协同车根据各自的车载传感器收集感知范围内的TV信息，CV将其对TV的测量数据和定位数据利用V2V通信设备发送给HV. HV融合本地感知数据与CV发送的数据，实现对TV状态的准确估计.

在跟踪过程中，假设系统状态转移方程服从如下线性模型：

(3) $ {{\boldsymbol{x}}_k} = {{\boldsymbol{F}}_k}{{\boldsymbol{x}}_{k - 1}} + {{\boldsymbol{w}}_k} . $

式中：k为离散时刻， $ k = 1,2,\cdots ,K $； ${{\boldsymbol{x}}_k}$为 $ k $时刻的系统状态变量， ${{\boldsymbol{x}}_k} = {\left[ {{\boldsymbol{x}}_k^{{\rm{HT}}},{\boldsymbol{x}}_k^{{\rm{HC}}}} \right]^{\text{T}}} \in {\mathbb{{\bf{R}}}^n}$， ${\boldsymbol{x}}_{k}^{{\rm{HT}}} = \left[ p_{x,k}^{{\rm{HT}}},p_{y,k}^{{\rm{HT}}}, $ $ \dot{p}_{x,k}^{{\rm{HT}}},\dot {p}_{y,k}^{{\rm{HT}}} \right]$为TV的状态变量，包含 $ x $、 $ y $方向上的位置和速度， ${\boldsymbol{x}}_k^{{\rm{HC}}} = {\left[ {p_{x,k}^{{\rm{HC}}},p_{y,k}^{{\rm{HC}}},\dot p_{x,k}^{{\rm{HC}}},\dot p_{y,k}^{{\rm{HC}}},\theta _k^{{\rm{HC}}},\dot \theta _k^{{\rm{HC}}}} \right]}$为CV的状态变量，包含 $ x $、 $ y $方向上的位置、速度、航向角和角速度； ${{\boldsymbol{F}}_k}$为系统状态转移矩阵， ${{\boldsymbol{F}}_k} \in {\mathbb{{\bf{R}}}^{n \times n}}$； ${{\boldsymbol{w}}_k}$为过程噪声并服从零均值方差为 ${\boldsymbol{Q}}$的高斯分布， ${{\boldsymbol{w}}_k} \in {\mathbb{{\bf{R}}}^n}$. 式(3)对应的条件概率模型为

(4) $ p({{\boldsymbol{x}}_k}|{{\boldsymbol{x}}_{k - 1}}) = N({{\boldsymbol{x}}_k};{{\boldsymbol{F}}_k}{{\boldsymbol{x}}_{k - 1}},{\boldsymbol{Q}}) . $

另外，假设系统初始状态 $ {{\boldsymbol{x}}_0} $服从均值为 $ {\hat {\boldsymbol{x}}_{0|0}} $、协方差矩阵为 $ {{\boldsymbol{P}}_{0|0}} $的高斯分布，即 $p({{\boldsymbol{x}}_0}) = N({{\boldsymbol{x}}_0}; $ $ {\hat {\boldsymbol{x}}_{0|0}},{{\boldsymbol{P}}_{0|0}})$.

定义如下系统测量方程：

(5) $ {\boldsymbol{y}}_k^1 = h\left( {{{\boldsymbol{x}}_k}} \right) + {\boldsymbol{v}}_k^1, {\boldsymbol{y}}_k^2 = {\boldsymbol{H}}{{\boldsymbol{x}}_k} + {\boldsymbol{v}}_k^2. $

式中： ${\boldsymbol{y}}_k^1 = {\left[ {{\boldsymbol{y}}_k^{{\rm{HT}}},{\boldsymbol{y}}_k^{{\rm{CT}}}} \right]^{\text{T}}} \in {\mathbb{{\bf{R}}}^{{m_1}}}$， ${\boldsymbol{y}}_k^{{\rm{HT}}}$、 ${\boldsymbol{y}}_k^{{\rm{CT}}}$分别表示TV在HV坐标系下的测量值和在CV坐标系下的测量值； ${\boldsymbol{y}}_k^2 = {\left[ {{\boldsymbol{y}}_k^{{\rm{HC}}},\theta _k^{{\rm{HC}}}} \right]^{\text{T}}} \in {\mathbb{{\bf{R}}}^{{m_2}}}$为CV在HV坐标系下的定位测量信息，包括 $ x $、 $ y $方向的位置和航向角； $h\left( \cdot \right)$为测量函数； ${\boldsymbol{H}}$为测量矩阵.

由于HV和CV基于各自车体坐标系感知TV状态，在测量方程中测量函数 $ h\left( {{{\boldsymbol{x}}_k}} \right) $涉及到的坐标转换如图2所示.

系统测量函数 $ h\left( {{{\boldsymbol{x}}_k}} \right) $与测量矩阵 $ {\boldsymbol{H}} $的表达式如下：

(6) $\begin{split} & h\left( {{{\boldsymbol{x}}_k}} \right) =\\ &\left[{\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} 1&0 \\ 0&1 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0&0 \\ 0&0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}} \\ {\boldsymbol{R}} &{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{ - {\boldsymbol{R}}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 0 \end{array}} \end{array}} \right]{{\boldsymbol{x}}_k}, \end{split}$

(7) $ {\boldsymbol{H}} = \left[ {\begin{array}{*{20}{c}} 0&0&0&0&1&0&0&0&0&0 \\ 0&0&0&0&0&1&0&0&0&0 \\ 0&0&0&0&0&0&0&0&1&0 \end{array}} \right] .$

式中： ${\boldsymbol{R}} = \left[ {\begin{array}{*{20}{c}} {\cos\, \theta _k^{{\rm{HC}}}}&{\sin\, \theta _k^{{\rm{HC}}}} \\ { - \sin\, \theta _k^{{\rm{HC}}}}&{\cos\, \theta _k^{{\rm{HC}}}} \end{array}} \right]$.

测量噪声 ${\boldsymbol{v}}_k^1 \in {\mathbb{{\bf{R}}}^{{m_1}}}$服从零均值、协方差矩阵为 ${\boldsymbol{\varSigma}} _k^1$的高斯分布，则似然概率密度函数为

(8) $ p\left( {{\boldsymbol{y}}_k^1|{{\boldsymbol{x}}_k}} \right) = N\left( {{\boldsymbol{y}}_k^1;h\left( {{{\boldsymbol{x}}_k}} \right),{\boldsymbol{\varSigma}} _k^1} \right). $

为了增强跟踪算法对定位数据中野值的鲁棒性，采用重尾非高斯多元学生t-分布建模定位测量噪声 ${\boldsymbol{v}}_k^2 \in {\mathbb{{\bf{R}}}^{{m_2}}}$，则似然概率密度函数 $ p\left( {{\boldsymbol{y}}_k^2|{{\boldsymbol{x}}_k}} \right) $表达式为

(9) $ p\left( {{\boldsymbol{y}}_k^2|{{\boldsymbol{x}}_k}} \right) = S\left( {{\boldsymbol{y}}_k^2;{\boldsymbol{H}}{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{\lambda _k}} \right). $

根据式(2)，引入自举变量 $ {u_k} $，式(9)可以被分解为

(10) $ \left. \begin{gathered} p\left( {{\boldsymbol{y}}_k^2|{{\boldsymbol{x}}_k},{u_k},{{\boldsymbol{\varLambda}} _k}} \right) = N\left( {{\boldsymbol{y}}_k^2;{\boldsymbol{H}}{{\boldsymbol{x}}_k},{\boldsymbol{\varSigma}} _k^2} \right), \hfill \\ {\boldsymbol{\varSigma}} _k^2 = {\left( {{u_k}{{\boldsymbol{\varLambda}} _k}} \right)^{ - 1}}, \hfill \\ p\left( {{u_k}|{\lambda _k}} \right) = G\left( {{u_k};{{{\lambda _k}}}{/2},{{{\lambda _k}}}{/2}} \right). \hfill \\ \end{gathered} \right\}$

假设精确度 ${{\boldsymbol{\varLambda}} _0}$和自由度 ${{{\lambda}} _0}$服从如下伽马先验：

(11) $ \left.\begin{gathered} p\left( {{{\boldsymbol{\varLambda}} _0}} \right) = \prod\nolimits_{l = 1}^{{m_2}} {G\left( {{{{\varLambda}} _{l,0}};{{\hat a}_{l,0}},{{\hat b}_{l,0}}} \right)}, \hfill \\ p\left( {{\lambda _0}} \right) = G\left( {{\lambda _0};{{\hat c}_0},{{\hat d}_0}} \right). \hfill \\ \end{gathered} \right\} $

式中： $ {\hat a_{l,0}} $、 $ {\hat b_{l,0}} $、 $ {\hat c_0} $和 $ {\hat d_0} $为超参数. 因此，协同目标跟踪的概率图模型如图3所示.

根据上述模型，选择状态变量 $ {{\boldsymbol{x}}_k} $，精确度 $ {{\boldsymbol{\varLambda}} _k} $，自举变量 $ {u_k} $和自由度 $ {\lambda _k} $为待估计的未知参数. 基于贝叶斯理论，可以推导后验分布 $p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)$：

(12) $ p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right) = \frac{{p\left( {{{\boldsymbol{Y}}_k}|{{{\varTheta}} _k}} \right)p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)}}{{p\left( {{{\boldsymbol{Y}}_k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)}} .$

式中： ${{{\varTheta}} _k} = \left\{ {{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{u_k},{\lambda _k}} \right\}$， ${\varDelta _k} = \left\{ {{a_k},{b_k},{c_k},{d_k}} \right\}$为噪声先验分布的超参数， $p\left( {{{\boldsymbol{Y}}_k}|{{{\varTheta}} _k}} \right)$为 ${{{\varTheta}} _k}$的似然函数， $p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)$为 ${{{\varTheta}} _k}$的先验分布， $p\left( {{{\boldsymbol{Y}}_k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)$为边缘似然函数. 在高维情况下，后验分布 $p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)$中的归一化因子 $p\left( {{{\boldsymbol{Y}}_k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right) = $ $ \displaystyle \int {p\left( {{{\boldsymbol{Y}}_k}|{{{\varTheta}} _k}} \right)} p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right){\text{d}}{{{\varTheta}} _k}$没有闭式解或者难以进行数值计算. 为了解决该问题，引入变分分布 $q({{{\varTheta}} _k})$以逼近真实的后验分布 $p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)$.应用平均场理论，假设变分分布 $q({{{\varTheta}} _k})$满足如下待估计参数组的因式分解：

(13) $ q({{{\varTheta}} _k}) = q\left( {{{\boldsymbol{x}}_k}} \right)q\left( {{{\boldsymbol{\varLambda}} _k}} \right)q\left( {{u_k}} \right)q\left( {{\lambda _k}} \right) . $

对数边缘似然函数 $\ln\, p\left( {{{\boldsymbol{Y}}_k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)$总有如下等式^[7]：

(14) $ \begin{split} \ln\, p\;&\left( {{{\boldsymbol{Y}}_k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right) = \hfill \\ &f\left[ {q({{{\varTheta}} _k})} \right] + {{{D}}_{{\text{KL}}}}\left[ {q\left( {{{{\varTheta}} _k}} \right)||p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)} \right] . \end{split} $

其中，

(15) $\begin{split} & f\left[ {q({{{\varTheta}} _k})} \right] = \\ & {\text{ }}\int {q\left( {{{{\varTheta}} _k}} \right)\ln\, \frac{{p\left( {{{\boldsymbol{Y}}_k}|{{{\varTheta}} _k}} \right)p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)}}{{q\left( {{{{\varTheta}} _k}} \right)}}} {\text{d}}{{{\varTheta}} _k} \text{，} \end{split} $

(16) $ {{\text{D}}_{{\text{KL}}}}\left[ {q\left( {{{{\varTheta}} _k}} \right)||p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)} \right] = \int {q\left( {{{{\varTheta}} _k}} \right)\ln\, \frac{{q\left( {{{{\varTheta}} _k}} \right)}}{{p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)}}} {\text{d}}{{{\varTheta}} _k} . $

式中： $ f\left[ {q({{{\varTheta}} _k})} \right] $为自由能； ${{{D}}_{{\text{KL}}}}\left[ {q||p} \right]$为Kullback-Leibler (KL)散度，表示分布 $ q $与 $ p $之间的差异. ${{{D}}_{{\text{KL}}}}\left[ {q\left( {{{{\varTheta}} _k}} \right)||p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)} \right]$非负，当且仅当两分布相同时， ${{{D}}_{{\text{KL}}}}\left[ {q\left( {{{{\varTheta}} _k}} \right)||p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)} \right] = 0$.

根据式(14)，可以通过最小化式(16)来寻找 $p\left( {{{\varTheta}} _k}| $ $ {{\boldsymbol{Y}}_{1:k}},{\varDelta _k} \right)$的变分后验分布 $ q\left( {{{{\varTheta}} _k}} \right) $. 然而， $p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k}},{\varDelta _k}} \right)$未知，无法直接最小化KL散度. 由式(14)可知对数边缘似然函数 $\ln p\left( {{{\boldsymbol{Y}}_k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)$与变分后验分布 $ q\left( {{{{\varTheta}} _k}} \right) $无关，因此，可以通过最大化自由能 $ f\left[ {q({{{\varTheta}} _k})} \right] $得到变分后验分布. 基于变分参数耦合的性质，对 $ {{{\varTheta}} _k} $进行分解，即 $ {{{\varTheta}} _k} = \left\{ {{{\varTheta}} _k^1,{{\varTheta}} _k^2,{{\varTheta}} _k^3,\cdots} \right\} $，从而可以交替优化 $ q\left( {{{\varTheta}} _k^i} \right) $. $ q\left( {{{\varTheta}} _k^i} \right) $的最优解^[19]如下：

(17) $ \ln\, q\left( {{{\varTheta}} _k^i} \right) \propto {{{E}}_{{{\varTheta}} _k^{ - i}}}\left[ {\ln \;\left[ {p\left( {{{\boldsymbol{Y}}_k}|{{{\varTheta}} _k}} \right)p\left( {{{{\varTheta}} _k}|{{\boldsymbol{Y}}_{1:k - 1}},{\varDelta _k}} \right)} \right]} \right]. $

式中： ${{\varTheta}} _k^{ - i} = \left\{ {{{\varTheta}} _k^1,\cdots,{{\varTheta}} _k^{i - 1},{{\varTheta}} _k^{i + 1},\cdots} \right\}$， ${{{E}}_{\varTheta _k^{ - i}}}\left[ \cdot \right]$为数学期望.

依据变分贝叶斯框架推导协同目标跟踪概率模型的变分后验分布. 由于测量函数 $ h\left( {{{\boldsymbol{x}}_k}} \right) $是一个非线性方程，依据扩展卡尔曼滤波原理^[20]进行泰勒展开，并略去二次及二次以上项，得到非线性系统的线性化模型，测量函数 $ h\left( {{{\boldsymbol{x}}_k}} \right) $的雅可比矩阵为

(18) $ {{\boldsymbol{J}}_k} = \frac{{\partial h\left( {{{\boldsymbol{x}}_k}} \right)}}{{\partial {{\boldsymbol{x}}_k}}}\left| {_{{{\boldsymbol{x}}_k} = {{\hat {\boldsymbol{x}}}_{k|k - 1}}}} .\right. $

式中： ${{\hat {\boldsymbol{x}}}_{k|k - 1}}$为由 $ k - 1 $时刻预测的 $ k $时刻系统状态.

为了简便起见，将测量向量 $ {\boldsymbol{y}}_k^1 $、 $ {\boldsymbol{y}}_k^2 $、测量矩阵 $ {{\boldsymbol{J}}_k} $、 $ {\boldsymbol{H}} $和协方差矩阵 ${\boldsymbol{\varSigma}} _k^1$、 ${\boldsymbol{\varSigma}} _k^2$组合成新的增广测量向量、矩阵和协方差矩阵：

(19) $ {{\boldsymbol{y}}_k} = {\left[ {{\boldsymbol{y}}_k^1,{\boldsymbol{y}}_k^2} \right]^{\rm T}} , {{\bar {\boldsymbol{H}}}_k} = {\left[ {{{\boldsymbol{J}}_k},{\boldsymbol{H}}} \right]^{\rm T}} , {{\boldsymbol{\varSigma}} _k} = {\text{diag}}\,\left( {\left[ {{\boldsymbol{\varSigma}} _k^1,{\boldsymbol{\varSigma}} _k^2} \right]} \right). \hfill \\ $

经上述处理后，新的测量方程表示如下：

(20) $ {{\boldsymbol{y}}_k} = {\bar {\boldsymbol{H}}_k}{{\boldsymbol{x}}_k} + {{\boldsymbol{v}}_k} . $

根据链式法则，利用分层高斯状态空间模型的条件独立性，将模型的联合概率分布函数表达为

(21) $ \begin{split} p&\left({{{\boldsymbol{y}}_{1:k}},{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{u_k},{{{\lambda}} _k},{\varDelta _k}} \right) = \hfill \\ &p\left( {{{\boldsymbol{y}}_k}|{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{u_k},{{{\lambda}} _k}} \right)p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{u_k},{{{\lambda}} _k}|{{\boldsymbol{y}}_{1:k - 1}},{\varDelta _k}} \right) . \end{split}$

根据如图3所示的概率图模型，式(21)中未知参数的似然函数为

(22) $ p\left( {{{\boldsymbol{y}}_k}|{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{u_k},{\lambda _k}} \right) = N\left( {{{\boldsymbol{y}}_k};{{\bar {\boldsymbol{H}}}_k}{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}} \right). $

未知参数 ${{\boldsymbol{x}}_k}$、 ${{\boldsymbol{\varLambda}} _k}$、 $ {u_k} $和 $ {\lambda _k} $的先验分布可以分解为如下形式：

(23) $ \begin{split} p &\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varLambda}} _k},{u_k},{\lambda _k}|{{\boldsymbol{y}}_{1:k - 1}},{\varDelta _k}} \right) = \hfill \\ &p\left( {{{\boldsymbol{x}}_k}|{{\boldsymbol{y}}_{1:k - 1}}} \right)p\left( {{{\boldsymbol{\varLambda}} _k}|{\varDelta _k}} \right)p\left( {{u_k}|{\lambda _k}} \right)p\left( {{\lambda _k}|{\varDelta _k}} \right) . \end{split}$

对于状态变量 ${{\boldsymbol{x}}_k}$，有

(24) $ {\text{ }}p\left( {{{\boldsymbol{x}}_k}|{{\boldsymbol{y}}_{1:k - 1}}} \right) = N\left( {{{\boldsymbol{x}}_k};{{\hat {\boldsymbol{x}}}_{k|k - 1}},{{\boldsymbol{P}}_{k|k - 1}}} \right) . $

式中： $ {\hat {\boldsymbol{x}}_{k|k - 1}} $和 $ {{\boldsymbol{P}}_{k|k - 1}} $表示在 $ k - 1 $时刻，利用卡尔曼滤波预测方程获得的 $ k $时刻的预测状态和协方差.

(25) $ \left. \begin{gathered} {{\hat {\boldsymbol{x}}}_{k|k - 1}} = {{\boldsymbol{F}}_k}{{\hat {\boldsymbol{x}}}_{k - 1|k - 1}}, \hfill \\ {{\boldsymbol{P}}_{k|k - 1}} = {{\boldsymbol{F}}_k}{{\boldsymbol{P}}_{k - 1|k - 1}}{\boldsymbol{F}}_k^{\rm T} + {\boldsymbol{Q}}. \hfill \\ \end{gathered} \right\}$

对于每一个参数的后验分布，有

(26) $ \left.\begin{gathered} p\left( {{{\boldsymbol{\varLambda}} _k}|{\varDelta _k}} \right) = \prod\nolimits_{l = 1}^{{m_2}} {G\left( {{{{\varLambda}} _{l,k}};a_{l,k}^ - ,b_{l,k}^ - } \right)}, \hfill \\ p\left( {{u_k}|{\lambda _k}} \right) = G\left( {{u_k};{{{\lambda _k}}}{/2},{{{\lambda _k}}}{/2}} \right), \hfill \\ p\left( {{\lambda _k}|{\varDelta _k}} \right) = G\left( {{\lambda _k};c_k^ - ,d_k^ - } \right) .\hfill \\ \end{gathered} \right\} $

式中： $ a_{l,k}^ - $、 $ b_{l,k}^ - $、 $ c_k^ - $和 $ d_k^ - $为 $ k $时刻先验分布 ${{{\varLambda}} _{l,k}}$和 $ {\lambda _k} $的预测超参数.

根据文献[11]，定位测量噪声协方差的预测分布参数可以设置为

(27) $ {a}_{l,k}^{-}=\rho {\hat{a}}_{l,k-1}\text{，}{b}_{l,k}^{-}=\rho {\hat{b}}_{l,k-1}\text{，}{c}_{k}^{-}=\rho {\hat{c}}_{k-1}\text{，}{d}_{k}^{-}=\rho {\hat{d }}_{k-1}. $

式中： $\rho \in \left( {0,\left. 1.0 \right]} \right.$为遗忘因子，反映噪声统计特性的波动性.

根据变分贝叶斯推断原理推导系统状态和噪声参数的后验分布，式(21)中对数联合概率分布函数可以表示为

(28) $\begin{split} \mathrm{ln}\;p\;&\left({{\boldsymbol{y}}}_{1:k},{{\boldsymbol{x}}}_{k},{{\boldsymbol{\varLambda}} }_{k},{u}_{k},{\lambda }_{k},{\varDelta }_{k}\right)=\mathrm{ln}\;N\left({{\boldsymbol{y}}}_{k}^{1};{{\boldsymbol{J}}}_{k}{{\boldsymbol{x}}}_{k},{{\boldsymbol{\varSigma}} }_{k}^{1}\right)+\\ \;&\mathrm{ln}\;N\left({{\boldsymbol{y}}}_{k}^{2};{\boldsymbol{H}}{{\boldsymbol{x}}}_{k}\text{，}{\left({u}_{k}{{\boldsymbol{\varLambda}} }_{k}\right)}^{-1}\right)+\mathrm{ln}\;N\left({{\boldsymbol{x}}}_{k};{\hat{{\boldsymbol{x}}}}_{k|k-1},{{\boldsymbol{P}}}_{k|k-1}\right)+\\ \;&{\displaystyle \sum _{l=1}^{{m}_{2}}\mathrm{ln}\;G\left({{{\varLambda}}}_{l,k};{a}_{l,k}^-,{b}_{l,k}^-\right)}+\mathrm{ln}\;G\left({u}_{k};\frac{{\lambda }_{k}}{2},\frac{{\lambda }_{k}}{2}\right)+\\ \;&\mathrm{ln}\;G\left({\lambda }_{k};{c}_{k}^-,{d}_{k}^-\right).\\[-12pt] \end{split} $

依据卡尔曼滤波，更新参数 $ {\hat {\boldsymbol{x}}_{k\left| k \right.}} $和 $ {{\boldsymbol{P}}_{k\left| k \right.}} $：

(29) $ {\left( {\boldsymbol{\varSigma} _k^2} \right)^r} = {\left( {{{E}}_{{u_k}}^{r - 1}\left[ {{u_k}} \right]{{E}}_{{\boldsymbol{\varLambda} _k}}^{r - 1}\left[ {{{\boldsymbol{\varLambda}} _k}} \right]} \right)^{ - 1}}, $

(30) $ \boldsymbol{\varSigma} _k^r = {\text{diag}}\,\left( {\left[ {\boldsymbol{\varSigma} _k^1,{{\left( {\boldsymbol{\varSigma} _k^2} \right)}^r}} \right]} \right), $

(31) $ {\boldsymbol{K}}_k^r = {{\boldsymbol{P}}_{k\left| {k - 1} \right.}}\bar {\boldsymbol{H}}_k^{\rm T}{\left( {{{\bar {\boldsymbol{H}}}_k}{{\boldsymbol{P}}_{k\left| {k - 1} \right.}}\bar {\boldsymbol{H}}_k^{\rm T} + \boldsymbol{\varSigma} _k^r} \right)^{ - 1}}, $

(32) $ \hat {\boldsymbol{x}}_{k\left| k \right.}^r = {\hat {\boldsymbol{x}}_{k\left| {k - 1} \right.}} + {\boldsymbol{K}}_k^r\left( {{{\boldsymbol{y}}_k} - {{\bar {\boldsymbol{H}}}_k}{{\hat {\boldsymbol{x}}}_{k\left| {k - 1} \right.}}} \right), $

(33) $ {\boldsymbol{P}}_{k\left| k \right.}^r = {{\boldsymbol{P}}_{k\left| {k - 1} \right.}} - {\boldsymbol{K}}_k^r{\bar {\boldsymbol{H}}_k}{{\boldsymbol{P}}_{k\left| {k - 1} \right.}}. $

式中： $ * {\text{ }}_k^r $为k时刻第r次变分迭代的参数值，比如， $ {\boldsymbol{K}}_k^r $为k时刻第r次变分迭代的卡尔曼增益矩阵.

第r次变分迭代，变分分布 $ {q^r}\left( {{{\boldsymbol{\varLambda}} _k}} \right) $满足：

(34) $ \begin{split} \ln\, {q^r}\left( {{{\boldsymbol{\varLambda}} _k}} \right) \propto\;& \frac{1}{2}{\sum}_{l=1}^{m_2} \ln\, {{{\varLambda}} _{l,k}} - \frac{1}{2}{\text{Tr}}\;\left( {{{E}}_{{u_k}}^{r - 1}\left[ {{u_k}} \right]{{\boldsymbol{\varLambda}} _k}{\boldsymbol{C}}_k^r} \right) + \hfill \\ &\sum\nolimits_{l = 1}^{{m_2}} {\left[ {\left( {a_{l,k}^ - - 1} \right)\ln\, {{{\varLambda}} _{l,k}} - b_{l,k}^ - {{{\varLambda}} _{l,k}}} \right]}. \end{split} $

式中： ${\boldsymbol{C}}_k^r = \left( {{\boldsymbol{y}}_k^2 - {\boldsymbol{H}}\hat {\boldsymbol{x}}_{k\left| k \right.}^r} \right){\left( {{\boldsymbol{y}}_k^2 - {\boldsymbol{H}}\hat {\boldsymbol{x}}_{k\left| k \right.}^r} \right)^{\text{T}}} + {\boldsymbol{HP}}_{k\left| k \right.}^r{{\boldsymbol{H}}^{\rm T}}$.

可以发现， ${q^r}\left( {{{\boldsymbol{\varLambda}} _k}} \right)$服从标准伽马分布 ${q^r}\left( {{{\boldsymbol{\varLambda}} _k}} \right) = \displaystyle \sum\nolimits_{l=1}^{{m_2}} {G\left( {{{{\varLambda}} _{l,k}};\hat a_{l,k}^r,\hat b_{l,k}^r} \right)}$，则变分迭代步骤中第r次参数计算如下：

(35) $ \left. \begin{gathered} \hat a_{l,k}^r = a_{l,k}^ - + \frac{1}{2}, \hfill \\ \hat b_{l,k}^r = b_{l,k}^ - + \frac{1}{2}{\text{Tr}}\,\left( {{{E}}_{{u_k}}^{r - 1}\left[ {{u_k}} \right]{{\left( {{\boldsymbol{C}}_k^r} \right)}_l}} \right), \hfill \\ {{E}}_{{{\boldsymbol{\varLambda}} _k}}^r\left[ {{{\boldsymbol{\varLambda}} _k}} \right] = {\text{diag}}\,\left( {\left[ {{{{{E}}}}_{{{{\varLambda}} _{1,k}}}^r\left[ {{{{\varLambda}} _{1,k}}} \right],{{{{E}}}}_{{{{\varLambda}} _{2,k}}}^r\left[ {{{{\varLambda}} _{2,k}}} \right],\cdots} \right]_{l = 1}^{{m_2}}} \right). \hfill \\ \end{gathered} \right\} $

式中： ${{E}}_{{{{\varLambda}} _{l,k}}}^r\left[ {{{{\varLambda}} _{l,k}}} \right] = {{\hat a_{l,k}^r}}/{{\hat b_{l,k}^r}}$， ${{{\varLambda}} _{l,k}}$为矩阵 $ {{\boldsymbol{\varLambda}} _{k}} $的第l个元素.

第r次变分迭代，变分分布 $ {q^r}\left( {{u_k}} \right) $满足：

(36) $\begin{split} \ln\, {q^r}\left( {{u_k}} \right) \propto\;& \frac{1}{2}\ln\, {u_k} + \left( {\frac{{{\lambda _k}}}{2} - 1} \right)\ln\, {u_k} - \hfill \\ &\frac{{{\lambda _k}}}{2}{u_k} - \frac{1}{2}{\text{Tr}}\,\left( {{u_k}{ E}_{{{\boldsymbol{\varLambda}} _k}}^r\left[ {{{\boldsymbol{\varLambda}} _k}} \right]{\boldsymbol{C}}_k^r} \right) . \end{split} $

可以发现， $ {q^r}\left( {{u_k}} \right) $服从标准伽马分布 ${q^r}\left( {{u_k}} \right) = $ $ G\left( {{u_k};\hat u_{a,k}^r,\hat u_{b,k}^r} \right)$，则变分迭代步骤中第r次参数计算如下：

(37) $ \left. \begin{gathered} \hat u_{a,k}^r = \frac{{{{E}}_{{\lambda _k}}^{r - 1}\left[ {{\lambda _k}} \right] + 1}}{2}, \hfill \\ \hat u_{b,k}^r = \frac{{{{E}}_{{\lambda _k}}^{r - 1}\left[ {{\lambda _k}} \right] + {\text{Tr}}\,\left( {{{E}}_{{{\boldsymbol{\varLambda}} _k}}^r\left[ {{{\boldsymbol{\varLambda}} _k}} \right]{\boldsymbol{C}}_k^r} \right)}}{2}, \hfill \\ {{E}}_{{u_k}}^r\left[ {{u_k}} \right] = {{\hat u_{a,k}^r}}/{{\hat u_{b,k}^r}}. \hfill \\ \end{gathered} \right\}$

第r次变分迭代，变分分布 $ {q^r}\left( {{\lambda _k}} \right) $满足：

(38) $ \begin{split} \ln\, {q^r}\left( {{\lambda _k}} \right) \propto\;& - \ln\, \varGamma \left( {\frac{{{\lambda _k}}}{2}} \right) + \frac{{{\lambda _k}}}{2}\ln\, \frac{{{\lambda _k}}}{2} - \frac{{{\lambda _k}}}{2}{{E}}_{{u_k}}^r\left[ {{u_k}} \right] + \hfill \\ &\left( {\frac{{{\lambda _k}}}{2} - 1} \right){{E}}_{{u_k}}^r\left[ {\ln\, {u_k}} \right] + \left( {c_k^ - - 1} \right)\ln\, {\lambda _k} - d_k^ - {\lambda _k} . \end{split} $

根据斯特林近似公式 $\ln\, \varGamma \left( {\dfrac{{{\lambda _k}}}{2}} \right) \approx \left( {\dfrac{{{\lambda _k}}}{2} - 1} \right) \times $ $ \ln \dfrac{{{\lambda _k}}}{2} - \dfrac{{{\lambda _k}}}{2}$，有

(39) $ \begin{split} \ln\, {q^r}\left( {{\lambda _k}} \right) \propto\;& \left( {1 + {{E}}_{{u_k}}^r\left[ {\ln\, {u_k}} \right] - {{E}}_{{u_k}}^r\left[ {{u_k}} \right]} \right)\frac{{{\lambda _k}}}{2} + \hfill \\ &\left( {c_k^ - - \frac{1}{2}} \right)\ln\, {\lambda _k} - d_k^ - {\lambda _k} . \end{split}$

可以发现， $ {q^r}\left( {{\lambda _k}} \right) $服从标准伽马分布 ${q^r}\left( {{\lambda _k}} \right) = $ $ G\left( {{\lambda _k};\hat c_k^r,\hat d_k^r} \right)$，则变分迭代步骤中第r次参数计算如下：

(40) $ \left. \begin{gathered} \hat c_k^r = c_k^ - + \frac{1}{2}, \hfill \\ \hat d_k^r = d_k^ - - \frac{1}{2}\left( {1 + {{E}}_{{u_k}}^r\left[ {\ln\, {u_k}} \right] - {{E}}_{{u_k}}^r\left[ {{u_k}} \right]} \right), \\ {{E}}_{{\lambda _k}}^r\left[ {{\lambda _k}} \right] = {{\hat c_k^r}}/{{\hat d_k^r}}. \hfill \\ \end{gathered} \right\} $

在变分迭代更新步骤中，须交替优化变分参数. 考虑到计算效率，提出如下终止条件.

条件1 如果2次相邻迭代，系统状态的变化量 $ \varepsilon $小于或等于收敛阈值 $ E $，则终止变分迭代，即

(41) $ \varepsilon = \frac{{||\hat {\boldsymbol{x}}_{k|k}^{r + 1} - \hat {\boldsymbol{x}}_{k|k}^r|{|_2}}}{{||\hat {\boldsymbol{x}}_{k|k}^{r + 1}|{|_2}}} \leqslant E . $

式中： $ || \cdot |{|_2} $表示向量之间的欧几里得距离.

条件2 如果变分迭代的次数达到设置的最大迭代次数 $ B $，则终止变分迭代，条件即

(42) $ r \geqslant B . $

最终，本研究提出的协同目标跟踪算法变分贝叶斯-鲁棒协同跟踪(variational Bayesian-robust cooperative tracking, VB-RCT)算法流程如下.

算法：VB-RCT算法

输入：$ {\hat {\boldsymbol{x}}_{k - 1\left| k \right. - 1}} $、 $ {{\boldsymbol{P}}_{k - 1\left| {k - 1} \right.}} $、 $ {{\boldsymbol{F}}_k} $、 $ {\boldsymbol{Q}} $、 $ {{\boldsymbol{y}}_k} $、 $ {\bar {\boldsymbol{H}}_k} $、 ${{\boldsymbol{\varSigma}} _k}$、 $\; \rho $、 $ K $、 $ B $、 $ E $

初始化：$ {\hat {\boldsymbol{x}}_{0|0}} $, $ {{\boldsymbol{P}}_{0|0}} $, ${\varDelta _0} = \left\{ {{{\hat a}_{l,0}},{{\hat b}_{l,0}},{{\hat c}_0},{{\hat d}_0}} \right\}_{l = 1}^{{m_2}}$, ${{{E}}_{{{\boldsymbol{\varLambda }}_0}}}\left[ {{{\boldsymbol{\varLambda}} _0}} \right]$,

$\small{ {{{E}}_{{u_0}}}\left[ {{u_0}} \right] , {{{E}}_{{\lambda _0}}}\left[ {{\lambda _0}} \right] , {{\boldsymbol{\varSigma}} _0}} $

For $ k = 1:K $ do

状态预测：式(25)、(27)

变分迭代更新：

For $ r = 1:B $ do

更新状态：式(29) ~ (33)

更新变分参数：式(35)、(37)、(40)

变分迭代终止条件：式(41)、(42)

End for

End for

输出：$ \left\{ {{{\hat {\boldsymbol{x}}}_{k\left| k \right.}},{{\boldsymbol{P}}_{k\left| k \right.}}} \right\}_{k = 1}^K $

为了验证VB-RCT算法的有效性与鲁棒性，对4种算法进行性能对比仿真实验.1）VB-RCT算法；2）非协同、仅依赖HV估计TV状态的单车跟踪(Kalman filter-single tracking，KF-ST)算法；3）基于扩展卡尔曼滤波，假定定位噪声协方差为固定值的协同跟踪(extend Kalman filter-cooperative tracking，EKF-CT)算法；4）定位噪声协方差建模为逆伽马分布的变分协同跟踪(variational Bayesian-cooperative tracking，VB-CT)^[21]算法. 另外，对VB-RCT算法中相关参数的影响进行实验分析.

在线协同目标跟踪仿真场景如图4所示，假设TV和CV根据以下状态空间模型在二维空间中以恒定的速度运动：

(43) $ \left. \begin{split} {{\boldsymbol{x}}_k} = {{\boldsymbol{F}}_k}{{\boldsymbol{x}}_{k - 1}} + {{\boldsymbol{w}}_k},\hfill \\ {{\boldsymbol{y}}_k} = {\bar {\boldsymbol{H}}_k}{{\boldsymbol{x}}_k} + {{\boldsymbol{v}}_k}. \end{split} \right\} $

式中： ${{\boldsymbol{F}}_k}$为状态转移矩阵， ${{\boldsymbol{F}}_k} = {\text{diag}} \, \left( \left[ {{\boldsymbol{F}}^{(1)}}, $ $ {{\boldsymbol{F}}^{(1)}},{{\boldsymbol{F}}^{(2)}} \right] \right)$， $ {{\boldsymbol{F}}^{(1)}} = \left[ {\begin{array}{*{20}{c}} 1&0&{\Delta t}&0 \\ 0&1&0&{\Delta t} \\ 0&0&1&0 \\ 0&0&0&1 \end{array}} \right] $， $ {{\boldsymbol{F}}^{(2)}} = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t} \\ 0&1 \end{array}} \right] $， $ \Delta t = 0.1 $为采样周期参数；设置TV和CV的初始状态为 $ {\left[ {3.0,0,1.1,0} \right]^{\text{T}}} $和 $ {\left[ {10,0,1,0,0,0} \right]^{\text{T}}} $，CV的航向角 $\theta _k^{{\rm{HC}}} = {\tan ^{ - 1}}\,{{\dot p_{y,k}^{{\rm{HC}}}}}/{{\dot p_{x,k}^{{\rm{HC}}}}} + \dfrac{1}{2}\sin\; ({k}/{{100}})$； ${{\boldsymbol{w}}_k}$为过程噪声，服从零均值协方差矩阵为 ${\boldsymbol{Q}}$的高斯白噪声， ${\boldsymbol{Q}} = $ $ {\text{diag}} \; \left( {\left[ {{{{{\boldsymbol{I}}}}_2} \otimes {{0.01}^2}{\boldsymbol{G}}{{\boldsymbol{G}}^{\text{T}}},{{0.01}^2}{{\boldsymbol{G}}_0}{{\boldsymbol{G}}_0}^{\text{T}}} \right]} \right)$， ${{\boldsymbol{I}}_n}$为 $ n $维单位矩阵， $ \otimes $表示克罗内克积， ${\boldsymbol{G}} = {{\boldsymbol{I}}_2} \otimes {{\boldsymbol{G}}_0}$， $ {{\boldsymbol{G}}_0} = \left[ {\begin{array}{*{20}{c}} {{{\Delta {t^3}}}/{3}}&{{{\Delta {t^2}}}/{2}} \\ {{{\Delta {t^2}}}/{2}}&{\Delta t} \end{array}} \right] $； $ {\boldsymbol{v}}_k^1 $为测量噪声，其协方差矩阵为 ${\boldsymbol{\varSigma}} _k^1 = {\text{diag}}\,\left( \left[ 0.5,0.5, $ $ 0.5,0.5 \right] \right)$； $ {\boldsymbol{v}}_k^2 $为含有野值的定位数据噪声，服从如下概率分布：

(44) $ p\left( {{\boldsymbol{v}}_k^2} \right) = \left\{ \begin{gathered} N\left( {{\boldsymbol{v}}_k^2;{\boldsymbol{0}},{\boldsymbol{M}}} \right),\;\;\;\;{\rm{w.p}}.{\text{ 1 }}-\eta \,;\hfill \\ S\left( {{\boldsymbol{v}}_k^2;{\boldsymbol{0}},{\boldsymbol A},15} \right),\;\;\;\;{\rm{w.p}}.{\text{ }}\eta \,.\hfill \\ \end{gathered} \right.{\text{ }} $

式中： ${\boldsymbol{M}} = {\text{diag}}\,\left( {\left[ {0.1,0.1,0.01} \right]} \right)$， $ \eta $为野值比率， $ {\boldsymbol A} $为野值的异常程度， ${\rm{ w.p}}. $表示按概率采样.

变分贝叶斯迭代的终止条件参数设置为： $ E = 5 \times {10^{ - 6}} $、最大迭代次数 $ B = 10 $. 为了消除随机性的影响，执行 $ N $次蒙特卡洛(Monte-Carlo，MC)循环. 利用位置和速度的均方根误差(root mean square error，RMSE)和平均均方根误差(average root mean square error，ARMSE)衡量不同算法的跟踪性能，位置的RMSE与ARMSE的表达式如下：

(45) $ \left. \begin{split} p_k^{{\text{RMSE}}} = {\left\{ {\frac{1}{N}\sum\limits_{n = 1}^N {\left[ {{{\left( {{p_{x,k}} - \hat p_{x,k}^n} \right)}^2} + {{\left( {{p_{y,k}} - \hat p_{y,k}^n} \right)}^2}} \right]} } \right\}^{1/2}} \text{，}\hfill \\ p_{}^{{\text{ARMSE}}} = \frac{1}{K}\sum\limits_{k = 1}^K {p_k^{{\text{RMSE}}}}. \end{split} \right\}$

式中： $ \left( {{p_{x,k}},{p_{y,k}}} \right) $表示目标在k时刻的真实位置； $ \left( {\hat p_{x,k}^n,\hat p_{y,k}^n} \right) $表示第n次MC循环时目标在k时刻的估计位置；N=100表示总的MC次数；K=400表示总的运行时间步，即40 s. 速度的RMSE和ARMSE计算方法类似.

设置野值比率 $\eta = 6 \text{%}$，异常程度 ${\boldsymbol A} = 25{\boldsymbol{R}}$，遗忘因子 $ \rho = 0.95 $，各种跟踪算法在线估计目标位置、速度的RMSE变化曲线如图5所示，对应算法的位置和速度的ARMSE如表1所示. 表中， $ {\varphi _1} $表示位置， $ {\chi _1} $表示位置估计的相对提升量， $ {\varphi _2} $表示速度， $ {\chi _2} $表示速度估计的相对提升量. 可以看出，EKF-CT、VB-CT和VB-RCT算法利用HV与CV的测量值实现数据融合，明显优于KF-ST算法，能有效改善目标跟踪的精度. 考虑到野值的存在，定位数据呈现重尾非高斯特征，依赖于高斯分布假设的EKF-CT算法，将定位噪声协方差仍设为固定值，无法自适应地处理野值引起的协方差波动问题，导致性能劣于VB-CT和VB-RCT算法. VB-CT算法可以自适应调整定位噪声协方差，但对野值较敏感. 当野值比率较大时，VB-CT算法跟踪能力急剧下降. 本研究提出的VB-RCT算法，与传统单车目标跟踪算法相比，在位置和速度ARMSE上，性能分别提升17.4%和14.2%.

某一时刻的定位噪声是否为野值点与前、后时刻的数据无必然联系. 比较常见的是，当某个时刻噪声为野值点时，在该时刻的一个邻域内的数据是正常的，即野值点的出现是随机、孤立的. 基于 $ \eta $=2%、6%、10%、14% 4种随机野值比率，模拟野值噪声百分比逐渐增加的情况，位置ARMSE随野值比率的变化曲线如图6所示.

在上述实验中，野值定位噪声建模为固定异常程度参数值的学生t-分布，在野值比率 $ \eta = 6 \text{%} $的情况下，设置低( $ {\boldsymbol A} = 15{\boldsymbol{R}} $)、中( $ {\boldsymbol A} = 25{\boldsymbol{R}} $)、高( $ {\boldsymbol A} = $ $ 35{\boldsymbol{R}} $)3种异常程度模拟噪声的干扰程度. 如图7所示为3种异常程度定位噪声下，CV位置的测量值. 可以看出，随着异常程度的增加，野值偏离程度明显加重. 进一步，设置3种重尾分布模拟野值定位噪声分布：精确度 ${\boldsymbol {A}} \in {\mathbb{{\bf{R}}}^{3 \times 3}}$的学生t-分布 $ S\left( {{\boldsymbol{v}}_k^2;{\boldsymbol{0}},{\boldsymbol A},15} \right) $、范围参数 $ {\boldsymbol{a}} > {\boldsymbol{0}} $的均匀分布 $ U\left( {{\boldsymbol{v}}_k^2; - {\boldsymbol{a}},{\boldsymbol{a}}} \right) $和尺度参数 $ {\boldsymbol{b}} > {\boldsymbol{0}} $的拉普拉斯 $ L\left( {{\boldsymbol{v}}_k^2;{\boldsymbol{0}},{\boldsymbol{b}}} \right) $. 分析不同 $ {\boldsymbol A} $、 $ {\boldsymbol{a}} $和 $ {\boldsymbol{b}} $值下4种算法的估计性能，实验结果和位置ARMSE如图8和表2所示. 可以看出，在所有重尾非高斯定位噪声分布下，KF-ST的跟踪精度明显低于EKF-CT、VB-CT和VB-RCT，而且由于没有涉及到协同车定位噪声，噪声异常程度的变化并没有引起ARMSE的变化. EKF-CT和VB-CT方法的跟踪精度都随着异常程度增加而降低，本研究所提出的VB-RCT算法呈现出稳定趋势，精度逐渐更优，充分证明了本研究算法的鲁棒性.

遗忘因子 $ \rho $用于根据 $ k - 1 $时刻定位噪声超参数 ${\varDelta _{k - 1}}$预测 $ k $时刻定位噪声超参数的先验分布，会影响在线变分推断的结果. 为了研究遗忘因子对跟踪性能的影响，设置遗忘因子以步长0.05的变化率从0.75增加到1.00在不同 $\; \rho $下，位置ARMSE变化曲线如图9所示. 可以看出，当 $\; \rho $较小时，跟踪误差性能较差，这是由于变分贝叶斯算法对参数的过度自修剪，导致前一时刻的状态信息没有得到充分利用；当 $ \;\rho $太大时，信息的过度保留影响当前时刻的状态更新，导致跟踪性能恶化. 结果表明，当 $ \;\rho = 0.95 $时，提出的算法达到最优跟踪性能.

变分内循环中的迭代次数 $ B $也是影响目标跟踪效果的重要因素. 通常，随着迭代次数的增加，变分分布可以更精确地近似真实的后验分布，从而获得更好的跟踪性能. 然而，迭代次数过大将降低计算效率和跟踪速度. 基于 $\; \rho = 0.95 $，设置最大变分迭代次数 $ B $从1增加到10，研究跟踪误差随迭代次数增加的变化情况，结果如图10所示. 可以看出，在线跟踪误差在前几轮变分迭代迅速减小，具有较快的收敛速度.

针对协同目标跟踪中定位数据统计特征未知且存在野值的问题，本研究提出一种鲁棒协同目标跟踪算法，将定位噪声建模为具有重尾的多元学生t-分布，建立目标状态与定位噪声参数的贝叶斯模型. 应用变分贝叶斯推断原理对变量进行解耦，以交替优化的方式实现后验分布的有效估计. 计算机仿真结果表明，与传统单车目标跟踪算法相比，提出的鲁棒跟踪算法能有效地提升目标跟踪的性能，位置和速度估计误差分别下降了17.4%和14.2%. 在未来工作中，将在真实环境下测试所提出方法的跟踪效果.