自动化学报  2017, Vol. 43 Issue (1): 114-131   PDF    
带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器
黄玉龙1, 张勇刚1, 武哲民2, 李宁1, 王刚3     
1. 哈尔滨工程大学自动化学院 哈尔滨 150001;
2. 哈尔滨工业大学电气工程及自动化学院 哈尔滨 150080;
3. 中国舰船研究设计中心 武汉 430064
摘要: 为了解决带有色厚尾量测噪声的非线性状态估计问题,本文提出了新的鲁棒高斯近似(Gaussian approximate,GA)滤波器和平滑器.首先,基于状态扩展方法将量测差分后带一步延迟状态和白色厚尾量测噪声的非线性状态估计问题,转化成带厚尾量测噪声的标准非线性状态估计问题.其次,针对量测差分后模型中的噪声尺度矩阵和自由度(Degrees of freedom,DOF)参数未知问题,设计了新的高斯近似滤波器和平滑器,通过建立未知参数和待估计状态的共轭先验分布,并利用变分贝叶斯方法同时估计未知的状态、尺度矩阵、自由度参数.最后,利用目标跟踪仿真验证了本文提出的带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器的有效性以及与现有方法相比的优越性.
关键词: 非线性状态估计     有色厚尾量测噪声     变分贝叶斯     Student's t 分布     状态扩展方法    
Robust Gaussian Approximate Filter and Smoother with Colored Heavy Tailed Measurement Noise
HUANG Yu-Long1, ZHANG Yong-Gang1, WU Zhe-Min2, LI Ning1, WANG Gang3     
1. College of Automation, Harbin Engineering University, Har-bin 150001;
2. School of Electrical Engineering and Automation, Harbin Institute of Technology, Harbin 150080;
3. China Ship Development and Design Center, Wuhan 430064
Received: 2015-12-01, Accepted: 2016-04-01.
Foundation Item: Supported by National Natural Science Foundations of China (61371173) and Fundamental Research Funds for the Central Universities of Harbin Engineering University (HEUCFQ20150407)
Author brief: HUANG Yu-Long Ph. D. candi-date at the College of Automation,Harbin Engineering University. His re-search interest covers inertial naviga-tion, filtering algorithm, and integrated navigation.E-mail:heuedu@163.com;
WU Zhe-Min Ph. D. candidate at the School of Electrical Engineering and Automation, Harbin Institute of Tech-nology. Her research interest covers fil-tering algorithm and integrated navigation.E-mail:myemailabc@163.com;
LI Ning Associate professor at the College of Automation, Harbin Engi-neering University. Her research inter-est covers adaptive filtering and inte-grated navigation.E-mail:ningli@hrbeu.edu.cn;
(WANG Gang Master student at the College of Automation, Harbin En-gineering University. His research inter-est covers inertial navigation, filtering algorithm, and integrated navigation.E-mail:wanggang2016@126.com
Corresponding author. ZHANG Yong-Gang Professor at the College of Automation, Harbin En-gineering University. He received his Ph. D. degree from Cardi® University, UK in 2007. His re-search interest covers fiber-optic gyroscope, inertial naviga-tion, filtering algorithms, and integrated navigation. Cor-responding author of this paper.E-mail:zhangyg@hrbeu.edu.cn
Abstract: In this paper, new robust Gaussian approximate (GA) filter and smoother are proposed to solve the problem of nonlinear state estimation with colored heavy tailed measurement noise. Firstly, the nonlinear state estimation problem with one-step delayed state and white heavy tailed measurement noise after measurement differencing is transformed into a standard nonlinear state estimation problem with heavy tailed measurement noise based on the state augmentation approach. Secondly, new GA filter and smoother are designed for the problem of unknown scale matrix and degrees of freedom (DOF) parameter of noise of the model after measurement differencing. The state, scale matrix and DOF parameter are estimated simultaneously by building the conjugate prior distributions for unknown parameters and estimated state and using variational Bayesian approach. Finally, the efficiency and superiority of the proposed robust GA filter and smoother with colored heavy tailed measurement noise, as compared with existing method, are shown in the simulation of target tracking.
Key words: Nonlinear state estimation     colored heavy tailed measurement noise     variational Bayesian     Student's t distribution     state augmentation approach    

非线性状态估计已被广泛应用于目标跟踪、定位、信号处理和自动控制中.基于随机的非线性状态空间模型,非线性状态估计的核心任务是计算状态的后验概率密度函数 (Probability density function,PDF).通常可以利用贝叶斯估计理论来处理非线性状态估计问题,通过递归地计算状态的后验PDF,贝叶斯估计理论为动态状态估计问题提供了一个最优解[1-3].但是,对于非线性随机动态系统而言,因为贝叶斯估计中包含的多维非线性积分一般无法解析求解,从而无法获得状态的后验PDF的解析解,所以通常情况下不存在最优的非线性滤波器和平滑器[1-3].为了完成非线性随机动态系统的状态估计任务,必须使用近似的方法获得次优的非线性滤波器和平滑器. 高斯近似(Gaussian approximate,GA)是一种被广泛使用的有效方法,它假设噪声为高斯,且系统状态经过非线性传播后仍近似为高斯,在实际应用中利用高斯近似推导出的高斯近似滤波器和平滑器具有良好的精度和计算效率[1].基于不同的数值近似方法,目前已经提出了许多次优的非线性高斯近似滤波器和平滑器[4-17].

在一些具有高采样频率和不可靠传感器的应用中,它们的非线性量测模型可能具有有色厚尾的量测噪声,因为在高采样频率下相邻时刻的样本之间可能具有相关性从而诱导出有色的量测噪声[18-20],而不可靠传感器可能会产生量测野值从而诱导出厚尾的量测噪声[21-22].厚尾非高斯噪声与高斯噪声相比,由于野值干扰,导致噪声取值在离均值较远的区域可能性增大,形成厚尾形状.图 1显示了零均值方差为1的高斯噪声与厚尾非高斯噪声的概率分布图,其中厚尾非高斯分布是在高斯噪声的基础上,以$0.2$的概率加入了方差为3的高斯噪声,从而模拟野值干扰. 从图 1可以看出,此时高斯分布已经无法准确模拟野值干扰下的噪声分布,从而导致基于高斯噪声假设设计的标准高斯近似状态估计方法[4-17]和带有色量测噪声的高斯近似状态估计方法[18-20, 23-24]精度下降,甚至发散. 研究表明,Student's t分布可以更好地对厚尾非高斯噪声进行准确建模[21]. 与高斯概率密度相比,Student's t分布能够更好地匹配厚尾非高斯分布. 在图 1中,Student's t分布的参数为均值为0、比例矩阵系数为2、自由度为5.

图 1 高斯分布、Student's t 分布、野值干扰导致的厚尾非高斯分布示意图 Figure 1 Diagrams of Gaussian distribution,Student's t distribution and heavy tailed non-Gaussian distribution induced by outlier interference

通过将厚尾量测噪声建模成平稳的 Student's t 分布,目前已经提出了许多状态估计方法.为了解决带厚尾量测噪声的线性滤波问题,Agamennoni等[25-26]提出了一种对野值量测具有鲁棒性的改进 Kalman滤波器. 为了解决带厚尾量测噪声的多传感器状态估计问题,Zhu等[27]利用 Student's t分布建模多个传感器的量测噪声从而提出了一种线性鲁棒传感器融合方法.为了解决带偏斜厚尾量测噪声的线性滤波和平滑问题,Nurminen等[28]通过将量测噪声建模成偏斜的 Student's t分布继而推导出了鲁棒偏斜滤波器和平滑器.为了解决带厚尾系统和量测噪声的线性滤波问题,Roth等[29]通过将状态的后验 PDF 近似为 Student's t分布继而推导出了一种鲁棒 Student's t 滤波器.但是以上这些线性鲁棒滤波器和平滑器不能用于解决带厚尾量测噪声的非线性滤波和平滑问题[21].为了解决带厚尾量测噪声的非线性滤波问题,Xu等[30]基于变分贝叶斯方法和边沿化方法提出了一种鲁棒粒子滤波算法.但是,这种鲁棒粒子滤波方法需要对每个粒子进行变分贝叶斯更新,因而遭受巨大的计算负担和维数灾难. 为了解决这个问题,Piché等[21]在高斯近似框架下基于变分贝叶斯方法推导出了对野值量测具有鲁棒性的高斯近似滤波器和平滑器.此外,为了解决带状态相关噪声和厚尾量测噪声的非线性状态估计问题,Agamennoni等[22]通过利用高斯牛顿算法求解二次复合目标函数的最优解继而提出了一种鲁棒状态估计方法.

但是,现有的鲁棒高斯近似滤波器和平滑器[21-22]在处理带有色厚尾量测噪声的非线性状态估计问题时性能会下降,因为它们是专门针对带白色厚尾量测噪声的非线性系统而设计的. 此外,我们也不能通过简单地结合现有的带有色量测噪声的状态估计方法和带厚尾量测噪声的状态估计方法来解决带有色厚尾量测噪声的非线性状态估计问题,因为虽然利用现有的量测差分方法可以将带有色厚尾量测噪声的非线性状态估计模型变换成带一步延迟状态和厚尾量测噪声的非线性状态估计模型,但是新的模型将带来两个新的问题: 1) 基于量测差分方法变换后的量测噪声的 Student's t PDF的尺度矩阵和自由度参数都是未知的,因为它们不具有真实的物理意义,无法直接通过传感器的精度获得,而现有的带厚尾量测噪声的高斯近似滤波器和平滑器都需要已知尺度矩阵和自由度参数[21];2) 现有的带厚尾量测噪声的高斯近似滤波器和平滑器都是针对带厚尾量测噪声的标准非线性系统(其当前时刻的量测只依赖于当前时刻的状态)而设计的,因而它们不能用于处理带一步延迟状态和厚尾量测噪声非线性系统(其当前时刻的量测不仅依赖于当前时刻的状态而且还依赖于前一时刻的状态)的状态估计问题.注意文献[24]中提出的带有色量测噪声的非线性系统辨识方法是针对有色高斯量测噪声而设计的,不能用于估计 Student's t PDF 的未知尺度矩阵和自由度参数.综上所述,讨论有色厚尾量测噪声情况下的非线性状态估计问题极具理论价值和现实意义.

为了解决带有色厚尾量测噪声的非线性状态估计问题,本文提出了新的鲁棒高斯近似滤波器和平滑器. 1) 利用状态扩展方法将量测差分后带一步延迟状态和白色厚尾量测噪声的非线性状态估计问题转化成带厚尾量测噪声的标准非线性状态估计问题.2) 针对量测差分后模型中的噪声尺度矩阵和自由度(Degrees of freedom,DOF)参数未知问题,通过建立未知参数和待估计状态的共轭先验分布,并利用变分贝叶斯方法将状态、尺度矩阵、自由度参数的后验PDF分别更新为高斯、逆Wishart、Gamma 分布,同时估计未知的状态、尺度矩阵、自由度参数. 3) 利用目标跟踪仿真验证了本文所提出的带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器的有效性以及与现有方法相比的优越性.

1 问题陈述

考虑如下状态空间形式的离散非线性系统[31]:

${{x}_{k}}={{f}_{k-1}}\left( {{x}_{k-1}} \right)+{{w}_{k-1}}$ (1)
${{z}_{k}}={{h}_{k}}\left( x \right)+{{v}_{k}}$ (2)

其中,k表示离散时间序列,$\pmb{x}_{0:M}:=\{\pmb{x}_k\in\mathbf{R}^{n}|0\leq k\leq M\}$表示由0时刻到M时刻所有状态向量组成的集合,${{z}_{1:M}}:=\{{{z}_{k}}\in {{\mathbf{R}}^{m}}|1\le k\le M\}$表示由1~M时刻所有量测向量组成的集合,$\pmb{f}_{k-1}(\cdot)$$\pmb{h}_{k}(\cdot)$为已知的任意函数,${{w}_{k}}\in {{\mathbf{R}}^{n}}$${{v}_{k}}\in {{\mathbf{R}}^{m}}$ 分别为系统噪声和量测噪声.${{w}_{k}}\in {{\mathbf{R}}^{n}}$ 是高斯白噪声,满足$\text{E}[{{w}_{k}}w_{l}^{\text{T}}]={{Q}_{k}}{{\delta }_{kl}}$,其中$\delta_{kl}$ 是Kronecker-$\delta$ 函数.${{v}_{k}}\in {{\mathbf{R}}^{m}}$ 是有色厚尾噪声,它的一阶自回归模型可以表示如下[24]:

${{v}_{k}}={{\Psi }_{k-1}}{{v}_{k-1}}+{{\xi }_{k-1}}$ (3)

其中,${\Psi}_{k-1}$ 是已知的相关参数.${{\xi }_{k}}\in {{\mathbf{R}}^{m}}$ 是白色厚尾噪声,它被建模成如下的平稳 Student's t 分布:

$\begin{array}{l} p({\xi _k}) = {\rm{St}}({\xi _k};0,R,\nu ) = \\ \int_0^{ + \infty } {{\rm{N}}\left( {{\xi _k};0,\frac{R}{{{\lambda _k}}}} \right) \times {\rm{G}}\left( {{\lambda _k};\frac{\nu }{2},\frac{\nu }{2}} \right){\rm{d}}{\lambda _k}} \end{array}$ (4)

其中,$\mathrm{St}(\cdot;\pmb{0},R,\nu)$ 表示均值为$\pmb{0}$、尺度矩阵为 R、自由度参数为 $\nu$ 的 Student's t PDF,$\mathrm{N}(\cdot;\pmb{\mu},\Sigma)$ 表示均值为$\pmb{\mu}$、方差为$\Sigma$ 的高斯PDF,$\mathrm{G}(\cdot;\alpha,\beta)$ 表示形状参数为$\alpha$、比率参数为$\beta$ 的 Gamma PDF,$λ_{k}$为辅助的随机变量. 初始状态$\pmb{x}_{0}$是一个均值和协方差矩阵分别为${\hat{\pmb x}}_{0|0}$${P}_{0|0}$的高斯随机向量,并且初始状态$\pmb{x}_{0}$、系统噪声$\pmb{w}_{k}$、量测噪声$\pmb{v}_{k}$相互独立. 初始联合共轭先验 PDF $p(\pmb{x}_{0},R,\nu)$可以表示如下:

