1.   引言

目标跟踪是一种利用传感器原始量测信息获取实时目标运动状态的数据处理方法，该方法广泛应用于军事监测、航空航天、智能交通等诸多领域。非线性滤波是目标跟踪的常见手段之一，标准的非线性滤波方法只考虑随机误差的干扰，因此当传感器存在测量偏差^[1,2]时，且测量偏差远大于随机误差时，目标跟踪精度将急剧恶化。

为提升测量偏差存在时的目标跟踪精度，文献[3]利用最大似然和期望最大化(Expectation-Maximization, EM)方法实现对多个传感器的测量偏差和单目标状态的联合估计，但该方法采用离线方式对量测数据进行批处理，在一些特定场景下，不满足实时性需求；文献[4]将目标状态和测量偏差联合扩维形成增广状态向量，利用卡尔曼滤波实现二者的实时估计；针对传感器时间非同步问题，文献[5]设计了一种两步融合的方法来同时估计目标状态和测量偏差。上述方法均假设同一目标可被多个传感器观测到，从而利用多个传感器与目标之间的位置关系建立估计方程，并不适用于单一传感器场景。

针对单传感器场景下的有偏量测目标跟踪问题，文献[6]利用变分贝叶斯理论设计了一种带有偏差检测与消除机制的非线性卡尔曼滤波方法，可在偏差恒定或突变的场景下对目标状态进行实时有效估计，但是该方法假设量测噪声协方差已知，在实际应用中具有一定的局限性；文献[7]提出了一种新的鲁棒卡尔曼滤波方法，该方法同时考虑了时变的测量偏差和厚尾量测噪声，将测量偏差和厚尾噪声联合建模为学生T逆Wishart分布，利用变分贝叶斯方法求解后可实时估计目标状态和测量偏差，但从实验结果可以看出，其对偏差的估计有一定的时间延迟；针对传感器存在未知测量偏差的欠观测系统，文献[8]提出一种增量卡尔曼滤波方法，该方法利用测量偏差恒定不变或缓变的性质，通过相邻时刻量测差分的方式消除未知偏差，同时构建增量量测方程，并推导了新的滤波方程。文献[9–13]在文献[8]的基础上提出了多种非线性增量滤波方法，但是上述基于增量的滤波方法均无法处理测量偏差随机突变且量测噪声未知的目标跟踪问题。

为有效解决测量偏差突变且量测噪声未知场景下的单传感器目标跟踪问题，本文提出一种偏差未补偿的自适应边缘化容积卡尔曼滤波跟踪方法。(1) 首先对相邻时刻的量测进行差分来构建差分量测方程，从而有效消除相邻时刻保持不变或缓变测量偏差的影响，同时，为实现目标状态的实时估计，将相邻时刻目标状态进行增广以构建扩展状态向量^[14,15]；(2) 量测差分后，突变的测量偏差将会使得当前时刻的差分量测成为离群值，通过构建Beta-Bernoulli指示变量^[16]来判断偏差是否突变，若突变，则舍弃该时刻的差分量测，并用预测状态代替更新状态，若未发生突变，则正常滤波；未知的高斯量测噪声经过差分后，依然满足高斯分布，因此可用逆Wishart分布建模其协方差矩阵；(3)建立目标状态、指示变量和量测噪声的联合分布，并通过变分贝叶斯方法推导了各参数的近似后验；(4)为解决扩展状态向量维度较大导致滤波负担增大的问题，对扩展目标状态进行边缘化处理^[17]，并结合容积卡尔曼方法实现边缘化容积卡尔曼滤波跟踪；(5)仿真实验表明，与现有方法相比，本文方法在存在突变测量偏差和未知时变量测噪声场景下具有更好的跟踪性能。

2.   系统模型

目标在2维平面上运动，目标的运动方程和传感器观测方程为

其中，${{\boldsymbol{x}}_k} = {[p_k^x,v_k^x,p_k^y,v_k^y]^{\text{T}}}$, ${{\boldsymbol{z}}_k} = {[{s_k},{\theta _k}]^{\text{T}}}$。$p_k^x$, $v_k^x$分别表示$x$方向的位置和速度，$p_k^y$, $v_k^y$分别表示$y$方向的位置和速度，${s_k}$, ${\theta _k}$分别表示传感器测量目标的距离和方位角。${{\boldsymbol{w}}_{k - 1}}$和${{\boldsymbol{v}}_k}$表示过程噪声和量测噪声，均满足零均值高斯分布，噪声协方差矩阵分别为${{\boldsymbol{Q}}_{k - 1}}$和${{\boldsymbol{R}}_k}$。${{\boldsymbol{b}}_k}$表示传感器测量偏差，假设测量偏差在相邻时刻具有保持不变或突变的性质，则其状态方程可表示为