$\begin{align} &p({{x}_{0}},R,\nu )=\text{N}({{x}_{0}};{{\widehat{x}}_{0|0}},{{P}_{0|0}})IW(R;{{u}_{0}},{{U}_{0}})\times \\ &\text{G}(\nu ;{{a}_{0}},{{b}_{0}}) \\ \end{align}$ (5)

其中,$ {IW}(\cdot;u_{0},U_{0})$ 表示自由度参数为$u_{0}$、逆尺度矩阵为$U_{0}$ 的逆 Wishart PDF.

为了设计带有色厚尾量测噪声的高斯近似滤波器和平滑器,需要利用量测差分方法对有色量测$\pmb{z}_k$进行如下的白色化[20]:

${{y}_{k}}={{z}_{k}}-{{\Psi }_{k-1}}{{z}_{k-1}}$ (6)

其中,$\pmb{y}_{k}$为重新构造的白色量测. 基于式(6) ,可以构造一个新的带白色厚尾量测噪声和一步延迟状态的非线性量测方程

${{y}_{k}}={{g}_{k}}({{x}_{k}},{{x}_{k-1}})+{{\xi }_{k-1}}$ (7)
${{g}_{k}}({{x}_{k}},{{x}_{k-1}})={{h}_{k}}({{x}_{k}})-{{\Psi }_{k-1}}{{h}_{k-1}}({{x}_{k-1}})$ (8)

从而式(1) ~(5) 中的带有色厚尾量测噪声的高斯近似滤波器和平滑器设计问题可以转化成式(1) 和式(4) ~(8) 中的带白色厚尾量测噪声和一步延迟状态的高斯近似滤波器和平滑器设计问题.

但是,现有的带厚尾量测噪声的高斯近似滤波器和平滑器不再适合处理模型(1) 和式(4) ~(8) 中的滤波和平滑问题,具体原因如下: 1) 在有色厚尾量测噪声情况下,当前时刻的量测$\pmb{y}_{k}$ 不仅依赖于当前时刻的状态$\pmb{x}_{k}$而且还依赖于前一时刻的状态$\pmb{x}_{k-1}$,而现有的带厚尾量测噪声的高斯近似滤波器和平滑器只适用于标准的非线性系统(其当前时刻的量测只依赖于当前时刻的状态);2) 在有色厚尾量测噪声情况下,尺度矩阵 R 和自由度参数$\nu$是未知的,因为它们不具有真实的物理意义,无法通过传感器的精度直接获得,而现有的带厚尾量测噪声的高斯近似滤波器和平滑器需要已知尺度矩阵 R和自由度参数$\nu$. 为了解决这些问题,接下来将基于模型(1) 和式(4) ~(8) 以及状态扩展方法和变分贝叶斯方法重新设计高斯近似滤波器和平滑器,同时估计状态$\pmb{x}_{k}$、尺度矩阵 R 和自由度参数$\nu$.

2 带有色厚尾量测噪声的鲁棒高斯近似滤波器

本节的目的是基于量测$\pmb{y}_{1:k}$,同时估计状态$\pmb{x}_{k}$、尺度矩阵 R和自由度参数$\nu$,其中量测$\pmb{y}_{1:k}:=\{\pmb{y}_{j}\}_{j=1}^{k}$.根据式(4) 和式(7) 以及Student's t分布的分层高斯性质,可以得到如下的似然PDF[21]:

$\begin{align} &p({{y}_{k}}|{{\zeta }_{k}},{{\lambda }_{k}},R)={{p}_{{{\xi }_{k}}}}({{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})|{{\lambda }_{k}},R)= \\ &\text{N}\left( {{y}_{k}}-{{g}_{k}}({{\zeta }_{k}});0,\frac{R}{{{\lambda }_{k}}} \right)\text{=} \\ &\text{N}\left( {{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\frac{R}{{{\lambda }_{k}}} \right) \\ \end{align}$ (9)

其中,扩展的状态向量$\pmb{\zeta}_{k}:=[\pmb{x}_{k}\;\pmb{x}_{k-1}]^{\rm T}$,$\pmb{g}_{k}(\pmb{\zeta}_{k}):=\pmb{g}_{k}(\pmb{x}_{k},\pmb{x}_{k-1})$,$p_{\pmb{\xi}_{k}}(\cdot)$ 表示$\pmb{\xi}_{k}$的PDF,辅助参数$λ_{k}$ 服从Gamma分布,并且它的PDF可以表示如下:

$p({{\lambda }_{k}}|\nu )=\text{G}\left( {{\lambda }_{k}};\frac{\nu }{2},\frac{\nu }{2} \right)$ (10)

从式(9) 可以看到,量测$\pmb{y}_{k}$ 依赖于扩展状态$\pmb{\zeta}_{k}$、辅助参数$λ_{k}$、尺度矩阵 R.因而在设计滤波器之前,需要提供如下的联合共轭先验:

$\begin{align} &p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k-1}})=\text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta } \right)\times \\ &\text{G}\left( {{\lambda }_{k}};\frac{\nu }{2},\frac{\nu }{2} \right)IW\left( R;{{{\hat{u}}}_{k-1}},{{U}_{k-1}} \right)\times \\ &\text{G}\left( \nu ;{{{\hat{a}}}_{k-1}},{{{\hat{b}}}_{k-1}} \right) \\ \end{align}$ (11)

其中,$\hat{\pmb{\zeta}}_{k|k-1}$$P_{k|k-1}^{\zeta\zeta}$分别表示$\pmb{\zeta}_{k}$ 的一步预测估计及相应的预测误差协方差矩阵,$\hat{u}_{k-1}$$U_{k-1}$ 分别表示 R 的自由度参数和逆尺度矩阵在k-1时刻的估计,$\hat{a}_{k-1}$$\hat{b}_{k-1}$ 分别表示$\nu$的形状参数和比率参数在k-1时刻的估计.

根据贝叶斯准则,联合后验滤波PDF $p({{\zeta }_{k}},{{\lambda }_{k}}$,$R,\nu|\pmb{y}_{1:k})$ 可以表示如下:

$\begin{align} &p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k}})=\frac{p({{y}_{k}},{{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k-1}})}{p({{y}_{k}}|{{y}_{1:k-1}})}\propto \\ &p({{y}_{k}}|{{\zeta }_{k}},{{\lambda }_{k}},R)p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k-1}}) \\ \end{align}$ (12)

将式(9) 和式(11) 代入式(12) 中,我们可以得到:

$\begin{align} &p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k}})\propto \text{N}\left( {{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\frac{R}{{{\lambda }_{k}}} \right)\times \\ &\text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta } \right)\text{G}\left( {{\lambda }_{k}};\frac{\nu }{2},\frac{\nu }{2} \right)\times \\ &IW(R;{{{\hat{u}}}_{k-1}},{{U}_{k-1}})\text{G}(\nu ;{{{\hat{a}}}_{k-1}},{{{\hat{b}}}_{k-1}}) \\ \end{align}$ (13)

从式(13) 可以看到,联合后验滤波PDF $p(\pmb{\zeta}_{k}$,$λ_{k},R,\nu|\pmb{y}_{1:k})$ 不存在解析解. 为了获得联合后验滤波PDF的一个近似解,将利用变分贝叶斯方法[32]寻找$p(\pmb{\zeta}_{k},λ_{k},R,\nu|\pmb{y}_{1:k})$的一个自由分解近似,即

$p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k}})\approx {{q}_{f}}({{\zeta }_{k}}){{q}_{f}}({{\lambda }_{k}}){{q}_{f}}(R){{q}_{f}}(\nu )$ (14)

其中,$q_{f}(\pmb{\zeta}_{k})$,$q_{f}(λ_{k})$,$q_{f}(R)$,$q_{f}(\nu)$ 分别表示扩展状态$\pmb{\zeta}_{k}$、辅助参数$λ_{k}$、尺度矩阵 R、自由度参数$\nu$ 的近似后验滤波PDF.根据变分贝叶斯方法,这些近似的后验滤波PDF 是通过最小化$q_{f}(\pmb{\zeta}_{k})q_{f}(λ_{k})q_{f}(R)q_{f}(\nu)$$p(\pmb{\zeta}_{k},λ_{k},R,\nu|\pmb{y}_{1:k})$之间的相对熵而获得的,即[32]

$\begin{align} &\left\{ {{q}_{f}}({{\zeta }_{k}}),{{q}_{f}}({{\lambda }_{k}}),{{q}_{f}}(R),{{q}_{f}}(\nu ) \right\}= \\ &\arg \min \text{KLD}[{{q}_{f}}({{\zeta }_{k}}){{q}_{f}}({{\lambda }_{k}}){{q}_{f}}(R){{q}_{f}}(\nu )|| \\ &p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k}})] \\ \end{align}$ (15)

其中,$\mathrm{KLD}$ 表示相对熵,其定义如下:

$KLD\left[ q(x)||p(x) \right]:=\int{q}(x)\log \frac{q(x)}{p(x)}\text{d}x$ (16)

式(15) 的最优解满足如下方程:

$\log {q_f}({\zeta _k}) = {\rm{E}}_{\lambda ,R,\nu }^f[\log p({\zeta _k},{\lambda _k},R,\nu ,{y_{1:k}})] + {c_\zeta }$ (17)
$\log {{q}_{f}}({{\lambda }_{k}})=\text{E}_{\zeta ,R,\nu }^{f}[\log p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu ,{{y}_{1:k}})]+c\lambda $ (18)
$\log {{q}_{f}}(R)=\text{E}_{\xi ,\lambda ,v}^{f}[\log p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu ,{{y}_{1:k}})]+{{c}_{R}}$ (19)
$\log {{q}_{f}}(\nu )=\text{E}_{\xi ,\lambda ,R}^{f}[\log p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu ,{{y}_{1:k}})]+{{c}_{\nu }}$ (20)

其中,$c_{\pmb{\zeta}}$,$c_{λ}$,$c_{R}$,$c_{\nu}$分别表示与扩展状态$\pmb{\zeta}_{k}$、辅助参数$λ_{k}$、尺度矩阵 R、自由度参数$\nu$ 无关的常值,$\log(\cdot)$ 表示自然对数函数,$\mathrm{E}_{x}^{f}[\cdot]$表示关于变量 x 的近似后验滤波PDF的期望. 因为$q_{f}(\pmb{\zeta}_{k})$,$q_{f}(λ_{k})$,$q_{f}(R)$,$q_{f}(\nu)$ 相互耦合,所以不能直接求解出式(17) ~(20) ,而需要利用固定点迭代方法来求解式(17) ~ (20) .固定点迭代方法在迭代更新 $q_{f}(\pmb{\zeta}_{k})$,$q_{f}(λ_{k})$,$q_{f}(R)$,$q_{f}(\nu)$ 中的某一个PDF时,需要固定其他的PDF为上次的迭代值[32].

2.1 计算近似的后验滤波PDF

根据贝叶斯定理,联合PDF $p(\pmb{\zeta}_{k},λ_{k},R,\nu,\pmb{y}_{1:k})$ 可以表示如下:

$\begin{align} &p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu ,{{y}_{1:k}})=p({{y}_{k}}|{{\zeta }_{k}},{{\lambda }_{k}},R)\times \\ &p({{\zeta }_{k}},{{\lambda }_{k}},R,\nu |{{y}_{1:k-1}}) \\ \end{align}$ (21)

将式(9) 和式(11) 代入式(21) 中,可以得到:

$\begin{align} &p\left( {{\zeta }_{k}},{{\lambda }_{k}},R,\nu ,{{y}_{1:k}} \right)=\text{N}\left( {{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\frac{R}{{{\lambda }_{k}}} \right)\times \\ &\text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta } \right)\text{G}\left( {{\lambda }_{k}};\frac{\nu }{2},\frac{\nu }{2} \right)\times \\ &IW\left( R;{{{\hat{u}}}_{k-1}},{{U}_{k-1}} \right)\text{G}\left( \nu ;{{{\hat{a}}}_{k-1}},{{{\hat{b}}}_{k-1}} \right)\times \\ &p\left( {{y}_{1:k-1}} \right) \\ \end{align}$ (22)

将式(22) 代入式(17) 中,$\log q_{f}(\pmb{\zeta}_{k})$可以被计算如下:

$\begin{align} &\log {{q}_{f}}({{\zeta }_{k}})=\log \text{N}({{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta })- \\ &0.5{{[{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]}^{\text{T}}}\text{E}_{R}^{f}[{{R}^{-1}}]\text{E}_{\lambda }^{f}[{{\lambda }_{k}}][{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]+{{c}_{\zeta }} \\ \end{align}$ (23)

其中,$\text{E}_{R}^{f}[\cdot ]$ 表示关于近似滤波PDF ${{q}_{f}}(R)$的期望. 定义如下的修正量测噪声协方差矩阵$\tilde{R}_{k}^{f}$:

$\tilde{R}_{k}^{f}=\frac{{{\left\{ \text{E}_{R}^{f}[{{R}^{-1}}] \right\}}^{-1}}}{\text{E}_{\lambda }^{f}[{{\lambda }_{k}}]}$ (24)

将式(24) 代入式(23) 中,可以得到:

$\begin{align} &\log {{q}_{f}}({{\zeta }_{k}})=\log \text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta } \right)+ \\ &\log \text{N}\left( {{y}_{k}};{{g}_{k}}\left( {{\zeta }_{k}} \right),\tilde{R}_{k}^{f} \right)+{{c}_{\zeta }} \\ \end{align}$ (25)

根据式(25) ,可以得到:

${{q}_{f}}({{\zeta }_{k}})\propto \text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta } \right)\text{N}\left( {{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\tilde{R}_{k}^{f} \right)$ (26)

从式(26) 可以看到,$q_{f}(\pmb{\zeta}_{k})$ 等同于具有修正似然PDF $\mathrm{N}(\pmb{y}_{k};\pmb{g}_{k}(\pmb{\zeta}_{k}),\tilde{R}_{k}^{f})$的非线性系统的后验滤波PDF. 因此,利用带一步延迟状态的高斯近似滤波器$q_{f}(\pmb{\zeta}_{k})$ 可以被近似为高斯PDF[19],即

${{q}_{f}}({{\zeta }_{k}})=\text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k}},P_{k|k}^{\zeta \zeta } \right)$ (27)

其中