其中，${{\boldsymbol{b}}_k} = {[\Delta {s_k},\Delta {\theta _k}]^{\text{T}}}$。${\boldsymbol{F}}_k^{\mathrm{b}}$表示传感器测量偏差的状态转移矩阵，当${\boldsymbol{F}}_k^{\mathrm{b}} = {{\boldsymbol{I}}_2}$时，表示测量偏差保持不变，否则表示偏差发生突变，偏差突变包括偏差突然出现、突然消失、突然增大、突然减小，突变是随机发生的，且当偏差发生突变时，${\boldsymbol{F}}_k^{\mathrm{b}}$未知。

3.   算法描述

3.1   差分量测方程构建

由于传感器测量偏差在相邻时刻具有保持不变或突变的性质，通过前后时刻的量测差分可有效消除恒定的测量偏差，从而构建差分量测方程

其中

将相邻时刻的目标状态进行扩维处理以满足实时滤波需求，即令${{\boldsymbol{\eta}} _k} = {[{{\boldsymbol{x}}_k},{{\boldsymbol{x}}_{k - 1}}]^{\text{T}}}$，则

当相邻时刻的偏差保持不变时，${\boldsymbol{b}}_k^ * = {{\textit{0}}}$；当偏差发生突变时，相较于随机误差，${\boldsymbol{b}}_k^ *$通常是一个较大的数值，因此可以看作是一个离群值，从而可以通过检测离群值来判断偏差是否发生突变。${\boldsymbol{v}}_k^ *$的协方差矩阵可用${\boldsymbol{R}}_k^ *$表示。

3.2   变分贝叶斯建模与求解

(1)建模：为了检测离群值，设置二值指示变量${r_k}$，当${r_k} = 1$时，表示未出现离群值，即测量偏差在相邻时刻保持不变，此时${\boldsymbol{b}}_k^ * = {{\textit{0}}}$，由式(6)可知，似然函数$p(\Delta {{\boldsymbol{z}}_k}|{{\boldsymbol{\eta}} _k}) = N(\Delta {{\boldsymbol{z}}_k};{h^ * }({{\boldsymbol{\eta}} _k}),{\boldsymbol{R}}_k^ * )$；当${r_k} = 0$时，表示出现离群值，即测量偏差发生突变，此时${\boldsymbol{b}}_k^ *$可以看作是一个离群值，使得似然函数$p(\Delta {{\boldsymbol{z}}_k}|{{\boldsymbol{\eta}} _k}) \ne N(\Delta {{\boldsymbol{z}}_k};{h^ * }({{\boldsymbol{\eta}} _k}),{\boldsymbol{R}}_k^ * )$。基于上述分析，可将似然函数建模为^[16]

其中，指示变量${r_k}$服从伯努利分布。同时将伯努利分布的参数变量${\tau _k}$建模为贝塔分布${{B}}({\alpha _0},{\beta _0})$，从而可构建如式(8)的Beta-Bernoulli分层先验模型

其中，$B({\alpha _0},{\beta _0})$是参数为${\alpha _0}$和${\beta _0}$的贝塔函数。同时，由于${\boldsymbol{R}}_k^*$未知，为构成共轭先验，可建模为逆Wishart分布，即$p({\boldsymbol{R}}_k^ * |\Delta {{\boldsymbol{z}}_{1:k - 1}}) = {\text{IW}}({\boldsymbol{R}}_k^ * ;{u_{k|k - 1}}, {{\boldsymbol{U}}_{k|k - 1}})$。

假设k时刻先验密度$p({{\boldsymbol{\eta}} _k}|\Delta {{\boldsymbol{z}}_{1:k - 1}})$服从高斯分布，即$p({{\boldsymbol{\eta}} _k}|\Delta {{\boldsymbol{z}}_{1:k - 1}}) = N({{\boldsymbol{\eta}} _k};{\tilde {\boldsymbol{\eta}} _{k|k - 1}},{\boldsymbol{P}}_{k|k - 1}^{{\boldsymbol{\eta}} })$，则由可构建高斯分层模型为

其中，自由度参数${u_{k|k - 1}} = \rho {u_{k - 1|k - 1}}$，逆尺度矩阵${{\boldsymbol{U}}_{k|k - 1}} = \rho {{\boldsymbol{U}}_{k - 1|k - 1}}$，遗忘因子$\rho \in (0,1]$。

联合后验可表示为

其中

(2)求解：联合后验无法直接求解，因此使用变分贝叶斯方法近似求解。根据平均场理论

其中，$q({{\boldsymbol{\eta}} _k})$, $q({r_k})$, $q({\tau _k})$, $q({\boldsymbol{R}}_k^ * )$分别表示${{\boldsymbol{\eta}} _k}$, ${r_k}$, ${\tau _k}$, ${\boldsymbol{R}}_k^ *$的近似后验分布，其求解方法为^[14]

其中，${{\text{E}}_r}[ \cdot ]$表示对变量关于近似后验分布$q({r_k})$求期望。

将式(11)代入式(13)可分别求解$q({{\boldsymbol{\eta }}_k})$, $q({r_k})$, $q({\tau _k})$, $q({\boldsymbol{R}}_k^ * )$。

(a)求解$q({{\boldsymbol{\eta}} _k})$

令${{{\stackrel \frown{{\boldsymbol{R}}} }}_k} = {({{\text{E}}_r}[{r_k}]{{\text{E}}_{\mathrm{R}}}[{({\boldsymbol{R}}_k^*)^{ - 1}}])^{ - 1}}$，则

由式(15)可知，${{\boldsymbol{\eta}} _k}$的近似后验密度满足高斯分布，即$q({{\boldsymbol{\eta}} _k}) = N({{\boldsymbol{\eta}} _k};{\tilde{\boldsymbol{ \eta}} _{k|k}},{\boldsymbol{P}}_{k|k}^{{\boldsymbol{\eta}} })$。

(b)求解$q({r_k})$

其中，${{\boldsymbol{D}}_k} = {{\text{E}}_{{\boldsymbol{\eta}} }}[(\Delta {{\boldsymbol{z}}_k} - {h^ * }({{\boldsymbol{\eta}} _k})){(\Delta {{\boldsymbol{z}}_k} - {h^ * }({{\boldsymbol{\eta}} _k}))^{\text{T}}}]$，由式(16)知，${r_k}$满足伯努利分布

其中，C表示伯努利分布的归一化常数。${{\text{E}}_\tau }[\ln {\tau _k}]$和${{\text{E}}_\tau }[\ln (1 - {\tau _k})]$可根据文献[18]计算，${{\text{E}}_{\boldsymbol{R}}}[\ln {\boldsymbol{R}}_k^ * ]$ 可根据文献[19]计算。伯努利变量${r_k}$的期望计算为

(c)求解$q({\tau _k})$

由式(19)可知，${\tau _k}$服从贝塔分布，即

(d)求解$q({\boldsymbol{R}}_k^ * )$

其中，m表示矩阵${\boldsymbol{R}}_k^ *$的维度。则由式(21)可知，${\boldsymbol{R}}_k^ *$服从逆Wishart分布，即

3.3   目标状态的边缘化容积卡尔曼滤波估计

(1)预测步。假设k-1时刻，目标状态${{\boldsymbol{x}}_{k - 1}} \sim N({{\boldsymbol{\tilde x}}_{k - 1|k - 1}}, {{\boldsymbol{P}}_{k - 1|k - 1}})$，则预测均值${{\boldsymbol{\tilde x}}_{k|k - 1}}$、预测协方差${{\boldsymbol{P}}_{k|k - 1}}$、预测互协方差矩阵${{\boldsymbol{C}}_{k|k - 1}}$计算为

扩展状态${{\boldsymbol{\eta}} _k}$的一步预测均值${\tilde {\boldsymbol{\eta}} _{k|k - 1}}$及相应的协方差矩阵${\boldsymbol{P}}_{k|k - 1}^{\boldsymbol{\eta}}$可表示为

(2)边缘化处理。扩展状态${{\boldsymbol{\eta}} _k} = [p_k^x,v_k^x,p_k^y,v_k^y, p_{k - 1}^x, v_{k - 1}^x,p_{k - 1}^y, v_{k - 1}^y]^{\text{T}}$是一个8维向量，为了减少容积卡尔曼滤波器(Cubature Kalman Filter, CKF)的计算量，利用边缘化思想^[17]，将${{\boldsymbol{\eta}} _k}$分解为线性和非线性两部分，非线性部分为${\boldsymbol{\eta}} _k^n = [p_k^x,p_k^y, p_{k - 1}^x, p_{k - 1}^y]^{\text{T}}$，线性部分为${\boldsymbol{\eta}} _k^{\mathrm{l}} = [v_k^x, v_k^y,v_{k - 1}^x,v_{k - 1}^y]^{\text{T}}$。边缘化处理之后，只需对非线性状态进行容积点采样，相比于一般的CKF，采样点数减少了1/2，可有效减小计算量。扩展状态的一步预测均值${\tilde {\boldsymbol{\eta}} _{k|k - 1}}$和协方差${\boldsymbol{P}}_{k|k - 1}^{\boldsymbol{\eta}}$可重新表示为