$\begin{align} &{{\widehat{\zeta }}_{k|k}}=\left[ \begin{matrix} {{\widehat{x}}_{k|k}} \\ {{\widehat{x}}_{k-1|k}} \\ \end{matrix} \right]P_{k|k}^{\zeta \zeta }= \\ &\left[ \begin{matrix} {{P}_{k|k}}&{{({{P}_{k-1,k|k}})}^{\text{T}}} \\ {{P}_{k-1,k|k}}&{{P}_{k-1|k}} \\ \end{matrix} \right] \\ \end{align}$ (28)

将式(22) 代入式(18) 中,$\log q_{f}(λ_{k})$ 可以被计算如下:

$\begin{align} &\log {{q}_{f}}({{\lambda }_{k}})=\left( \frac{m+\text{E}_{\nu }^{f}[\nu ]}{2}-1 \right)\log {{\lambda }_{k}}- \\ &\frac{\text{E}_{\nu }^{f}[\nu ]+\text{tr}\{D_{k}^{f}\text{E}_{R}^{f}[{{R}^{-1}}]\}}{2}{{\lambda }_{k}}+{{c}_{\lambda }} \\ \end{align}$ (29)

其中,$\mathrm{tr}(\cdot)$ 表示矩阵的迹运算,$D_{k}^{f}$ 定义如下:

$D_{k}^{f}=\text{E}_{\zeta }^{f}\left\{ [{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]{{[{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]}^{\text{T}}} \right\}$ (30)

根据式(29) 可以知道,$q_{f}(λ_{k})$ 为 Gamma PDF,即

${{q}_{f}}({{\lambda }_{k}})=\text{G}\left( {{\lambda }_{k}};\alpha _{k}^{f},\beta _{k}^{f} \right)$ (31)

其中,形状参数$\alpha_{k}^{f}$ 和比率参数$\beta_{k}^{f}$可以表示如下:

$\alpha _{k}^{f}=\frac{m+\text{E}_{\nu }^{f}[\nu ]}{2}$ (32)
$\beta _{k}^{f}=\frac{\text{E}_{\nu }^{f}[\nu ]+\text{tr}\left\{ D_{k}^{f}\text{E}_{R}^{f}[{{R}^{-1}}] \right\}}{2}$ (33)

将式(22) 代入式(19) 中,$\log q_{f}(R)$ 可以被计算如下:

$\begin{align} &\log {{q}_{f}}(R)=-0.5({{{\hat{u}}}_{k-1}}+m+1+1)\log |R|- \\ &0.5\text{tr}\left( [{{U}_{k-1}}+\text{E}f({{\lambda }_{k}})D_{k}^{f}]{{R}^{-1}} \right)+{{c}_{R}} \\ \end{align}$ (34)

根据式(34) 可以知道,$q_{f}(R)$ 为逆 Wishart PDF,即

${{q}_{f}}(R)=IW(R;{{{\hat{u}}}_{k}},{{U}_{k}})$ (35)

其中,自由度参数$\hat{u}_{k}$ 和逆尺度矩阵$U_{k}$ 可以表示如下:

${{{\hat{u}}}_{k}}={{{\hat{u}}}_{k-1}}+1$ (36)
${{U}_{k}}={{U}_{k-1}}+\text{E}_{\lambda }^{f}({{\lambda }_{k}})D_{k}^{f}$ (37)

将式(22) 代入式(20) 中,$\log q_{f}(\nu)$ 可以被计算如下:

$\begin{align} &\log {{q}_{f}}(\nu )=0.5\nu \log (0.5\nu )-\log \Gamma (0.5\nu )+ \\ &(0.5\nu -1)\text{E}_{\lambda }^{f}(\log {{\lambda }_{k}})-0.5\nu \text{E}_{\lambda }^{f}({{\lambda }_{k}})+ \\ &({{{\hat{a}}}_{k-1}}-1)\log \nu -{{{\hat{b}}}_{k-1}}\nu +{{c}_{\nu }} \\ \end{align}$ (38)

利用 Stirling's 近似[27],可以得到:

$\log \Gamma (0.5\nu )=(0.5\nu -0.5)\log (0.5\nu )-0.5\nu $ (39)

将式(39) 代入式(38) 中,可以得到:

$\begin{align} &\log {{q}_{f}}(\nu )=({{{\hat{a}}}_{k-1}}+0.5-1)\log \nu - \\ &[{{{\hat{b}}}_{k-1}}-0.5-0.5\text{E}_{\lambda }^{f}(\log {{\lambda }_{k}})+0.5\text{E}_{\lambda }^{f}({{\lambda }_{k}})]\nu +{{c}_{\nu }} \\ \end{align}$ (40)

根据式(40) 可以知道,$q_{f}(\nu)$ 为 Gamma PDF,即

${{q}_{f}}(\nu )=\text{G}(\nu ;{{{\hat{a}}}_{k}},{{{\hat{b}}}_{k}})$ (41)

其中,形状参数$\hat{a}_{k}$ 和比率参数$\hat{b}_{k}$ 可以表示如下:

${{{\hat{a}}}_{k}}={{{\hat{a}}}_{k-1}}+0.5$ (42)
${\hat b_k} = {\hat b_{k - 1}} - 0.5 - 0.5{\rm{E}}_\lambda ^f(\log {\lambda _k}) + 0.5{\rm{E}}_\lambda ^f({\lambda _k})$ (43)

从式(27) 、(35) 和(41) 中可以看到,k 时刻扩展状态$pmb{\zeta}_{k}$、尺度矩阵 R、自由度参数$\nu$的近似后验滤波PDF被分别更新为高斯、逆 Wishart、Gamma. 从而状态$\pmb{x}_{k}$ 的滤波估计为${\hat{\pmb x}}_{k|k}$,尺度矩阵 R和自由度参数$\nu$ 基于量测$\pmb{y}_{1:k}$$ 的滤波估计可以表示如下:

${{{\hat{R}}}_{k}}=\frac{{{U}_{k}}}{{{{\hat{u}}}_{k}}-m-1}$ (44)
${{{\hat{\nu }}}_{k}}=\frac{{{{\hat{a}}}_{k}}}{{{{\hat{b}}}_{k}}}$ (45)
2.2 计算滤波所需期望

本节的目的是求解在计算近似后验滤波PDF所需的期望. 根据式(27) 、(31) 、(35) 和(41) ,并利用 Gamma PDF 和逆 Wishart PDF 的性质,可以得到:

$\text{E}_{\lambda }^{f}[{{\lambda }_{k}}]=\frac{\alpha _{k}^{f}}{\beta _{k}^{f}}$ (46)
$E_{\lambda }^{f}(\log {{\lambda }_{k}})=\psi (\alpha _{k}^{f})-\log \beta _{k}^{f}$ (47)
$E_{R}^{f}[{{R}^{-1}}]=({{{\hat{u}}}_{k}}-m-1){{[{{U}_{k}}]}^{-1}}$ (48)
$E_{\nu }^{f}[\nu ]=\frac{{{{\hat{a}}}_{k}}}{{{{\hat{b}}}_{k}}}$ (49)
$\begin{align} &D_{k}^{f}=\int{\left[ {{y}_{k}}-{{g}_{k}}\left( {{\varsigma }_{k}} \right) \right]}{{\left[ {{y}_{k}}-{{g}_{k}}\left( {{\varsigma }_{k}} \right) \right]}^{\text{T}}}\times \\ &\text{N}\left( {{\zeta }_{k}};{{\widehat{\zeta }}_{k|k}},P_{k|k}^{\zeta \zeta } \right)\text{d}{{\zeta }_{k}} \\ \end{align}$ (50)

其中,$\psi(\cdot)$ 表示 digamma 函数.

2.4 带有色厚尾量测噪声的鲁棒高斯近似滤波算法

本节将基于固定点迭代方法给出提出的带有色厚尾量测噪声的高斯近似滤波算法的详细实施过程.

算法 1. 带有色厚尾量测噪声的高斯近似滤波算法

步骤 1. k时刻的滤波输入: ${{y}_{k}},N,{{Q}_{k-1}},{{\widehat{x}}_{k-1|k-1}},{{P}_{k-1|k-1}},{{\hat{u}}_{k-1}},{{U}_{k-1}},{{\hat{a}}_{k-1}},{{\hat{b}}_{k-1}}$.

步骤 2. 时间更新.

步骤 2.1. 计算状态一步预测估计${\hat{\pmb x}}_{k|k-1}$、预测误差协方差矩阵 $P_{k|k-1}$、互协方差矩阵$P_{k-1,k|k-1}$.

$\begin{align} &{{\widehat{x}}_{k|k-1}}=\int{{{f}_{k-1}}({{x}_{k-1}})\times } \\ &\text{N}({{x}_{k-1}};{{\widehat{x}}_{k-1|k-1}},{{P}_{k-1|k-1}})\text{d}{{x}_{k-1}} \\ \end{align}$ (51)
$\begin{align} &{{P}_{k|k-1}}=\int{{{f}_{k-1}}({{x}_{k-1}})f_{k-1}^{\text{T}}({{x}_{k-1}})\times } \\ &N({{x}_{k-1}};{{\widehat{x}}_{k-1|k-1}},{{P}_{k-1|k-1}})\text{d}{{x}_{k-1}}- \\ &{{\widehat{x}}_{k|k-1}}\widehat{x}_{k|k-1}^{\text{T}}+{{Q}_{k-1}} \\ \end{align}$ (52)
$\begin{align} &{{P}_{k-1,k|k-1}}=\int{{{x}_{k-1}}f_{k-1}^{\text{T}}({{x}_{k-1}})\times } \\ &\text{N}({{x}_{k-1}};{{\widehat{x}}_{k-1|k-1}},{{P}_{k-1|k-1}})\text{d}{{x}_{k-1}}- \\ &{{\widehat{x}}_{k-1|k-1}}\widehat{x}_{k|k-1}^{\text{T}} \\ \end{align}$ (53)

步骤 2.2. 计算扩展状态一步预测估计$\hat{\pmb{\zeta}}_{k|k-1}$及相应的预测误差协方差矩阵${P}_{k|k-1}^{\pmb{\zeta\zeta}}$.

$\begin{align} &{{\widehat{\zeta }}_{k|k-1}}=\left[ \begin{matrix} {{\widehat{x}}_{k|k-1}} \\ {{\widehat{x}}_{k-1|k-1}} \\ \end{matrix} \right]P_{k|k-1}^{\zeta \zeta }= \\ &\left[ \begin{matrix} {{P}_{k|k-1}}&P_{k-1,k|k-1}^{\text{T}} \\ {{P}_{k-1,k|k-1}}&{{P}_{k-1|k-1}} \\ \end{matrix} \right] \\ \end{align}$ (54)

步骤 3. 量测更新.

步骤 3.1. 计算量测一步预测估计${\hat{\pmb y}}_{k|k-1}$、误差协方差矩阵$S_{k}$、互协方差矩阵${P}_{k|k-1}^{\pmb{xy}}$${P}_{k-1,k|k-1}^{\pmb{xy}}$.

${{\widehat{y}}_{k|k-1}}=\int{{{g}_{k}}({{\zeta }_{k}})}\text{N}({{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta })\text{d}{{\zeta }_{k}}$ (55)
$\begin{align} &{{S}_{k}}=\int{{{g}_{k}}({{\zeta }_{k}})}g_{k}^{\text{T}}({{\zeta }_{k}})\times \\ &N({{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta })\text{d}{{\zeta }_{k}}-{{\widehat{y}}_{k|k-1}}\widehat{y}_{k|k-1}^{\text{T}} \\ \end{align}$ (56)
$\begin{align} &P_{k|k-1}^{xy}=\int{{{x}_{k}}g_{k}^{\text{T}}({{\zeta }_{k}})}\times \\ &\text{N}({{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta })\text{d}{{\zeta }_{k}}-{{\widehat{x}}_{k|k-1}}\widehat{y}_{k|k-1}^{\text{T}} \\ \end{align}$ (57)
$\begin{align} &P_{k-1,k|k-1}^{xy}=\int{{{x}_{k-1}}g_{k}^{\text{T}}({{\zeta }_{k}})}\times \\ &N({{\zeta }_{k}};{{\widehat{\zeta }}_{k|k-1}},P_{k|k-1}^{\zeta \zeta })\text{d}{{\zeta }_{k}}-{{\widehat{x}}_{k-1|k-1}}\widehat{y}_{k|k-1}^{\text{T}} \\ \end{align}$ (58)

步骤 3.2. 固定点迭代初始化:$\hat{u}_{k}^{(0)}={{\hat{u}}_{k-1}}+1,U_{k}^{(0)}={{U}_{k-1}},\hat{a}_{k}^{(0)}={{\hat{a}}_{k-1}}+0.5,\hat{b}_{k}^{(0)}={{\hat{b}}_{k-1}},\alpha _{k}^{f(0)}=1,\beta _{k}^{f(0)}=1,\widehat{\zeta }_{k|k}^{(0)}={{\widehat{\zeta }}_{k|k-1}},P_{k|k}^{\zeta \zeta (0)}=P_{k|k-1}^{\zeta \zeta }$,i=0.

步骤 3.3. 利用式(46) 、(48) 和(49) 分别计算第 $i+1$ 次迭代所需的期望$\text{E}_{\lambda }^{f}(i)[{{\lambda }_{k}}],\text{E}_{R}^{f(i)}[{{R}^{-1}}],\text{E}_{\nu }^{f(i)}[\nu ]$,并利用式(24) 计算 $\tilde{R}_{k}^{f(i)}$.

步骤 3.4. 计算第i+1次迭代的新息协方差矩阵${P}_{k|k-1}^{\pmb{yy}(i+1) }$、滤波增益矩阵$K_{k}^{(i+1) }$、滤波估计${\hat{\pmb x}}_{k|k}^{(i+1) }$及相应的估计误差协方差矩阵 $P_{k|k}^{(i+1) }$、一步平滑增益矩阵$K_{k-1}^{s(i+1) }$、一步平滑估计${\hat{\pmb x}}_{k-1|k}^{(i+1) }$及相应的估计误差协方差矩阵$P_{k-1|k}^{(i+1) }$、互协方差矩阵$P_{k-1,k|k}^{(i+1) }$.

$P_{k|k-1}^{yy(i+1)}={{S}_{k}}+\tilde{R}_{k}^{f(i)}$ (59)
$K_{k}^{(i+1)}=P_{k|k-1}^{xy}{{[P_{k|k-1}^{yy(i+1)}]}^{-1}}$ (60)
$\widehat{x}_{k|k}^{(i+1)}={{\widehat{x}}_{k|k-1}}+K_{k}^{(i+1)}({{y}_{k}}-{{\widehat{y}}_{k|k-1}})$ (61)
$P_{k|k}^{(i+1)}={{P}_{k|k-1}}-K_{k}^{(i+1)}P_{k|k-1}^{yy(i+1)}{{(K_{k}^{(i+1)})}^{\text{T}}}$ (62)
$K_{k-1}^{s(i+1)}=P_{k-1,k|k-1}^{xy}{{[P_{k|k-1}^{yy(i+1)}]}^{-1}}$ (63)
$\widehat{x}_{k-1|k}^{(i+1)}={{\widehat{x}}_{k-1|k-1}}+K_{k-1}^{s(i+1)}({{y}_{k}}-{{\widehat{y}}_{k|k-1}})$ (64)
$P_{k-1|k}^{(i+1)}={{P}_{k-1|k-1}}-K_{k-1}^{s(i+1)}P_{k|k-1}^{yy(i+1)}{{(K_{k-1}^{s(i+1)})}^{\text{T}}}$ (65)
$P_{k-1,k|k}^{(i+1)}={{P}_{k-1,k|k-1}}-K_{k-1}^{s(i+1)}P_{k|k-1}^{yy(i+1)}{{(K_{k}^{(i+1)})}^{\text{T}}}$ (66)

步骤 3.5. 计算第 $i+1$ 次迭代扩展状态的滤波估计$\hat{\pmb{\zeta}}_{k|k}^{(i+1) }$ 及相应的估计误差协方差矩阵${P}_{k|k}^{\pmb{\zeta\zeta}(i+1) }$.

$\begin{align} &\widehat{\zeta }_{k|k}^{(i+1)}=\left[ \begin{matrix} \widehat{x}_{k|k}^{(i+1)} \\ \widehat{x}_{k-1|k}^{(i+1)} \\ \end{matrix} \right]P_{k|k}^{\zeta \zeta (i+1)}= \\ &\left[ \begin{matrix} P_{k|k}^{(i+1)}&{{(P_{k-1,k|k}^{(i+1)})}^{\text{T}}} \\ P_{k-1,k|k}^{(i+1)}&P_{k-1|k}^{(i+1)} \\ \end{matrix} \right] \\ \end{align}$ (67)

步骤 3.6. 利用式(50) 和式(67) 计算 $D_{k}^{f(i+1) }$,$并代入$D_{k}^{f(i+1) }$,$\mathrm{E}_{R}^{f(i)}[R^{-1}]$,$\mathrm{E}_{\nu}^{f(i)}[\nu]$ 到式(32) 和式(33) 中计算$\alpha_{k}^{f(i+1) }$$\beta_{k}^{f(i+1) }$.

步骤 3.7. 代入$\alpha_{k}^{f(i+1) }$,$\beta_{k}^{f(i+1) }$到式(46) 和式(47) 中计算 $\mathrm{E}_{λ}^{f(i+1) }[λ_{k}]$,$\mathrm{E}_{λ}^{f(i+1) }(\logλ_{k})$,并将$D_{k}^{f(i+1) }$,$\mathrm{E}_{λ}^{f(i+1) }[λ_{k}]$,$\mathrm{E}_{λ}^{f(i+1) }(\logλ_{k})$代入到式(36) 和式(37) 及式(42) 和式(43) ,计算$u_{k}^{(i+1) }$,$U_{k}^{(i+1) }$,$\hat{a}_{k}^{(i+1) }$,$\hat{b}_{k}^{(i+1) }$.

步骤 3.8. $i+1\rightarrow{i}$,并检查迭代终止条件 iN-1.如果这个条件满足,那么迭代终止; 否则,迭代继续,并返回到步骤3.3.

步骤 4. 经过 N 次固定点迭代,$\hat{\pmb{\zeta}}_{k|k}=\hat{\pmb{\zeta}}_{k|k}^{(N)}$,${P}_{k|k}^{\pmb{\zeta\zeta}}={P}_{k|k}^{\pmb{\zeta\zeta}(N)}$,$\hat{u}_{k}=\hat{u}_{k}^{(N)}$,${U}_{k}={U}_{k}^{(N)}$,$\hat{a}_{k}=\hat{a}_{k}^{(N)}$,${{\hat{b}}_{k}}=\hat{b}_{k}^{(N)}$,并利用式(28) ,(44) 和(45) 分别获得状态估计及相应的估计误差协方差矩阵${\hat{\pmb x}}_{k|k}$${P}_{k|k}$、 尺度矩阵估计$\hat{R}_{k}$、自由度参数估计$\hat{\nu}_{k}$.

步骤 5. k 时刻的滤波输出: ${\hat{\pmb x}}_{k|k}$,$P_{k|k}$,$\hat{R}_{k}$,$\hat{\nu}_{k}$,$\hat{u}_{k}$,$U_{k}$,$\hat{a}_{k}$,$\hat{b}_{k}$.

3 带有色厚尾量测噪声的鲁棒高斯近似平滑器

本节的目的是基于量测$\pmb{y}_{1:M}$,同时估计状态 $\pmb{x}_{0:M}$、尺度矩阵 R和自由度参数$\nu$,其中量测 $\pmb{y}_{1:M}$:=$\{\pmb{y}_{j}\}_{j=1}^{M}$. 根据贝叶斯准则,联合后验平滑PDF $p(\pmb{x}_{0:M},λ_{1:M},R,\nu|\pmb{y}_{1:M})$ 可以表示如下:

$\begin{align} &p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu |{{y}_{1:M}})= \\ &\frac{p({{y}_{1:M}},{{x}_{0:M}},{{\lambda }_{1:M}},R,\nu )}{p({{y}_{1:M}})}\propto \\ &p({{x}_{0}},R,\nu )\prod\limits_{k=1}^{M}{[}p({{y}_{k}}|{{\zeta }_{k}},{{\lambda }_{k}},R)\times \\ &p({{x}_{k}}|{{x}_{k-1}})p({{\lambda }_{k}}|\nu )] \\ \end{align}$ (68)

利用式(1) ,(5) ,(9) 和(10) ,式(68) 可以重新表示如下:

$\begin{align} &p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu |{{y}_{1:M}})\propto \\ &\text{N}({{x}_{0}};{{\widehat{x}}_{0|0}},{{P}_{0|0}})IW(R;{{u}_{0}},{{U}_{0}})\text{G}(\nu ;{{a}_{0}},{{b}_{0}})\times \\ &\prod\limits_{k=1}^{M}{[}\text{N}({{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\frac{R}{{{\lambda }_{k}}})\times \\ &\text{N}({{x}_{k}};{{f}_{k-1}}({{x}_{k-1}}),{{Q}_{k-1}})\text{G}({{\lambda }_{k}};\frac{\nu }{2},\frac{\nu }{2})] \\ \end{align}$ (69)

从式(69) 可以看到,联合后验平滑PDF $p(\pmb{x}_{0:M},λ_{1:M},R,\nu|\pmb{y}_{1:M})$ 不存在解析解. 与求解联合后验滤波PDF $p(\pmb{\zeta}_{k},λ_{k},R,\nu|\pmb{y}_{1:k})$ 类似,利用变分贝叶斯方法寻找$p(\pmb{x}_{0:M},λ_{1:M},R,\nu|\pmb{y}_{1:M})$的一个自由分解近似,即

$\begin{align} &p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu |{{y}_{1:M}})\approx \\ &{{q}_{s}}({{x}_{0:M}}){{q}_{s}}({{\lambda }_{1:M}}){{q}_{s}}(R){{q}_{s}}(\nu ) \\ \end{align}$ (70)

其中,$q_{s}(\pmb{x}_{0:M})$,$q_{s}(λ_{1:M})$,$q_{s}(R)$,$q_{s}(\nu)$ 分别表示状态$\pmb{x}_{0:M}$、辅助参数$λ_{1:M}$、尺度矩阵 R、自由度参数$\nu$ 的近似后验平滑PDF,且满足下式:

$\begin{align} &\log {{q}_{s}}({{x}_{0:M}})= \\ &E_{\lambda R,\nu }^{s}[\log p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})]+{{c}_{x}} \\ \end{align}$ (71)
$\begin{align} &\log {{q}_{s}}({{\lambda }_{1:M}})= \\ &E_{x,R,\nu }^{s}[\log p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})]+c\lambda \\ \end{align}$ (72)
$\begin{align} &\log {{q}_{s}}(R)= \\ &E_{x,\lambda ,\nu }^{s}[\log p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})]+{{c}_{R}} \\ \end{align}$ (73)
$\begin{align} &\log {{q}_{s}}(\nu )= \\ &E_{x,\lambda ,R}^{s}[\log p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})]+{{c}_{\nu }} \\ \end{align}$ (74)