其中，$\tilde{\boldsymbol{ \eta}} _{k|k - 1}^{\mathrm{n}}$为非线性状态的预测均值，$\tilde {\boldsymbol{\eta}} _{k|k - 1}^{\mathrm{l}}$为线性状态的预测均值，${\boldsymbol{P}}_{k|k - 1}^{{\boldsymbol{\eta}} {\mathrm{n}}}$为非线性状态的预测协方差，${\boldsymbol{P}}_{k|k - 1}^{{\boldsymbol{\eta}} {\mathrm{l}}}$为线性状态的预测协方差，${\boldsymbol{P}}_{k|k - 1}^{{\boldsymbol{\eta}} {\mathrm{nl}}}$为非线性状态与线性状态的预测互协方差。

量测方程可重新表示为

根据条件高斯分布的性质，线性状态${\boldsymbol{\eta}} _k^{\mathrm{l}}$一步预测的条件均值$\tilde {\boldsymbol{\eta}} _{k|k - 1}^{{\mathrm{l|n}}}$和条件协方差${\boldsymbol{P}}_{k|k - 1}^{{\boldsymbol{\eta}} {\mathrm{l|n}}}$计算为

(3) 更新步。根据非线性状态${\boldsymbol{\eta}} _k^{\mathrm{n}}$的预测均值$\tilde {\boldsymbol{\eta}} _{k|k - 1}^{\mathrm{n}}$和预测协方差${\boldsymbol{P}}_{k|k - 1}^{\eta {\mathrm{n}}}$计算容积采样点

其中，d表示非线性状态的维度，${{\boldsymbol{e}}_i}$表示d维空间点集${\boldsymbol{e}}$的第i列元素，${\boldsymbol{e}} \subset {\mathbb{R}^d}$^[10]。

预测量测均值$\Delta {{\boldsymbol{\tilde z}}_{k|k - 1}}$和协方差矩阵${\boldsymbol{P}}_{k|k - 1}^{\Delta z}$计算为

扩展状态${{\boldsymbol{\eta}} _k}$与差分量测$\Delta {{\boldsymbol{z}}_k}$的互协方差${\boldsymbol{P}}_{k|k - 1}^{{{\boldsymbol{\eta}} \Delta {\boldsymbol{z}}}}$计算为

当前时刻目标状态${{\boldsymbol{x}}_k}$与$\Delta {{\boldsymbol{z}}_k}$的互协方差矩阵$P_{k|k - 1}^{{\boldsymbol{x}}\Delta {\boldsymbol{z}}}$计算为

其中，${{\boldsymbol{\xi}} ^{(i)}}(1)$表示向量${{\boldsymbol{\xi}} ^{(i)}}$的第1个元素。

上一时刻目标状态${{\boldsymbol{x}}_{k - 1}}$与$\Delta {{\boldsymbol{z}}_k}$互协方差矩阵${\boldsymbol{P}}_{k - 1,k|k - 1}^{{\boldsymbol{x}}\Delta {\boldsymbol{z}}}$计算为

新息协方差矩阵${{\boldsymbol{S}}_k}$、卡尔曼滤波增益${\boldsymbol{K}}_k^{\mathrm{f}}$计算、一步平滑增益${\boldsymbol{K}}_k^{\mathrm{s}}$计算为

其中，根据式(14)可知，${{{\stackrel \frown{{\boldsymbol{R}}} }}_k} = {({{\text{E}}_r}[{r_k}]{{\text{E}}_{\boldsymbol{R}}}[{({\boldsymbol{R}}_k^*)^{ - 1}}])^{ - 1}}$。

由3.1节分析可知，当传感器测量偏差发生突变时，量测将被严重污染，无法为状态更新提供有用信息，此时可将预测值作为滤波输出值。根据式(18)计算的指示变量${r_k}$的期望${{\text{E}}_r}[{r_k}]$来判断量测是否为离群值，进而判断传感器测量偏差是否发生突变，判断条件为

其中，$\varepsilon$表示人为设定的阈值。

当${{\text{E}}_r}[{r_k}] > \varepsilon$时，即测量偏差未发生突变时，量测将被用来更新目标状态，${{\boldsymbol{x}}_k}$的滤波估计值${{\boldsymbol{\tilde x}}_{k|k}}$及相应的协方差矩阵${{\boldsymbol{P}}_{k|k}}$，${{\boldsymbol{x}}_{k - 1}}$的平滑估计值${{\boldsymbol{\tilde x}}_{k - 1|k}}$及相应的协方差矩阵${{\boldsymbol{P}}_{k - 1|k}}$，${{\boldsymbol{x}}_{k - 1}}$与${{\boldsymbol{x}}_k}$的互协方差矩阵${{\boldsymbol{P}}_{k - 1,k|k}}$计算为

当${{\text{E}}_r}[{r_k}] \le\varepsilon$时，即测量偏差发生突变时，量测无法提供有用信息，不再使用量测对目标状态进行更新，相关量计算为

扩展状态${{\boldsymbol{\eta}} _k}$的滤波估计值${\tilde {\boldsymbol{\eta}} _{k|k}}$与相应的误差协方差矩阵${\boldsymbol{P}}_{k|k}^{\boldsymbol{\eta}}$计算为

3.4   算法伪代码

根据3.1～3.3节中内容，在算法1中给出了本文方法的具体实现步骤。

表 1  偏差未补偿自适应边缘化容积卡尔曼滤波跟踪算法

输入：状态估计值${{\boldsymbol{\tilde x}}_{0|0}}$，误差协方差${{\boldsymbol{P}}_{0|0}}$，逆Wishart分布参数 　${u_{0|0}}$, ${{\boldsymbol{U}}_{0|0}}$，贝塔分布参数${\alpha _0}$, ${\beta _0}$，初始时刻伯努利变量的期望 　值${\text{E}}[{r_0}]$，遗忘因子$\rho$，迭代次数N。
输出：${{\boldsymbol{\tilde x}}_{k|k}}$,${{\boldsymbol{P}}_{k|k}}$, ${u_{k|k}}$, ${{\boldsymbol{U}}_{k|k}}$。
(1) for k = 1:K
(2) 　通过式(23)计算${{\boldsymbol{\tilde x}}_{k|k - 1}}$, ${{\boldsymbol{P}}_{k|k - 1}}$, ${{\boldsymbol{C}}_{k|k - 1}}$；
(3) 　计算：${u_{k|k - 1}} = \rho {u_{k - 1|k - 1}}$, ${{\boldsymbol{U}}_{k|k - 1}} = \rho {{\boldsymbol{U}}_{k - 1|k - 1}}$；
(4) 　初始化：${\boldsymbol{\tilde x}}_{k|k}^{{\text{(0)}}} = {{\boldsymbol{\tilde x}}_{k|k - 1}}$, ${\boldsymbol{P}}_{k|k}^{{\text{(0)}}} = {{\boldsymbol{P}}_{k|k - 1}}$, 　　　　$u_{k|k}^{(0)} = {u_{k|k - 1}}$, ${\boldsymbol{U}}_{k|k}^{(0)} = {{\boldsymbol{U}}_{k|k - 1}}$；
(5) 　for i = 0:N
(6) 　　通过式(14)计算${{\stackrel \frown{{\boldsymbol{R}}} }}_k^{(i + 1)}$；
(7) 　　通过式(35)计算${\boldsymbol{\tilde x}}_{k|k}^{{\text{(}}i{\text{ + 1)}}}$, ${\boldsymbol{P}}_{k|k}^{{\text{(}}i{\text{ + 1)}}}$；
(8) 　　通过式(18)计算${({\text{E}}[{r_k}])^{(i + 1)}}$；
(9) 　　根据式(34)判断传感器测量偏差是否突变
(10) 　　 若传感器测量偏差突变：
(11) 　　 ${{\boldsymbol{\tilde x}}_{k|k}} = {\boldsymbol{\tilde x}}_{k|k}^{{\text{(}}0{\text{)}}}$, ${{\boldsymbol{P}}_{k|k}} = {\boldsymbol{P}}_{k|k}^{(0)}$；
(12) 　　 ${u_{k|k}} = u_{k|k}^{(0)}$, ${{\boldsymbol{U}}_{k|k}} = {\boldsymbol{U}}_{k|k}^{(0)}$；
(13) 　　 跳出循环；
(14) 　 若传感器测量偏差不突变：
(15) 　　 ${{\boldsymbol{\tilde x}}_{k|k}} = {\boldsymbol{\tilde x}}_{k|k}^{{\text{(}}i{\text{ + 1)}}}$,${{\boldsymbol{P}}_{k|k}} = {\boldsymbol{P}}_{k|k}^{(i + 1)}$；
(16) 　　 通过式(20)计算$\alpha _k^{(i + 1)}$和$\beta _k^{(i + 1)}$；
(17) 　　 通过式(22)计算$u_{k|k}^{(i + 1)}$和${\boldsymbol{U}}_{k|k}^{(i + 1)}$；
(18) 　　 ${u_{k|k}} = u_{k|k}^{{\text{(}}i{\text{ + 1)}}}$, ${{\boldsymbol{U}}_{k|k}} = {\boldsymbol{U}}_{k|k}^{(i + 1)}$；
(19)　　 计算迭代停止阈值$\kappa$： 　　　　　　$\kappa = ||{\boldsymbol{\tilde x}}_{k|k}^{{\text{(}}i + 1{\text{)}}} - {\boldsymbol{\tilde x}}_{k|k}^{{\text{(}}i{\text{)}}}||/||{\boldsymbol{\tilde x}}_{k|k}^{{\text{(}}i{\text{)}}}||$；
(20) 　　 当$\kappa \le {10^{ - 6}}$时：
(21) 　　　 跳出循环。
(22)　　 end for
(23) end for