其中,$\mathrm{E}_{x}^{s}[\cdot]$ 表示关于变量 x的近似后验平滑PDF的期望. 与求解式(17) ~(20) 一样,我们也需要利用固定点迭代方法求解式(71) ~(74) .

3.1 计算近似的后验平滑PDF

根据贝叶斯定理,联合PDF $p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})$ 可以表示如下:

$\begin{align} &p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})=p({{x}_{0}},R,\nu )\times \\ &\prod\limits_{k=1}^{M}{\left[ p\left( {{y}_{k}}|{{\xi }_{k}},{{\lambda }_{k}},R \right)p\left( {{x}_{k}}{{|}_{k-1}} \right)p\left( {{\lambda }_{k}}|v \right) \right]} \\ \end{align}$ (75)

利用式(1) ,(5) ,(9) 和(10) ,式(75) 可以重新表示如下:

$\begin{align} &p({{x}_{0:M}},{{\lambda }_{1:M}},R,\nu ,{{y}_{1:M}})= \\ &\text{N}({{x}_{0}};{{\widehat{x}}_{0|0}},{{P}_{0|0}})IW(R;{{u}_{0}},{{U}_{0}})\text{G}(\nu ;{{a}_{0}},{{b}_{0}})\times \\ &\prod\limits_{k=1}^{M}{[}\text{N}\left( {{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\frac{R}{{{\lambda }_{k}}} \right)\times \\ &\text{N}({{x}_{k}};{{f}_{k-1}}({{x}_{k-1}}),{{Q}_{k-1}})\text{G}\left( {{\lambda }_{k}};\frac{\nu }{2},\frac{\nu }{2} \right)] \\ \end{align}$ (76)

将式(76) 代入式(71) 中,$\log q_{s}(\pmb{x}_{0:M})$ 可以被计算如下:

$\begin{align} &\log {{q}_{s}}({{x}_{0:M}})=\log \text{N}({{x}_{0}};{{\widehat{x}}_{0|0}},{{P}_{0|0}})+ \\ &\sum\limits_{k=1}^{M}{\log }\text{N}({{x}_{k}};{{f}_{k-1}}({{x}_{k-1}}),{{Q}_{k-1}})- \\ &0.5{{\sum\limits_{k-1}^{M}{\left[ {{y}_{k}}-{{g}_{k}}\left( {{\xi }_{k}} \right) \right]}}^{\text{T}}}\text{E}_{R}^{s}\left[ {{R}^{-1}} \right]\times \\ &\text{E}_{\lambda }^{s}[{{\lambda }_{k}}][{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]+{{c}_{x}} \\ \end{align}$ (77)

其中,$\mathrm{E}_{R}^{s}[\cdot]$ 表示关于近似平滑PDF $q_{s}(R)$的期望. 定义如下的修正量测噪声协方差矩阵$\tilde{R}_{k}^{s}$:

$\tilde{R}_{k}^{s}=\frac{{{\left\{ \text{E}_{R}^{s}[{{R}^{-1}}] \right\}}^{-1}}}{\text{E}_{\lambda }^{s}[{{\lambda }_{k}}]}$ (78)

将式(78) 代入式(77) 中,可以得到:

$\begin{align} &\log {{q}_{s}}({{x}_{0:M}})=\log \text{N}({{x}_{0}};{{\widehat{x}}_{0|0}},{{P}_{0|0}})+ \\ &\sum\limits_{k=1}^{M}{\log }\text{N}({{x}_{k}};{{f}_{k-1}}({{x}_{k-1}}),{{Q}_{k-1}})+ \\ &\sum\limits_{k=1}^{M}{\log }\text{N}({{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\tilde{R}_{k}^{s})+{{c}_{x}} \\ \end{align}$ (79)

根据式(79) ,$q_{s}(\pmb{x}_{0:M})$ 可以被计算如下:

$\begin{align} &{{q}_{s}}({{x}_{0:M}})\propto \text{N}({{x}_{0}};{{\widehat{x}}_{0|0}},{{P}_{0|0}})\times \\ &\prod\limits_{k=1}^{M}{\{}\text{N}({{y}_{k}};{{g}_{k}}({{\zeta }_{k}}),\tilde{R}_{k}^{s})\times \\ &\text{N}({{x}_{k}};{{f}_{k-1}}({{x}_{k-1}}),{{Q}_{k-1}})\} \\ \end{align}$ (80)

从式(80) 可以看到,$q_{s}(\pmb{x}_{0:M})$ 等同于具有修正似然PDF $\mathrm{N}(\pmb{y}_{k};\pmb{g}_{k}(\pmb{\zeta}_{k}),\tilde{R}_{k}^{s})$的非线性系统的后验平滑PDF. 因此,利用带一步延迟状态的高斯近似平滑器,$q_{s}(\pmb{x}_{k})$ 可以被近似为高斯PDF[20],即

${{q}_{s}}({{x}_{k}})=\text{N}({{x}_{k}};{{\widehat{x}}_{k|M}},{{P}_{k|M}})$ (81)

将式(76) 代入式(72) 中,$\log {{q}_{s}}({{\lambda }_{1:M}})$ 可以表示如下:

$\begin{align} &\log {{q}_{s}}({{\lambda }_{1:M}})=\sum\limits_{k=1}^{M}{\left( \frac{m+\text{E}_{\nu }^{s}[\nu ]}{2}-1 \right)}\log {{\lambda }_{k}}- \\ &\sum\limits_{k=1}^{M}{\frac{\text{E}_{\nu }^{s}[\nu ]+\text{tr}\{D_{k}^{s}\text{E}_{R}^{s}[{{R}^{-1}}]\}}{2}}{{\lambda }_{k}}+{{c}_{\lambda }} \\ \end{align}$ (82)

其中,$D_{k}^{s}$ 定义如下:

$D_{k}^{s}=\text{E}_{\zeta }^{s}\{[{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]{{[{{y}_{k}}-{{g}_{k}}({{\zeta }_{k}})]}^{\text{T}}}\}$ (83)

根据式(82) 可以知道,${{q}_{s}}({{\lambda }_{k}})$ 为Gamma PDF,即

${{q}_{s}}({{\lambda }_{k}})=\text{G}({{\lambda }_{k}};\alpha _{k}^{s},\beta _{k}^{s})$ (84)

其中,形状参数 $\alpha_{k}^{s}$ 和比率参数$\beta_{k}^{s}$可以表示如下:

$\alpha _{k}^{s}=\frac{m+\text{E}_{\nu }^{s}[\nu ]}{2}$ (85)
$\beta _{k}^{s}=\frac{\text{E}_{\nu }^{s}[\nu ]+\text{tr}\{D_{k}^{s}\text{E}_{R}^{s}[{{R}^{-1}}]\}}{2}$ (86)

将式(76) 代入式(73) 中,$\log q_{s}(R)$ 可以被计算如下:

$\begin{align} &\log {{q}_{s}}(R)=-0.5({{u}_{0}}+M+m+1)\log |R|- \\ &0.5\text{tr}[{{U}_{0}}+\sum\limits_{k=1}^{M}{\text{E}_{\lambda }^{s}}({{\lambda }_{k}})D_{k}^{s}]{{R}^{-1}}+{{c}_{R}} \\ \end{align}$ (87)

根据式(87) 可以知道,$q_{s}(R)$ 为逆 Wishart PDF,即

${{q}_{s}}(R)=IW(R;\hat{u},U)$ (88)

其中,自由度参数 $\hat{u}$ 和逆尺度矩阵 U 可以表示如下:

$\hat{u}={{u}_{0}}+M$ (89)
$U={{U}_{0}}+\sum\limits_{k=1}^{M}{\text{E}_{\lambda }^{s}}({{\lambda }_{k}})D_{k}^{s}$ (90)

将式(76) 代入式(74) 中,$\log q_{s}(\nu)$ 可以被计算如下:

$\begin{align} &\log {{q}_{s}}(\nu )=\sum\limits_{k=1}^{M}{\{0.5\nu \log (0.5\nu )-\log \Gamma (0.5\nu )}+ \\ &(0.5\nu -1)\text{E}_{\lambda }^{s}(\log {{\lambda }_{k}})-0.5\nu \text{E}_{\lambda }^{s}({{\lambda }_{k}})\}+ \\ &({{a}_{0}}-1)\log \nu -{{b}_{0}}\nu +{{c}_{\nu }} \\ \end{align}$ (91)

将式(39) 代入式(91) 中,可以得到:

$\begin{align} &\log {{q}_{s}}(\nu )=({{a}_{0}}+0.5M-1)\log \nu - \\ &\left\{ {{b}_{0}}-0.5M-0.5\sum\limits_{k=1}^{M}{\left[ \text{E}_{\lambda }^{s}\left( \log {{\lambda }_{k}} \right) \right.} \right.- \\ &\left. \left. \text{E}_{\lambda }^{s}({{\lambda }_{k}}) \right] \right\}v+{{c}_{v}} \\ \end{align}$ (92)

根据式(92) 可以知道,$q_{s}(\nu)$ 为 Gamma PDF,即

${{q}_{s}}(\nu )=\text{G}(\nu ;\hat{a},\hat{b})$ (93)

其中,形状参数 $\hat{a}$ 和比率参数$\hat{b}$ 可以表示如下:

$\hat{a}={{a}_{0}}+0.5M$ (94)
$\hat{b}={{b}_{0}}-0.5M-0.5\sum\limits_{k=1}^{M}{\left[ \text{E}_{\lambda }^{s}(\log {{\lambda }_{k}})-\text{E}_{\lambda }^{s}({{\lambda }_{k}}) \right]}$ (95)

从式(81) 、(88) 和(93) 可以看到,k 时刻的状态$\pmb{x}_{k}$ 、尺度矩阵R、自由度参数$\nu$ 的近似后验平滑PDF被分别更新为高斯、逆Wishart、Gamma. 从而状态$\pmb{x}_{k}$ 的平滑估计为${\hat{\pmb x}}_{k|M}$,尺度矩阵 R 和自由度参数$\nu$ 基于量测$\pmb{y}_{1:M}$的平滑估计可以表示如下:

$\hat{R}=\frac{U}{\hat{u}-m-1}$ (96)
$\hat{\nu }=\frac{{\hat{a}}}{{\hat{b}}}$ (97)
3.2 计算平滑所需期望

本节的目的是求解在计算近似后验平滑PDF所需的期望.根据式(84) 、(88) 和(93) ,并利用 Gamma PDF 和逆Wishart PDF的性质,可以得到:

$\text{E}_{\lambda }^{s}[{{\lambda }_{k}}]=\frac{\alpha _{k}^{s}}{\beta _{k}^{s}}$ (98)
$\text{E}_{\lambda }^{s}(\log {{\lambda }_{k}})=\psi (\alpha _{k}^{s})-\log \beta _{k}^{s}$ (99)
$\text{E}_{R}^{s}[{{R}^{-1}}]=(\hat{u}-m-1){{U}^{-1}}$ (100)
$\text{E}_{\nu }^{s}[\nu ]=\frac{{\hat{a}}}{{\hat{b}}}$ (101)

根据式(83) 可以知道,计算期望$D_{k}^{s}$ 需要$q_{s}(\pmb{\zeta}_{k})$. 但是,从式(81) 可以看到,现有的带一步延迟状态的高斯近似平滑器只能提供$\{q_{s}(\pmb{x}_{k})|0$\leq$k\leq M\}$. 因此,在计算期望$D_{k}^{s}$ 之前,需要利用$\{q_{s}(\pmb{x}_{k})|0\leq k\leq M\}$获得$q_{s}(\pmb{\zeta}_{k})$. 因为$q_{s}(\pmb{\zeta}_{k})$$p(\pmb{x}_{k},\pmb{x}_{k-1}|\pmb{y}_{1:M})$在固定点迭代中的高斯近似,所以可以得到:

$\begin{align} &p({{x}_{k}},{{x}_{k-1}}|{{y}_{1:M}})={{q}_{s}}({{\zeta }_{k}})= \\ &\text{N}\left( \left[ \begin{matrix} {{x}_{k}} \\ {{x}_{k-1}} \\ \end{matrix} \right];\left[ \begin{matrix} {{\widehat{x}}_{k|M}} \\ {{\widehat{x}}_{k-1|M}} \\ \end{matrix} \right], \right. \\ &\left. \left[ \begin{matrix} {{P}_{k|M}}&P_{k-1,k|M}^{\text{T}} \\ {{P}_{k-1,k|M}}&{{P}_{k-1|M}} \\ \end{matrix} \right] \right) \\ \end{align}$ (102)

其中

$\begin{align} &{{P}_{k-1,k|M}}=\int{\int{\left( {{x}_{k-1}}-{{\widehat{x}}_{k-1|M}} \right){{\left( {{x}_{k}}-{{\widehat{x}}_{k|M}} \right)}^{\text{T}}}}}\times \\ &p({{x}_{k}},{{x}_{k-1}}|{{y}_{1:M}})\text{d}{{x}_{k-1}}\text{d}{{x}_{k}} \\ \end{align}$ (103)

从式(102) 中可以看到,为了获得$q_{s}(\pmb{\zeta}_{k})$ 的高斯近似,需要利用式(103) 计算${P}_{k-1,k|M}$.

根据贝叶斯定理和模型(1) 和式(7) 的马尔科夫性质,可以得到:

$p\left( {{x}_{k}},{{x}_{k-1}}|{{y}_{1:M}} \right)=p\left( {{x}_{k-1}}|{{x}_{k}},{{y}_{1:k}} \right)p\left( {{x}_{k}}|{{y}_{1:M}} \right)$ (104)

在高斯近似平滑框架下,$p(\pmb{x}_{k-1}|\pmb{x}_{k},\pmb{y}_{1:k})$$p(\pmb{x}_{k}|\pmb{y}_{1:M})$都可以被近似为如下的高斯PDF[20]:

$p({{x}_{k-1}}|{{x}_{k}},{{y}_{1:k}})=\text{N}({{x}_{k-1}};{{\widehat{x}}_{k-1|k,k}},{{P}_{k-1|k,k}})$ (105)
$p({{x}_{k}}|{{y}_{1:M}})=\text{N}({{x}_{k}};{{\widehat{x}}_{k|M}},{{P}_{k|M}})$ (106)

其中,${\hat{\pmb x}}_{k-1|k,k}$$P_{k-1|k,k}$ 可以表示如下:

${{\widehat{x}}_{k-1|k,k}}={{\widehat{x}}_{k-1|k}}+{{A}_{k-1}}({{x}_{k}}-{{\widehat{x}}_{k|k}})$ (107)
${{P}_{k-1|k,k}}={{P}_{k-1|k}}-{{A}_{k-1}}{{P}_{k|k}}A_{k-1}^{\text{T}}$ (108)

将式(104) ~(108) 代入式(103) 中,可以得到:

$\begin{align} &{{P}_{k-1,k|M}}= \\ &\int{[}{{\widehat{x}}_{k-1|k}}+{{A}_{k-1}}\left( {{x}_{k}}-{{\widehat{x}}_{k|k}} \right)-{{\widehat{x}}_{k-1|M}}]\times \\ &{{\left( {{x}_{k}}-{{\widehat{x}}_{k|M}} \right)}^{\text{T}}}\text{N}\left( {{x}_{k}};{{\widehat{x}}_{k|M}},{{P}_{k|M}} \right)\text{d}{{x}_{k}} \\ \end{align}$ (109)

根据带一步延迟状态的高斯近似平滑器,${\hat{\pmb x}}_{k-1|M}$可以表示如下[20]:

${{\widehat{x}}_{k-1|M}}={{\widehat{x}}_{k-1|k}}+{{A}_{k-1}}({{\widehat{x}}_{k|M}}-{{\widehat{x}}_{k|k}})$ (110)

将式(110) 代入式(109) 中,可以得到:

$\begin{align} &{{P}_{k-1,k|M}}= \\ &\int{{{A}_{k-1}}}\left( {{x}_{k}}-{{\widehat{x}}_{k|M}} \right){{\left( {{x}_{k}}-{{\widehat{x}}_{k|M}} \right)}^{\text{T}}}\times \\ &\text{N}\left( {{x}_{k}};{{\widehat{x}}_{k|M}},{{P}_{k|M}} \right)\text{d}{{x}_{k}}= \\ &{{A}_{k-1}}{{P}_{k|M}} \\ \end{align}$ (111)

利用式(83) 和式(111) ,$D_{k}^{s}$ 可以被计算如下:

$\begin{array}{l} D_k^s = \int {\left[ {{\mathit{\boldsymbol{y}}_k} - {\mathit{\boldsymbol{g}}_k}\left( {{\mathit{\boldsymbol{x}}_k};{\mathit{\boldsymbol{x}}_{k - 1}}} \right)} \right]} {\left[ {{\mathit{\boldsymbol{y}}_k} - {\mathit{\boldsymbol{g}}_k}\left( {{\mathit{\boldsymbol{x}}_k};{\mathit{\boldsymbol{x}}_{k - 1}}} \right)} \right]^{\rm{T}}} \times \\ N\left( {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{x}}_k}}\\ {{\mathit{\boldsymbol{x}}_{k - 1}}} \end{array}} \right]} \right);\left[ {\begin{array}{*{20}{c}} {{{\mathit{\boldsymbol{\hat x}}}_{k|M}}}\\ {{{\mathit{\boldsymbol{\hat x}}}_{k - 1|M}}} \end{array}} \right],\\ \left. {\left[ {\begin{array}{*{20}{c}} {{P_{k|M}}}&{{{({A_{k - 1}}{P_{k|M}})}^{\rm{T}}}}\\ {{A_{k - 1}}{P_{k|M}}}&{{P_{k - 1|M}}} \end{array}} \right]} \right){\rm{d}}{\mathit{\boldsymbol{x}}_{k - 1}}{\rm{d}}{\mathit{\boldsymbol{x}}_k} \end{array}$ (112)
3.3 带有色厚尾量测噪声的鲁棒高斯近似平滑算法