下载: 导出CSV 

| 显示表格

4.   仿真实验

4.1   场景设置

目标在2维平面上运动，运动方程为

其中，${{\boldsymbol{x}}_k} = {[p_k^x,v_k^x,p_k^y,v_k^y]^{\text{T}}}$，${{\boldsymbol{w}}_{k - 1}}$是协方差矩阵为${{\boldsymbol{Q}}_{k - 1}}$的高斯白噪声，状态转移模型$\psi$为协同转弯模型^[20]。

量测方程为

其中，${{\boldsymbol{z}}_k} = {[{s_k},{\theta _k}]^{\text{T}}}$, ${{\boldsymbol{b}}_k} = {[\Delta {s_k},\Delta {\theta _k}]^{\text{T}}}$, ${{\boldsymbol{v}}_k}$是高斯白噪声，协方差矩阵为${{\boldsymbol{R}}_k}$，假设传感器位于坐标原点，则非线性量测函数$h({{\boldsymbol{x}}_k}) = [{\text{sqrt}}({(p_k^x)^2} + {(p_k^y)^2}) \arctan ({{p_k^x} \mathord{\left/ {\vphantom {{p_k^x} {p_k^y}}} \right. } {p_k^y}})]^{\text{T}}{\text{ }}$。

仿真实验中，目标初始状态为${{\boldsymbol{x}}_0} = [2\;000, 5,1\;000,10]^{\text{T}}$，初始协方差矩阵为${{\boldsymbol{P}}_0} = {\text{diag}}([50, 0.5,50,0.5])$；过程噪声协方差矩阵为${{\boldsymbol{Q}}_{k - 1}} = {\text{diag}}([10,0.1,10,0.1])$；转弯运动的转弯率为$- 0.032{\text{ rad/s}}$；仿真总时长为100 s，采样周期$T = 1{\text{ s}}$。量测噪声矩阵${{\boldsymbol{R}}_k} = {v_k}{{\boldsymbol{R}}_{k - 1}}$，在1～10 s时，${v_k} = 0.96$；在11～50 s时，${v_k} = 1.03$；在51～80 s时，${v_k} = 0.96$；在81～100 s时，${v_k} = 1.03$。${{\boldsymbol{R}}_0} = {\text{diag}}([{5^2},{0.001^2}])$。

传感器测量偏差设置为

上述参数设置中，距离单位为m，速度单位为m/s，角度的单位为rad。

4.2   对比分析

为验证本文方法的有效性，选取传统的CKF方法、文献[7]中的方法和文献[10]中的增量CKF方法与本文方法进行对比实验，其中文献[10]是对文献[8]方法的一种扩展实现。因为CKF方法和增量CKF方法无法有效处理量测噪声时变未知的情况，所以在该对比方法进行实验时，假设量测噪声精确已知，而在用本文方法实验时，量测噪声是未知的。为了保证对比的公平性，文献[7]中的方法同样用CKF实现，并记作NRCKF。本文方法的具体实现步骤见算法1。选取目标位置估计的均方根误差(Root Mean Square Error, RMSE)作为性能评价标准，为了使实验结果更具一般性，进行500次蒙特卡洛实验，则k时刻位置的RMSE计算为

其中，第i次蒙特卡洛实验中，目标在k时刻的位置为$(p_k^{x,i},{\text{ }}p_k^{y,i})$，同理，可以计算速度的RMSE。

设置式(34)中的阈值$\varepsilon = 1{\text{e}} - 15$，目标跟踪效果如图1所示，其中绿色线是目标真实运动轨迹，红色线是所提方法的跟踪结果，其余为对比方法的跟踪结果，可以直观地看出，所提方法的跟踪性能是最优的。图2展示了指示变量期望值${{\text{E}}_r}[{r_k}]$的变化曲线，可以看出，在偏差发生突变的时刻，即第11, 31和91 s，${{\text{E}}_r}[{r_k}]$取值接近于0，且偏差突变程度越大，${{\text{E}}_r}[{r_k}]$的值越小，而在偏差未发生突变的时刻，${{\text{E}}_r}[{r_k}]$取值接近于1。偏差发生突变时的${{\text{E}}_r}[{r_k}]$值与不发生突变时的${{\text{E}}_r}[{r_k}]$值具有较大差异，进一步说明可以根据${{\text{E}}_r}[{r_k}]$的值对偏差是否发生突变进行有效判别。

图 1  跟踪效果

下载: 全尺寸图片 幻灯片

图 2  E[r]值变化图

下载: 全尺寸图片 幻灯片

图3、图4分别为目标位置、速度的RMSE对比图，由图3可知，在0～10 s，由于测量偏差保持恒定不变，增量CKF与所提方法的估计精度基本保持一致，在第11 s，测量偏差发生突变，增量CKF的估计精度急剧下降，且在之后90 s，该方法均无法对目标进行有效跟踪，而所提方法能有效应对偏差突变问题，保持较高的估计精度。对于NRCKF，在偏差恒定的前10 s内，能到达较高的估计精度，当偏差突变时，由于无法及时估计出突变偏差值，导致之后的滤波估计精度大幅下降。在93～100 s，传统CKF的估计精度要高于所提方法的估计精度，这是由于所提方法通过差分方法构建量测方程，导致量测噪声协方差增大，会在一定程度上影响估计精度，且在用传统CKF进行实验时，假设量测噪声精确已知，而在用本文方法实验时，量测噪声是未知的。综合比较而言，所提方法的跟踪性能要优于对比方法。从图4中可以看出，所提方法的速度估计性能也是最优的。

图 3  不同方法的位置RMSE

下载: 全尺寸图片 幻灯片

图 4  不同方法的速度RMSE

下载: 全尺寸图片 幻灯片

为测试变分迭代次数对算法性能的影响，统计了不同迭代次数下的平均均方根误差(Average Root Mean Square Error, ARMSE)，500次蒙特卡洛实验下，位置的ARMSE计算为

同理，可计算速度的ARMSE。

图5、图6分别为不同方法的目标位置、速度ARMSE对比图，由此可知，所提方法在迭代次数N>2时收敛，此后，随着迭代次数的增加，估计性能保持稳定，位置和速度的ARMSE均保持最优。

图 5  不同方法的位置ARMSE

下载: 全尺寸图片 幻灯片

图 6  不同方法的速度ARMSE

下载: 全尺寸图片 幻灯片

为测试阈值$\varepsilon$对算法性能的影响，统计了不同$\varepsilon$值下所提方法的ARMSE，结果如图7、图8所示，可以看出，$\varepsilon$取值从1e–2到1e–20，所提方法的估计性能保持稳定。

图 7  不同$\varepsilon$值的位置ARMSE

下载: 全尺寸图片 幻灯片

图 8  不同$\varepsilon$值的速度ARMSE

下载: 全尺寸图片 幻灯片

为验证本文方法中边缘化处理的时间有效性，表1中统计了不同方法的500次蒙特卡洛运行总时间，其中迭代次数N设为3。由表可知，传统的CKF方法运行时间最短；增量CKF方法运行时间略高于传统CKF方法；NRCKF方法在滤波过程中需要进行变分迭代更新，因此需要较长时间；提出的方法(非边缘化)同样需要经过变分迭代的过程，运行时间要略高于NRCKF，但是经过边缘化处理后，运行时间将减少43%，这是因为在变分贝叶斯迭代过程中需要多次用到容积采样和积分，目标状态经过边缘化处理后，容积采样的维度降低至原来的1/2，每次变分迭代的采样点数随之减少1倍，采样点在非线性传递过程中的计算量会相应减少，且计算减少量随着变分迭代次数的增加而增加。