本节将基于固定点迭代方法给出提出的带有色厚尾量测噪声的高斯近似平滑算法的详细实施过程.

算法 2. 带有色厚尾量测噪声的高斯近似平滑算法

步骤 1. 初始平滑输入: $\pmb{y}_{1:M}$,N,$\{Q_{k-1}|0\leq k$ $\leq$M\}$,${\hat{\pmb x}}_{0|0}$,$P_{0|0}$,${u}_{0}$,$U_{0}$,${a}_{0}$,${b}_{0}$.

步骤 2. 固定点迭代初始化: $\hat{u}^{(0) }={u}_{0}+M$,$U^{(0) }=U_{0}$,$\hat{a}^{(0) }={a}_{0}+0.5M$,$\hat{b}^{(0) }={b}_{0}$,$\{\alpha_{k}^{s(0) }=1|1\leq k\leq M\}$,$\{\beta_{k}^{s(0) }=$1|1$\leq$ k $\leq M\}$,$\hat{\pmb{x}}_{0|0}^{(0) }=$\hat{\pmb{x}}_{0|0}$,${P}_{0|0}^{(0) }={P}_{0|0}$,$i=0$.

步骤 3. 前向回路.

步骤 3.1. 计算第 $i+1$ 次迭代的状态一步预测估计${\hat{\pmb x}}_{k|k-1}^{(i+1) }$,预测误差协方差矩阵 $P_{k|k-1}^{(i+1) }$,互协方差矩阵 $P_{k-1,k|k-1}^{(i+1) }$.

$\begin{align} &\widehat{x}_{k|k-1}^{(i+1)}=\int{{{f}_{k-1}}({{x}_{k-1}})}\times \\ &\text{N}\left( {{x}_{k-1}};\widehat{x}_{k-1|k-1}^{(i+1)},P_{k-1|k-1}^{(i+1)} \right)\text{d}{{x}_{k-1}} \\ \end{align}$ (113)
$\begin{align} &P_{k|k-1}^{(i+1)}=\int{{{f}_{k-1}}({{x}_{k-1}})f_{k-1}^{\text{T}}({{x}_{k-1}})}\times \\ &N\left( {{x}_{k-1}};\widehat{x}_{k-1|k-1}^{(i+1)},P_{k-1|k-1}^{(i+1)} \right)\text{d}{{x}_{k-1}}- \\ &\widehat{x}_{k|k-1}^{(i+1)}{{\left( \widehat{x}_{k|k-1}^{(i+1)} \right)}^{\text{T}}}+{{Q}_{k-1}} \\ \end{align}$ (114)
$\begin{align} &P_{k-1,k|k-1}^{(i+1)}=\int{{{x}_{k-1}}f_{k-1}^{\text{T}}({{x}_{k-1}})}\times \\ &N\left( {{x}_{k-1}};\widehat{x}_{k-1|k-1}^{(i+1)},P_{k-1|k-1}^{(i+1)} \right)\text{d}{{x}_{k-1}}- \\ &\widehat{x}_{k-1|k-1}^{(i+1)}{{\left( \widehat{x}_{k|k-1}^{(i+1)} \right)}^{\text{T}}} \\ \end{align}$ (115)

步骤 3.2. 计算扩展状态一步预测估计 $\hat{\pmb{\zeta}}_{k|k-1}^{(i+1) }$及相应的预测误差协方差矩阵${P}_{k|k-1}^{\pmb{\zeta\zeta}(i+1) }$.