表 1  运行时间对比(s)

方法	时间
传统CKF	1.038 1
增量CKF	1.210 0
NRCKF	11.343 8
提出的方法(非边缘化)	12.866 4
提出的方法(边缘化)	7.333 3

下载: 导出CSV 

| 显示表格

为测试边缘化处理对跟踪精度的影响，单独比较了所提方法在边缘化处理前、后的RMSE和ARMSE。图9和图10分别为边缘化处理前、后的RMSE差值图像和ARMSE差值图像。其中位置的RMSE差值的最大值为0.071 m，速度的RMSE差值的最大值为0.011 m/s，相对所提方法的真实RMSE, RMSE差值可以忽略不计；位置的ARMSE差值的最大值为0.103 m，速度的ARMSE差值的最大值为0.001 m/s，均远小于所提方法的真实ARMSE。因此，边缘化处理能够在不影响目标跟踪精度的前提下，有效减少所提方法的计算量。同理，边缘化处理方法也可应用到其它非线性变分贝叶斯滤波跟踪问题以减小计算负担。

图 9  边缘化处理前后的RMSE差值

下载: 全尺寸图片 幻灯片

图 10  边缘化处理前后的ARMSE差值

下载: 全尺寸图片 幻灯片

为进一步验证所提方法的有效性，对测量偏差${{\boldsymbol{b}}_k}$和量测噪声协方差矩阵${{\boldsymbol{R}}_k}$单独变化时该方法的跟踪性能进行测试。当${{\boldsymbol{b}}_k}$单独变化时，其取值遵循式(40)，设置${{\boldsymbol{R}}_k} = {\text{diag}}([{5^2},{0.001^2}])$，ARMSE对比结果如表2所示，可以看出，所提方法对目标速度和位置的估计性能均是最优的；当${{\boldsymbol{R}}_k}$单独变化时，其取值变化与4.1节中相同，同时设置${{\boldsymbol{b}}_k} = {[50,0.001]^{\text{T}}}$，ARMSE对比结果如表3所示，可以看出，所提方法的位置估计精度是最优的，速度估计精度要略低于传统的CKF方法，但在用传统CKF进行实验时，假设${{\boldsymbol{R}}_k}$精确已知，而在用所提方法进行实验时，${{\boldsymbol{R}}_k}$是未知的，综合比较而言，所提方法跟踪性能依然是最优的。

表 2  仅测量偏差${{\boldsymbol{b}}_k}$变化时各方法ARMSE对比

	传统CKF	增量CKF	NRCKF	提出的边缘化CKF
${\text{ARMS}}{{\text{E}}_{{\text{pos}}}}$(m)	155.359 1	78.411 8	47.686 8	15.359 0
${\text{ARMS}}{{\text{E}}_{{\text{vel}}}}$(m/s)	6.533 4	3.533 9	2.062 2	1.549 0

下载: 导出CSV 

| 显示表格

表 3  仅量测协方差矩阵${{\boldsymbol{R}}_k}$变化时各方法ARMSE对比

	传统CKF	增量CKF	NRCKF	提出的边缘化CKF
${\text{ARMS}}{{\text{E}}_{{\text{pos}}}}$(m)	49.822 0	29.690 6	22.204 5	9.597 0
${\text{ARMS}}{{\text{E}}_{{\text{vel}}}}$(m/s)	1.481 2	1.655 1	1.696 7	1.518 7

下载: 导出CSV 

| 显示表格

5.   结论

针对存在突变测量偏差和未知时变量测噪声场景下的目标跟踪问题，本文提出了一种偏差未补偿自适应边缘化容积卡尔曼滤波跟踪方法，通过量测差分消除恒定的测量偏差，构建指示变量识别突变测量偏差，利用逆Wishart分布建模未知量测噪声协方差矩阵，从而建立各个参数的联合后验分布，并利用变分贝叶斯求解近似后验。同时，为降低滤波负担，首先对目标状态进行边缘化处理，然后在CKF框架下实现滤波跟踪。通过仿真实验得出以下结论：(1) 相比于现有方法，所提方法在测量偏差突变和量测噪声未知场景下具有更优的目标跟踪性能；(2) 边缘化处理能够大幅减小所提方法的滤波负担，因此可应用到其它更为复杂的非线性变分贝叶斯滤波问题以降低滤波复杂度，如鲁棒随机有限集类滤波跟踪^[20,21]问题；(3) 本方法可以扩展至更高维的目标跟踪场景。