$\left\{ \begin{align} &\widehat{\zeta }_{k|k-1}^{(i+1)}=\left[ \begin{matrix} \widehat{x}_{k|k-1}^{(i+1)} \\ \widehat{x}_{k-1|k-1}^{(i+1)} \\ \end{matrix} \right] \\ &P_{k|k-1}^{\zeta \zeta (i+1)}=\left[ \begin{matrix} P_{k|k-1}^{(i+1)}&{{(P_{k-1,k|k-1}^{(i+1)})}^{\text{T}}} \\ P_{k-1,k|k-1}^{(i+1)}&P_{k-1|k-1}^{(i+1)} \\ \end{matrix} \right] \\ \end{align} \right.$ (116)

步骤 3.3. 计算量测一步预测估计 ${\hat{\pmb y}}_{k|k-1}^{(i+1) }$,误差协方差矩阵 $S_{k}^{(i+1) }$,互协方差矩阵${P}_{k|k-1}^{\pmb{xy}(i+1) }$,${P}_{k-1,k|k-1}^{\pmb{xy}(i+1) }$.

$\widehat{y}_{k|k-1}^{(i+1)}=\int{{{g}_{k}}({{\zeta }_{k}})}\text{N}\left( {{\zeta }_{k}};\widehat{\zeta }_{k|k-1}^{(i+1)},P_{k|k-1}^{\zeta \zeta (i+1)} \right)\text{d}{{\zeta }_{k}}$ (117)
$\begin{align} &S_{k}^{(i+1)}=\int{{{g}_{k}}({{\zeta }_{k}})g_{k}^{\text{T}}({{\zeta }_{k}})}\times \\ &N\left( {{\zeta }_{k}};\widehat{\zeta }_{k|k-1}^{(i+1)},P_{k|k-1}^{\zeta \zeta (i+1)} \right)\text{d}{{\zeta }_{k}}- \\ &\widehat{y}_{k|k-1}^{(i+1)}{{(\widehat{y}_{k|k-1}^{(i+1)})}^{\text{T}}} \\ \end{align}$ (118)
$\begin{align} &P_{k|k-1}^{xy(i+1)}=\int{{{x}_{k}}g_{k}^{\text{T}}({{\zeta }_{k}})}\times \\ &N\left( {{\zeta }_{k}};\widehat{\zeta }_{k|k-1}^{(i+1)},P_{k|k-1}^{\zeta \zeta (i+1)} \right)\text{d}{{\zeta }_{k}}- \\ &\widehat{x}_{k|k-1}^{(i+1)}{{(\widehat{y}_{k|k-1}^{(i+1)})}^{\text{T}}} \\ \end{align}$ (119)
$\begin{align} &P_{k-1,k|k-1}^{xy(i+1)}=\int{{{x}_{k-1}}g_{k}^{\text{T}}({{\zeta }_{k}})}\times \\ &N\left( {{\zeta }_{k}};\widehat{\zeta }_{k|k-1}^{(i+1)},P_{k|k-1}^{\zeta \zeta (i+1)} \right)\text{d}{{\zeta }_{k}}- \\ &\widehat{x}_{k-1|k-1}^{(i+1)}{{(\widehat{y}_{k|k-1}^{(i+1)})}^{\text{T}}} \\ \end{align}$ (120)

步骤 3.4. 利用式(98) 、式(100) 和式(101) 分别计算$\{\text{E}_{\lambda }^{s(i)}[{{\lambda }_{k}}]|1\le k\le M\}$$\mathrm{E}_{R}^{s(i)}[R^{-1}]$$\mathrm{E}_{\nu}^{s(i)}[\nu]$,并利用式(78) 计算 $\{\tilde{R}_{k}^{s(i)}|1\leq k\leq M\}$.

步骤 3.5. 计算第 $i+1$ 次迭代的新息协方差矩阵${P}_{k|k-1}^{\pmb{yy}(i+1) }$、滤波增益矩阵$K_{k}^{(i+1) }$、滤波估计${\hat{\pmb x}}_{k|k}^{(i+1) }$及相应的估计误差协方差矩阵 $P_{k|k}^{(i+1) }$、一步平滑增益矩阵$K_{k-1}^{s(i+1) }$、一步平滑估计 ${\hat{\pmb x}}_{k-1|k}^{(i+1) }$及相应的估计误差协方差矩阵$P_{k-1|k}^{(i+1) }$、互协方差矩阵$P_{k-1,k|k}^{(i+1) }$.

$P_{k|k-1}^{yy(i+1)}=S_{k}^{(i+1)}+\tilde{R}_{k}^{s(i)}$ (121)
$K_{k}^{(i+1)}=P_{k|k-1}^{xy(i+1)}{{[P_{k|k-1}^{yy(i+1)}]}^{-1}}$ (122)
$\widehat{x}_{k|k}^{(i+1)}=\widehat{x}_{k|k-1}^{(i+1)}+K_{k}^{(i+1)}({{y}_{k}}-\widehat{y}_{k|k-1}^{(i+1)})$ (123)
$P_{k|k}^{(i+1)}=P_{k|k-1}^{(i+1)}-K_{k}^{(i+1)}P_{k|k-1}^{yy(i+1)}{{(K_{k}^{(i+1)})}^{\text{T}}}$ (124)
$K_{k-1}^{s(i+1)}=P_{k-1,k|k-1}^{xy(i+1)}{{[P_{k|k-1}^{yy(i+1)}]}^{-1}}$ (125)
$\widehat{x}_{k-1|k}^{(i+1)}=\widehat{x}_{k-1|k-1}^{(i+1)}+K_{k-1}^{s(i+1)}({{y}_{k}}-\widehat{y}_{k|k-1}^{(i+1)})$ (126)
$P_{k-1|k}^{(i+1)}=P_{k-1|k-1}^{(i+1)}-K_{k-1}^{s(i+1)}P_{k|k-1}^{yy(i+1)}{{(K_{k-1}^{s(i+1)})}^{\text{T}}}$ (127)
$P_{k-1,k|k}^{(i+1)}=P_{k-1,k|k-1}^{(i+1)}-K_{k-1}^{s(i+1)}P_{k|k-1}^{yy(i+1)}{{(K_{k}^{(i+1)})}^{\text{T}}}$ (128)

步骤 4. 后向回路.

步骤 4.1. 计算第 $i+1$ 次迭代的平滑增益矩阵 $A_{k-1}^{(i+1) }$、平滑估计$\hat{\pmb{x}}_{k-1|M}^{(i+1) }$ 及相应的估计误差协方差矩阵${P}_{k-1|M}^{(i+1) }$.

$A_{k-1}^{(i+1)}=P_{k-1,k|k}^{(i+1)}{{(P_{k|k}^{(i+1)})}^{-1}}$ (129)
$\widehat{x}_{k-1|M}^{(i+1)}=\widehat{x}_{k-1|k}^{(i+1)}+A_{k-1}^{(i+1)}(\widehat{x}_{k|M}^{(i+1)}-\widehat{x}_{k|k}^{(i+1)})$ (130)
$P_{k-1|M}^{(i+1)}=$ (131)
$P_{k-1|k}^{(i+1)}+A_{k-1}^{(i+1)}[P_{k|M}^{(i+1)}-P_{k|k}^{(i+1)}]{{(A_{k-1}^{(i+1)})}^{\text{T}}}$ (132)

步骤 4.2. 利用式(111) 计算第 $i+1$ 次迭代的平滑互协方差矩阵$P_{k-1,k|M}^{(i+1) }$.

$P_{k-1,k|M}^{(i+1)}=A_{k-1}^{(i+1)}P_{k|M}^{(i+1)}$ (133)

步骤 5. 利用式(112) 计算 $\{D_{k}^{s(i+1) }|1\leq k \leq$M\}$,并代入$\{D_{k}^{s(i+1) }|1\leq k\leq M\}$,$\mathrm{E}_{R}^{s(i)}[R^{-1}]$,$\mathrm{E}_{\nu}^{s(i)}[\nu]$到式(85) 和式(86) 中计算 $\{\alpha_{k}^{s(i+1) },\beta_{k}^{s(i+1) }|$1 $\leq$ k $\leq$M\}$.

步骤 6.$\{\alpha_{k}^{s(i+1) }$,$\beta_{k}^{s(i+1) }|1$\leq$ k $\leq$M\}$代入式(98) 和式(99) 中计算$\{\text{E}_{\lambda }^{s(i+1)}[{{\lambda }_{k}}],\text{E}_{\lambda }^{s(i+1)}(\log {{\lambda }_{k}})|1\le k\le M\}$,并代入 $\{\text{E}_{\lambda }^{s(i+1)}[{{\lambda }_{k}}],\text{E}_{\lambda }^{s(i+1)}(\log {{\lambda }_{k}}),D_{k}^{s(i+1)}|1\le k\le M\}$到式(89) 和式(90) 及式(94) 和式(95) 中分别计算$\hat{u}^{i+1}$,$U^{i+1}$,$\hat{a}^{(i+1) }$,$\hat{b}^{(i+1) }$.

步骤 7. $i+1\rightarrow{i}$,并检查迭代终止条件 $i > N$ - 1.如果这个条件满足,那么迭代终止; 否则,迭代继续,并返回步骤3.

步骤 8. 经过 N 次固定点迭代,$\hat{\pmb{x}}_{k|k}=\hat{\pmb{x}}_{k|k}^{(N)}$,${P}_{k|k}$=${P}_{k|k}^{(N)}$,$\hat{u}=\hat{u}^{(N)}$,${U}={U}^{(N)}$,$\hat{a}=\hat{a}^{(N)}$,$\hat{b}$=$\hat{b}^{(N)}$,并利用式(96) 和式(97) 分别获得尺度矩阵估计 $\hat{R}$、自由度参数估计$\hat{\nu}$.

步骤 9. 平滑输出: $\{\hat{\pmb{x}}_{k|M}$,${P}_{k|M}|0\leq k\leq M\}$,$\hat{R}$,$\hat{\nu}$.

3.4 计算复杂度分析

本节将比较和分析提出的带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器、现有的带有色量测噪声的高斯近似滤波器和平滑器[19-20]、现有的鲁棒高斯近似滤波器和平滑器[21]的计算复杂度.为了比较方便,不失一般性,采用三阶球径容积准则来实施提出的和现有的高斯近似滤波器和平滑器,分别得到提出的鲁棒容积卡尔曼滤波器 (Cubature Kalman filter,CKF)、提出的鲁棒容积卡尔曼平滑器 (Cubature Kalman smoother,CKS)、现有的有色CKF[19]、现有的有色CKS[20]、现有的鲁棒CKF[21]、现有的鲁棒CKS[21].在实施提出方法和现有方法时,计算复杂度包括计算矩阵乘法、Cholesky分解、矩阵逆、对数函数、digamma函数、系统函数和量测函数所需的浮点运算数[33].提出方法和现有方法的浮点运算总数可以表示如下:

$\begin{align} &f{{l}_{PRCKF}}=\text{O}(8N{{n}^{3}}+3N{{m}^{3}}+9{{n}^{3}})+ \\ &13Nn{{m}^{2}}+3N{{n}^{2}}m+N{{m}^{3}}+2Nnm+ \\ &N{{m}^{2}}+8Nn{{M}_{h}}+N{{M}_{\psi }}+N{{M}_{l}}+4{{n}^{3}}+ \\ &8n{{m}^{2}}+8{{n}^{2}}m+2{{n}^{2}}+2nm+ \\ &{{m}^{2}}+2n{{M}_{f}}+8n{{M}_{h}} \\ \end{align}$ (134)
$\begin{align} &f{{l}_{PRCKS}}=\text{O}(18N{{n}^{3}}+3N{{m}^{3}})+8N{{n}^{3}}+ \\ &21Nn{{m}^{2}}+11N{{n}^{2}}m+N{{m}^{3}}+2Nn{{M}_{f}}+ \\ &16Nn{{M}_{h}}+N{{M}_{\psi }}+N{{M}_{l}}+NM{{m}^{2}}+ \\ &3N{{n}^{2}}+4Nnm+N{{m}^{2}} \\ \end{align}$ (135)
$\begin{align} &f{{l}_{ECCKF}}=\text{O}(9{{n}^{3}}+{{m}^{3}})+4{{n}^{3}}+ \\ &10n{{m}^{2}}+5{{n}^{2}}m+2{{n}^{2}}+2nm+{{m}^{2}}+ \\ &2n{{M}_{f}}+8n{{M}_{h}} \\ \end{align}$ (136)
$\begin{align} &f{{l}_{ECCKS}}=\text{O}(10{{n}^{3}}+{{m}^{3}})+7{{n}^{3}}+13n{{m}^{2}}+ \\ &11{{n}^{2}}m+3{{n}^{2}}+4nm+ \\ &{{m}^{2}}+2n{{M}_{f}}+8n{{M}_{h}} \\ \end{align}$ (137)
$\begin{align} &f{{l}_{ERCKF}}=\text{O}(2N{{n}^{3}}+2N{{m}^{3}}+{{n}^{3}})+ \\ &N{{m}^{3}}+4Nn{{M}_{h}}+6Nn{{m}^{2}}+3N{{n}^{2}}m+ \\ &Nnm+2{{n}^{3}}+2n{{M}_{f}} \\ \end{align}$ (138)
$\begin{align} &f{{l}_{ERCKS}}=\text{O}(4N{{n}^{3}}+2N{{m}^{3}})+7N{{n}^{3}}+ \\ &N{{m}^{3}}+2Nn{{M}_{f}}+4Nn{{M}_{h}}+6Nn{{m}^{2}}+ \\ &3N{{n}^{2}}m+Nnm+N{{n}^{2}} \\ \end{align}$ (139)

其中,${fl}_{PRCKF}$,${fl}_{PRCKS}$,${fl}_{ECCKF}$,${fl}_{ECCKS}$,${fl}_{ERCKF}$,${fl}_{ERCKS}$分别表示实施所提出的鲁棒CKF、提出的鲁棒CKS、现有的有色CKF、现有的有色CKS、现有的鲁棒CKF、现有的鲁棒CKS需要的浮点运算总数,$ {\rm O}(\cdot)$ 表示浮点运算的阶数,$M_{f}$,$M_{h}$,$M_{\psi}$,$M_{l}$ 分别表示计算系统函数 $\pmb{f}_{k-1}(\cdot)$、量测函数$\pmb{h}_{k}(\cdot)$、digamma 函数 $\psi(\cdot)$、对数函数$\log(\cdot)$ 需要的浮点运算数. 从式(133) ~(136) 可以看出,提出的鲁棒CKF和CKS分别与现有的有色CKF和CKS需要的浮点运算总数的关系可以表示如下:

$f{{l}_{PRCKF}}>f{{l}_{ECCKF}},f{{l}_{PRCKS}}>f{{l}_{ECCKS}}$ (140)

根据式(139) 可以知道,本文所提出的鲁棒CKF和CKS分别比现有的有色CKF和CKS具有更高的计算复杂度.

从式(133) 和式(134) 及式(137) 和式(138) 可以看出,提出的鲁棒CKF和CKS分别与现有的鲁棒CKF和CKS需要的浮点运算总数的关系可以表示如下:

$f{{l}_{PRCKF}}>f{{l}_{ERCKF}},f{{l}_{PRCKS}}>f{{l}_{ERCKS}}$ (141)
$d(f{{l}_{PRCKF}})=d(f{{l}_{ERCKF}})=N{{n}^{3}}~~\text{or}~~N{{m}^{3}}$ (142)
$d(f{{l}_{PRCKS}})=d(f{{l}_{ERCKS}})=N{{n}^{3}}~~\text{or}~~N{{m}^{3}}$ (143)

其中,${d}(\cdot)$ 表示浮点运算总数的主阶数.根据式(140) ~(142) 可以知道,本文所提出的鲁棒CKF和CKS分别比现有的鲁棒CKF和CKS具有更高的计算复杂度,但是它们的计算复杂度的量级大致相同.

4 仿真

本节通过一个目标跟踪仿真验证本文提出的带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器的有效性和优越性.目标跟踪已经被广泛地用作一个标准问题来验证非线性滤波器和平滑器的性能,它的系统方程[1, 4]可以表示如下:

$\begin{align} &{{x}_{k}}=\left[ \begin{matrix} 1&\frac{\text{sin}\Omega {{T}_{\text{0}}}}{\Omega }&0&\frac{\text{cos}\Omega {{T}_{\text{0}}}\text{-1}}{\Omega }&0 \\ 0&\text{cos}\Omega {{T}_{\text{0}}}&0&-\text{sin}\Omega {{T}_{\text{0}}}&0 \\ 0&\frac{1-\text{cos}\Omega {{T}_{\text{0}}}}{\Omega }&1&\frac{\text{sin}\Omega {{T}_{\text{0}}}}{\Omega }&0 \\ 0&\text{sin}\Omega {{T}_{\text{0}}}&0&\text{cos}\Omega {{T}_{\text{0}}}&0 \\ 0&0&0&0&1 \\ \end{matrix} \right]\times \\ &{{x}_{k-1}}+{{w}_{k-1}} \\ \end{align}$ (144)

其中,状态向量$\pmb{x}=[{\varsigma}\;{\dot{\varsigma}}\;{\eta}\;{\dot{\eta}}\;{\Omega}]^{\rm T}$; $\varsigma$$\eta$ 分别表示 ${x}$${y}$ 方向的位置,${\dot{\varsigma}}$${\dot{\eta}}$ 分别表示 ${x}$${y}$方向的速度; $\Omega$ 表示未知恒定的转弯速率;量测间隔(或系统离散时间) ${T_{0}=1rm{s}}$; 系统噪声$\pmb{w}_{k}\sim\mathrm{N}(\pmb{0},{Q})$,非奇异系统噪声协方差矩阵$Q=\text{diag}\{{{q}_{1}}M{{q}_{1}}M{{q}_{2}}{{T}_{0}}\}$,其中 $q_{1}=0.1rm{m}^{2}\rm{s}^{-3}$,$q_{2}=1.75×10^{-4}rm{s}^{-3}$,

$M=\left[ \begin{matrix} \frac{T_{0}^{3}}{3}&\frac{T_{0}^{2}}{2} \\ \frac{T_{0}^{2}}{2}&{{T}_{0}} \\ \end{matrix} \right]$ (145)

量测式如下:

${{z}_{k}}=\left[ \begin{matrix} {{r}_{k}} \\ {{\theta }_{k}} \\ \end{matrix} \right]=\left[ \begin{matrix} \sqrt{\varsigma _{k}^{2}+\eta _{k}^{2}} \\ \text{ta}{{\text{n}}^{\text{-1}}}(\frac{{{\eta }_{k}}}{{{\varsigma }_{k}}}) \\ \end{matrix} \right]+{{v}_{k}}$ (146)

其中,$r_{k}$k 时刻的幅值量测,$\theta_{k}$k时刻的方位量测,$\pmb{v}_{k}$ 为有色厚尾量测噪声,它的一阶自回归模型如式(3) 所示,其中${\Psi}_{k-1}=\mathrm{diag}\{\phi\quad\phi\}$,白色厚尾噪声$\pmb{\xi}_{k-1}$ 按照如下方式产生[27, 29]:

${{\xi }_{k-1}}\tilde{\ }\left\{ \begin{array}{*{35}{l}} \text{N}(0,{{\Sigma }_{0}}),&\text{w}.\text{p}.~~1-p \\ \text{N}(0,100{{\Sigma }_{0}}),&\text{w}.\text{p}.~~p \\ \end{array} \right.$ (147)

其中,$\mathrm{w.p.}$ 表示以一定的概率,p 表示出现野值的概率,$\Sigma_{0}$$\pmb{\xi}_{k-1}$ 的名义协方差矩阵:

${{\Sigma }_{0}}=\left[ \begin{matrix} 100{{\text{m}}^{2}}&0 \\ 0&10\text{m}\cdot \text{ra}{{\text{d}}^{2}} \\ \end{matrix} \right]$ (148)

式(146) 表示 $\pmb{\xi}_{k-1}$ 以概率 $1-p$ 从具有协方差矩阵$\Sigma_{0}$ 的高斯分布中抽取,而以概率 p 从具有协方差矩阵$100Sigma_{0}$ 的高斯分布中抽取. 按照式(146) 产生的$\pmb{\xi}_{k-1}$ 具有厚尾分布. 真实的初始状态为

$\begin{align} & {{x}_{0}}= \\ & {{\left[ \begin{matrix} 1000\rm{m} & 300{{\rm{m}}^{-1}} & 1000\rm{m} & 0{{\rm{m}}^{-1}} & -3{}^\circ {{\rm{s}}^{\rm{-1}}} \\ \end{matrix} \right]}^{\rm{T}}} \\ \end{align}$ (149)

相应的协方差矩阵为

$\begin{align} &{{P}_{0|0}}=\text{diag}\left\{ \text{100}{{\text{m}}^{\text{2}}}\text{10}{{\text{m}}^{\text{2}}}{{\text{s}}^{\text{-2}}}\text{100}{{\text{m}}^{\text{2}}} \right. \\ &\left. 10{{\text{m}}^{2}}{{\text{s}}^{\text{-2}}}\text{100m}\cdot \text{ra}{{\text{d}}^{\text{2}}}{{\text{s}}^{\text{-2}}} \right\} \\ \end{align}$ (150)

在每一次运行中,初始状态估计 ${\hat{\pmb x}}_{0|0}$ 被随机地从 ${\rm N}({\pmb{x}_{0},{P}_{0|0}})$ 中选取,并且所有滤波器和平滑器具有相同的初始条件,仿真时间为T=100s. 提出的鲁棒高斯近似滤波器和平滑器的参数设置如下:$u_{0}$=$ 4,$U_{0}=\Sigma_{0}$,$a_{0}=5$,$b_{0}=1$.根据文献[21]的建议,现有的鲁棒高斯近似滤波器和平滑器的尺度矩阵和自由度参数分别被选择为$R=\Sigma_{0}$$\nu=5$.

为了公平比较,我们做了200 次独立Monte Carlo运行.考虑到在实施目标跟踪仿真时一阶泰勒线性化存在巨大的截断误差以及无迹变换存在数值不稳定问题[4],因而本仿真中采用三阶球径容积准则来实施提出的和现有的高斯近似滤波器和平滑器,分别得到提出的鲁棒CKF、提出的鲁棒CKS、现有的有色CKF[19]、现有的有色CKS[20]、现有的鲁棒CKF[21]、现有的鲁棒CKS[21]. 为了比较这些滤波器和平滑器的性能,我们使用位置、速度和转弯速率的每个时刻方根均方误差 (Root-mean square error,RMSE)和它们的平均RMSE (Averaged RMSE,ARMSE)作为性能指标.定义第 k 时刻位置的RMSE 为

$\begin{align} &\text{RMS}{{\text{E}}_{\text{pos}}}(k)\text{=} \\ &\sqrt{\frac{\text{1}}{{{M}_{\text{c}}}}\sum\limits_{s\text{=1}}^{{{M}_{\text{c}}}}{{{\left( {{\left( \varsigma _{k}^{s}\text{-}\hat{\varsigma }_{k}^{s} \right)}^{\text{2}}}\text{+}\left( \eta _{k}^{s}\text{-}\hat{\eta }_{k}^{s} \right) \right)}^{\text{2}}}}} \\ \end{align}$ (151)

定义第 k 时刻位置的ARMSE为

$\begin{align} &ARMS{{E}_{\text{pos}}}(k)= \\ &\sqrt{\frac{1}{{{M}_{c}}T}\sum\limits_{s=1}^{{{M}_{c}}}{\sum\limits_{k=1}^{T}{{{\left( {{\left( \varsigma _{k}^{s}-\hat{\varsigma }_{k}^{s} \right)}^{2}}+\left( \eta _{k}^{s}-\hat{\eta }_{k}^{s} \right) \right)}^{2}}}}} \\ \end{align}$ (152)

其中,$(\varsigma_{k}^{s},\eta_{k}^{s})$$(\hat{\varsigma}_{k}^{s},\hat{\eta}_{k}^{s})$表示在第s次Monte Carlo运行时第k时刻真实的位置和估计的位置,$M_{c}$=$200$表示Monte Carlo仿真次数. 与位置的 RMSE 和ARMSE类似,可以写出速度和转弯速度的RMSE和ARMSE公式. 为了评估提出方法的性能,考虑如下5种仿真:

仿真 1. 比较提出方法和现有方法的单次跟踪性能. 相关参数$\phi=0.5$,野值概率 $p=0.05$,变分迭代次数 $N=5$.

图 2展示了目标的真实轨迹以及现有方法和提出方法的估计轨迹.从图 2可以看到,在以上的仿真条件下,提出的方法和现有的方法都能较好地跟踪上目标.但是提出的鲁棒CKF和鲁棒CKS分别比现有的有色CKF、鲁棒CKF和现有的有色CKS、鲁棒CKS具有更高的跟踪精度,并且提出的鲁棒CKS具有最高的跟踪精度. 尺度矩阵 R 和自由度参数 $\nu$的滤波估计如图 3所示,而它们的平滑估计分别为$\hat{R}(1,1)=94.59{{\text{m}}^{2}},\hat{R}(2,2)=9.00\text{m}\cdot \text{ra}{{\text{d}}^{2}},\hat{\nu }=1.77$. 尺度矩阵 R 和自由度参数 $\nu$的滤波估计及平滑估计之间的差异也反映到了状态滤波估计和平滑估计的精度差异上.

图 2 目标的真实轨迹和估计轨迹 Figure 2 True and estimated trajectories of the target
图 3 尺度矩阵 R 和自由度参数 $\nu$ 的滤波估计 Figure 3 The filtering estimations of scale matrix R and degree of freedom parameter $\nu$

仿真 2. 比较提出方法和现有方法在200次 Monte Carlo仿真中的平均估计精度. 相关参数 $\phi$=$0.5$,野值概率 $p=0.05$,变分迭代次数 $N=5$.

图 4~6分别展示了提出方法和现有方法的位置RMSE、速度RMSE和转弯速率RMSE.从图 4~6可以看出,提出的鲁棒CKF和鲁棒CKS分别比现有的有色CKF、鲁棒CKF和现有的有色CKS、鲁棒CKS具有更小的RMSE,并且提出的鲁棒CKS具有最小的RMSE.注意本文提出的平滑器为固定区间平滑器,越接近最后时刻性能越接近提出的滤波器的性能,而且在最后时刻$k=100mathrm{s}$ 时提出的平滑器等价于提出的滤波器,因此在最后的5 $\mathrm{s}$提出的平滑器的RMSE会出现急剧上升.

图 4 不同方法的位置RMSE Figure 4 RMSEs of the position of di®erent methods
图 5 不同方法的速度RMSE Figure 5 RMSEs of the velocity of di®erent methods
图 6 不同方法的转弯速率RMSE Figure 6 RMSEs of the turn rate of di®erent methods

仿真 3. 研究提出方法和现有方法在不同相关参数 $\phi$的情况下的估计性能. 相关参数 $\phi= 0.1,0.2$,…,$0.9$,野值概率 $p=0.05$,变分迭代次数 $N=5$.

图 7~9分别展示了提出方法和现有方法在相关参数$\phi=0.1,0.2,\cdots,0.9$时的位置ARMSE、 速度ARMSE和转弯速率ARMSE.从图 7~9可以看出,当相关参数 $\phi=0.1,0.2,\cdots,0.9$时,提出的鲁棒CKF 和鲁棒CKS分别比现有的有色CKF、 鲁棒CKF和现有的有色CKS、鲁棒CKS具有更小的ARMSE,并且提出的鲁棒CKS具有最小的ARMSE.

图 7$\phi=0.1,0.2,\cdots,0.9$时,不同方法的位置ARMSE Figure 7 ARMSEs of the position of different methods when $\phi=0.1,0.2,\cdots,0.9$
图 8$\phi=0.1,0.2,\cdots,0.9$时,不同方法的速度ARMSE Figure 8 ARMSEs of the velocity of different methods when $\phi=0.1,0.2,\cdots,0.9$
图 9$\phi=0.1,0.2,\cdots,0.9$时,不同方法的转弯速率ARMSE Figure 9 ARMSEs of the turn rate of different methods when $\phi=0.1,0.2,\cdots,0.9$

仿真 4. 研究提出方法和现有方法在不同野值概率 p的情况下的估计性能. 相关参数 $\phi=0.5$,野值概率$p=0.05,0.1,\cdots,0.3$,变分迭代次数 $N=5$.

图 10~12分别展示了提出方法和现有方法在野值概率$p=0.05,0.1,\cdots,0.3$ 时的位置ARMSE、 速度ARMSE和转弯速率ARMSE.从图 10~12可以看出,当野值概率 $p=0.05,0.1,\cdots,0.3$ 时,提出的鲁棒CKF 和鲁棒CKS分别比现有的有色CKF、鲁棒CKF和现有的有色CKS、鲁棒CKS具有更小的ARMSE,并且提出的鲁棒CKS具有最小的ARMSE.

图 10p = 0.05, 0.1,…, 0.3 时, 不同方法的位置ARMSE Figure 10 ARMSEs of the position of different methods when p = 0.05, 0.1,…, 0.3
图 11 当$p = 0.05, 0.1,…, 0.3 时,不同方法的速度ARMSE Figure 11 ARMSEs of the velocity of different methods when p = 0.05, 0.1,…, 0.3
图 12p = 0.05, 0.1,…, 0.3 时,不同方法的\\转弯速率ARMSE Figure 12 ARMSEs of the turn rate of different methods when p = 0.05, 0.1,…, 0.3

仿真 5.研究提出的鲁棒方法和现有的鲁棒方法在不同迭代次数 N的情况下的估计性能. 相关参数 $\phi=0.5$,野值概率 $p=0.05$,变分迭代次数 N $=1,2,\cdots,10$.图 13~15分别展示了提出的鲁棒方法和现有的鲁棒方法在迭代次数$N=1,2,\cdots$,$10$ 时的位置ARMSE、速度ARMSE和转弯速率ARMSE.从图 13~15可以看出,当迭代次数 $N=$1,2,\cdots,10$ 时,提出的鲁棒CKF和鲁棒CKS分别比现有的鲁棒CKF和现有的鲁棒CKS具有更小的ARMSE,并且提出的鲁棒CKS具有最小的ARMSE. 当迭代次数N=3时,提出的鲁棒CKF和鲁棒CKS的ARMSEs 基本稳定,因此在此仿真条件下提出的方法在迭代次数N=3时收敛.

图 13N = 1, 2,…,10时,不同方法的位置ARMSE Figure 13 ARMSEs of the position of different methods when N = 1, 2,…,10
图 14N = 1, 2,…,10时,不同方法的速度ARMSE Figure 14 ARMSEs of the velocity of different methods when N = 1, 2,…,10
图 15N = 1, 2,…,10时,不同方法的转弯速率ARMSE Figure 15 ARMSEs of the turn rate of different methods when N = 1, 2,…,10

仿真 6. 研究初值选取对提出方法性能的影响. 相关参数$\phi=0.5$,野值概率 $p=0.05$,变分迭代次数 $N=5$. 给出了5种初值选取,如表 1所示. 在5种初值选取中,均选择 $u_{0}=4$,$b_{0}=1$,旨在让初始的比例矩阵等于 $U_{0}$以及初始的自由度参数等于 $a_{0}$,即

${{R}_{0}}=\frac{{{U}_{0}}}{{{u}_{0}}-2-1}={{U}_{0}},{{\nu }_{0}}=\frac{{{a}_{0}}}{{{b}_{0}}}={{a}_{0}}$ (153)

从而将$U_{0}$$a_{0}$ 的选取问题转变为初始比例矩阵 $R_{0}$和初始自由度参数 $\nu_{0}$ 的选取问题.

表 1 5种初值选取 Table 1 Five choices of initial values

表 2展示了提出方法在不同初值下的位置ARMSE、速度ARMSE和转弯速率ARMSE(由于提出的CKF和CKS在不同初值下的性能相近,使得无法辨别RMSE曲线的差异,因此在仿真结果中未提供RMSE图). 从表 2中可以看出,提出方法在初值1、初值2和初值4 下的估计性能很相近,而在初值3 和初值5 下的估计性能略差于在初值1、 初值2和初值4下的估计性能. 因此,在此仿真中,初值选取对提出方法的性能有较小的影响,并且在 $U_{0}$\geq$\Sigma_{0}$$a_{0}\geq5$ 情况下初值选取对提出方法性能的影响更小.

表 2 提出的CKF和CKS在不同初值下的位置ARMSE、速度ARMSE、转弯速率ARMSE Table 2 ARMSEs of position,velocity,and turn rate from the proposed CKF and CKS under different initial values
5 结论

本文提出了带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器.通过建立未知参数和待估计状态的共轭先验分布,并利用变分贝叶斯方法更新状态、尺度矩阵、自由度参数的后验PDF,从而解决量测差分后模型中的噪声尺度矩阵和自由度参数未知问题.目标跟踪的仿真结果表明本文提出的方法适用于解决带有色厚尾量测噪声的非线性状态估计问题,并且所提出的方法比现有的方法具有更高的估计精度和更好的鲁棒性.

参考文献
1 Zhang Yong-Gang, Huang Yu-Long, Zhao Lin. A general framework solution to Gaussian filter with multiple-step randomly-delayed measurements. Acta Automatica Sinica, 2015, 41 (1): 122–135.
( 张勇刚, 黄玉龙, 赵琳. 一种带多步随机延迟量测高斯滤波器的一般框架解. 自动化学报, 2015, 41 (1): 122–135. )
2 Zhang Yong-Gang, Huang Yu-Long, Li Ning, Zhao Lin. Conditional posterior Cramér-Rao lower bound for nonlinear sequential Bayesian estimation with one-step randomly delayed measurements. Acta Automatica Sinica, 2015, 41 (3): 559–574.
( 张勇刚, 黄玉龙, 李宁, 赵琳. 带一步随机延迟量测非线性序列贝叶斯估计的条件后验克拉美罗下界. 自动化学报, 2015, 41 (3): 559–574. )
3 Lei M, van Wyk B J, Qi Y. Online estimation of the approximate posterior Cramer-Rao lower bound for discrete-time nonlinear filtering. IEEE Transactions on Aerospace and Electronic Systems, 2011, 47 (1): 37–57. DOI:10.1109/TAES.2011.5705658
4 Arasaratnam I, Haykin S. Cubature Kalman filters. IEEE Transactions on Automatic Control, 2009, 54 (6): 1254–1269. DOI:10.1109/TAC.2009.2019800
5 Arasaratnam I, Haykin S. Cubature Kalman smoothers. Automatica, 2011, 47 (10): 2245–2250. DOI:10.1016/j.automatica.2011.08.005
6 Sarmavuori J, Särkkä S. Fourier-Hermite Kalman filter. IEEE Transactions on Automatic Control, 2012, 57 (6): 1511–1515. DOI:10.1109/TAC.2011.2174667
7 Sarmavuori J, Särkkä S. Fourier-Hermite Rauch-Tung-Striebel smoother. In:Proceedings of the 20th European Signal Processing Conference. Bucharest, Romania:IEEE, 2012 : 2109–2013.
8 Jia B, Xin M, Cheng Y. High-degree cubature Kalman filter. Automatica, 2013, 49 (2): 510–518. DOI:10.1016/j.automatica.2012.11.014
9 Jia B, Xin M. A new class of nonlinear Rauch-Tung-Striebel cubature Kalman smoothers. ISA Transactions, 2015, 55 (3): 72–80.
10 Zhang Yong-Gang, Huang Yu-Long, Wu Zhe-Min, Li Ning. A high order unscented Kalman filtering method. Acta Automatica Sinica, 2014, 40 (5): 838–848.
( 张勇刚, 黄玉龙, 武哲民, 李宁. 一种高阶无迹卡尔曼滤波方法. 自动化学报, 2014, 40 (5): 838–848. )
11 Jia B, Xin M, Cheng Y. Sparse-grid quadrature nonlinear filtering. Automatica, 2012, 48 (2): 327–341. DOI:10.1016/j.automatica.2011.08.057
12 Duník J, Straka O, Šimandl M. Stochastic integration filter. IEEE Transactions on Automatic Control, 2013, 58 (6): 1561–1566. DOI:10.1109/TAC.2013.2258494
13 Zhang X C. A novel cubature Kalman filter for nonlinear state estimation. In:Proceedings of the 52nd IEEE Conference on Decision and Control. Florence, Italy:IEEE, 2013 : 7797–7802.
14 Zhang Y G, Huang Y L, Li N, Zhao L. Embedded cubature Kalman filter with adaptive setting of free parameter. Signal Processing, 2015, 114 : 112–116. DOI:10.1016/j.sigpro.2015.02.022
15 Wang S Y, Feng J C, Tse C K. Spherical simplex-radial cubature Kalman filter. IEEE Signal Processing Letters, 2014, 21 (1): 43–46. DOI:10.1109/LSP.2013.2290381
16 Chang L B, Hu B Q, Li A, Qin F J. Transformed unscented Kalman filter. IEEE Transactions on Automatic Control, 2013, 58 (1): 252–257. DOI:10.1109/TAC.2012.2204830
17 Zhang Y G, Huang Y L, Li N, Zhao L. Interpolatory cubature Kalman filters. IET Control Theory and Applications, 2015, 9 (11): 1731–1739. DOI:10.1049/iet-cta.2014.0873
18 Wang Xiao-Xu, Liang Yan, Pan Quan, Zhao Chun-Hui, Li Han-Zhou. Unscented Kalman filter for nonlinear systems with colored measurement noise. Acta Automatica Sinica, 2012, 38 (6): 986–998.
( 王小旭, 梁彦, 潘泉, 赵春晖, 李汉舟. 带有色量测噪声的非线性系统Unscented卡尔曼滤波器. 自动化学报, 2012, 38 (6): 986–998. DOI:10.3724/SP.J.1004.2012.00986 )
19 Wang X X, Pan Q. Nonlinear Gaussian filter with the colored measurement noise. In:Proceedings of the 17th International Conference on Information Fusion. Salamanca, Spain:IEEE, 2014 : 1–7.
20 Wang X X, Liang Y, Pan Q, Zhao C H, Yang F. Nonlinear Gaussian smoothers with colored measurement noise. IEEE Transactions on Automatic Control, 2015, 60 (3): 870–876. DOI:10.1109/TAC.2014.2337991
21 Piché R, Särkkä S, Hartikainen J. Recursive outlier-robust filtering and smoothing for nonlinear systems using the multivariate student-t distribution. In:Proceedings of the 2012 IEEE International Workshop on Machine Learning for Signal Processing. Santander, Spain:IEEE, 2012 : 1–6.
22 Agamennoni G, Nebot E M. Robust estimation in non-linear state-space models with state-dependent noise. IEEE Transactions on Signal Processing, 2014, 62 (8): 2165–2175. DOI:10.1109/TSP.2014.2305636
23 Simon D. Optimal State Estimation. New Jersey:John Wiley and Sons, Inc., 2006.
24 Huang Yu-Long, Zhang Yong-Gang, Li Ning, Zhao Lin. An identification method for nonlinear systems with colored measurement noise. Acta Automatica Sinica, 2015, 41 (11): 1877–1892.
( 黄玉龙, 张勇刚, 李宁, 赵琳. 一种带有色量测噪声的非线性系统辨识方法. 自动化学报, 2015, 41 (11): 1877–1892. )
25 Agamennoni G, Nieto J I, Nebot E M. An outlier-robust Kalman filter. In:Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA). Shanghai, China:IEEE, 2011 : 1551–1558.
26 Agamennoni G, Nieto J I, Nebot E M. Approximate inference in state-space models with heavy-tailed noise. IEEE Transactions on Signal Processing, 2012, 60 (10): 5024–5037. DOI:10.1109/TSP.2012.2208106
27 Zhu H, Leung H, He Z S. A variational Bayesian approach to robust sensor fusion based on student-t distribution. Information Sciences, 2013, 221 : 201–214. DOI:10.1016/j.ins.2012.09.017
28 Nurminen H, Ardeshiri T, Piché R, Gustafsson F. Robust inference for state-space models with skewed measurement noise. IEEE Signal Processing Letters, 2015, 22 (11): 1898–1902. DOI:10.1109/LSP.2015.2437456
29 Roth M, Özkan E, Gustafsson F. A student's t filter for heavy tailed process and measurement noise. In:Proceedings of the 2013 IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP). Vancouver, BC, Canada:IEEE, 2013 : 5770–5774.
30 Xu D J, Shen C, Shen F. A robust particle filtering algorithm with non-Gaussian measurement noise using student-t distribution. IEEE Signal Processing Letters, 2014, 21 (1): 30–34. DOI:10.1109/LSP.2013.2289975
31 Huang Yu-Long, Zhang Yong-Gang, Li Ning, Zhao Lin. An improved Gaussian approximate filtering method. Acta Automatica Sinica, 2016, 42 (3): 385–401.
( 黄玉龙, 张勇刚, 李宁, 赵琳. 一种改进的高斯近似滤波方法. 自动化学报, 2016, 42 (3): 385–401. )
32 Tzikas D G, Likas A C, Galatsanos N P. The variational approximation for Bayesian inference. IEEE Signal Processing Magazine, 2008, 25 (6): 131–146. DOI:10.1109/MSP.2008.929620
33 Golub G H, Van Loan C F. Matrix Computations (4th edition). Baltimore, Maryland: The Johns Hopkins University Press, 2013.