-
摘要: 考虑到运动目标跟踪系统机动、隐身等人为对抗特征以及非视距、干扰、遮挡等环境因素, 其系统建模、估计与辨识过程中越来越无法回避非线性、非高斯以及参数未知等复杂系统特征的影响. 针对过程噪声先验信息不准确以及量测噪声非高斯环境下运动目标的非线性状态估计问题, 提出一种基于自然梯度的噪声自适应变分贝叶斯(Variational Bayes, VB)滤波算法. 首先, 利用指数族分布具有统一表达形式的优势, 构建参数化逆威沙特(Inverse-Wishart, IW)分布作为状态一步预测误差协方差的共轭先验分布, 同时选取学生t分布重构因量测随机缺失导致的具有非高斯特点的似然函数; 其次, 在变分贝叶斯优化框架下采用平均场理论将状态变量联合后验分布近似分解为独立的变分分布, 在此基础上, 结合坐标上升方法更新各变量的变分分布参数; 进而, 结合 Fisher 信息矩阵推导置信下界最大化关于状态估计及其估计误差协方差的自然梯度, 使非线性状态后验分布的近似分布沿梯度下降, 以实现对状态后验概率密度函数(Probability density function, PDF)的“紧密”逼近. 理论分析和仿真实验表明: 相对传统的非线性滤波方法, 本文算法对噪声不确定问题具有较好的自适应能力, 并且能够获得较高的状态估计精度.
-
关键词:
- 非线性滤波 /
- 自适应滤波 /
- 变分贝叶斯推断 /
- 自然梯度 /
- Fisher 信息矩阵
Abstract: Considering the increasing complexity and changeability of characteristics such as maneuvering and stealth in moving target tracking system and the influence of adverse factors such as non-line-of-sight, interference and occlusion in measurement environment. State estimation is likely to be confronted with complex system characteristics such as nonlinearity, non-Gaussian noise and unknown parameters. Aiming at nonlinear adaptive state estimation of moving target in a system with unknown process noise and non-Gaussian measurement noise, a novel noise adaptive variational Bayesian (VB) filter using natural gradient is proposed. Firstly, a parameterized inverse-Wishart (IW) distribution and a student＇s t distribution are constructed as the conjugate prior distribution of predicted state error covariance and measurement likelihood respectively. Then, in the framework of variational Bayesian optimization, the joint a posteriori distribution of estimation variables is approximately decomposed into independent variational distributions by using mean-field theory. On this basis, the variational distribution parameters of each variable are updated by combining coordinate ascend method and the characteristics of exponential distributions. Furthermore, under the condition of maximizing evidence lower bound, the natural gradients with respect to state estimation and its error covariance are derived by combining with Fisher information matrix. So that the variational distribution of nonlinear state gradually approaches the posteriori probability density function (PDF) of state along the natural gradient direction. Finally, simulation results show that the proposed algorithm has better adaptive ability to the problem of noise uncertainty and can obtain higher estimation accuracy compared to traditional algorithms. -
非线性滤波器设计与实现因其普适性及重要性一直是国内外学者研究的热点问题, 近年来非线性状态估计理论已成功应用于陆、海、空、天中运动目标的预警与防御, 智能交通的精确导航与制导, 无人机定位与遥感监测、工业过程监控与故障诊断等众多领域[1-3]. 考虑到运动目标跟踪系统机动、隐身等复杂多变的人为对抗特征以及非视距、干扰、遮挡等环境因素无法避免, 其系统建模、估计与辨识过程中越来越无法回避非线性、非高斯以及参数未知等复杂系统特征的影响[4]. 在对复杂环境下运动目标系统噪声先验信息进行建模时, 建模误差存在于状态演化模型中, 并通常假设其满足一定的参数化统计特性[5]. 然而, 在实际工程中, 由于先验建模信息的不足导致难以对此类参数进行准确赋值. 例如, 在现代目标跟踪系统中出现的欺骗、干扰、杂波、未知分布特性的量测噪声和系统噪声等情形, 尤其在非合作运动目标强机动场景中, 因难以对其运动过程进行精细化建模, 常造成目标跟踪航迹间断甚至无法正常起始的现象.
针对系统噪声未知问题, 其解决思路通常选取逆伽马分布和逆威沙特(Inverse-Wishart, IW) 分布等具有统一参数表达形式的指数族分布作为共轭先验分布. Särkkä 等[6] 在变分贝叶斯 (Variational Bayes, VB) 推断框架下利用指数族分布构建共轭先验分布近似未知量测噪声的后验概率密度函数 (Probability density function, PDF), 进而结合贝叶斯滤波机理实现时变噪声方差和目标状态的联合估计, 其滤波效果达到与交互式多模型方法相接近的估计精度, 并且凭借 VB 便于结合平均场近似解耦理论将高维变量求解转化为多个低维变量计算的特点, 使其在解决多未知扰动问题方面更具有优势. 随后, 文献[7] 采用IW分布近似多变量噪声后验 PDF的思想, 给出变分推断框架下噪声方差估计的一般实现形式. 在此基础上, 变分贝叶斯方法在未知量测/系统噪声估计方面得到了进一步的发展[8−9], 并成功推广应用于多目标跟踪环境[10−11]. 在文献[12−13] 中滤波器的构建均建立在量测噪声和系统噪声统计特性未知的假设条件下, 其中, 文献[12] 在状态和量测扩维基础上通过采用批处理方式实现对未知噪声矩阵的估计, 然而这种扩维方式不可避免造成计算复杂度的急剧增加; 文献[13] 给出两类噪声统计特性均未知条件下噪声方差的在线估计方法, 并且为避免系统噪声和状态相互独立假设的不合理性, 采用独立于当前状态的前一时刻状态预测误差协方差代替系统噪声的统计特性, 进而利用平均场理论近似解耦计算边缘后验 PDF 的变分分布, 以迭代递推方式求解其估计变量期望的解析解, 同时, 将状态一步预测误差协方差的先验分布建模为参数化IW分布, 以实现状态后验概率分布的更新.
考虑到外界环境干扰以及非线性传播等因素的影响, 量测数据概率分布往往呈现出重尾和非对称等非高斯特征. 例如, 在基于电磁信号的距离估计中, 障碍物遮挡造成的非视距量测误差往往远大于其他来源服从对称分布的误差, 导致量测分布呈现非对称非高斯现象. 其次, 受传感器精度和灵敏度的限制, 当运动目标发生强机动时, 雷达极化反射不稳定性将导致的量测随机缺失现象, 也将造成量测产生非高斯特征. 针对量测噪声非高斯问题的处理, 文献[14]采用莱斯分布构建非共轭指数族变换点检测模型, 并将其成功应用于雷达目标跟踪系统中. 文献[15]则提出一种通过构建状态演化模型和量测模型的条件矩实现非线性非高斯系统的状态估计方法. 为了进一步提升滤波器对不同分布形状噪声的鲁棒性, 文献[16] 将 Skew-$ t $分布作为非高斯量测似然的近似分布形式, 在此基础上设计了一种鲁棒变分贝叶斯估计器. 文献[17] 考虑在$ \alpha $散度最小化准则下, 利用变分分布实现对后验 PDF 的近似. $ \alpha $散度对异常数据具有较好的抑制作用, 但是也打破了对数边缘概率与变分置信下界 (Evidence lower bound, ELBO)、KL 散度(Kullback-Leibler divergence) 之和的等式约束关系, 其实质是一种伪 VB 推断方法. 考虑到学生$ t $分布和 Skew-$ t $分布对因量测异常导致的量测重尾现象和非对称性具有较好鲁棒性[18-19], 文献[20] 针对系统噪声和量测噪声均具有重尾特性的线性系统采用学生$ t $分布对状态预测概率密度函数和量测似然函数进行建模, 进而提出了一种基于高斯−学生$ t $混合分布的线性滤波器, 与传统高斯假设条件下滤波器估计效果相比进一步提升了系统的状态估计的精度和鲁棒性. 在系统噪声未知且时变和量测噪声重尾条件下, 文献[21] 构建参数化IW分布作为状态一步预测误差协方差的共轭先验分布, 同时, 选取参数化学生$ t $分布刻画具有厚尾特点的量测似然函数. 然而, 上述噪声自适应方法未考虑非线性滤波器自身的优化问题和如何衡量后验分布近似程度的问题.
考虑到在非线性滤波器设计过程中, 参数化变分分布对系统状态后验近似的程度是提高估计精度的关键因素之一, 基于采样的随机优化和基于拟牛顿、高斯牛顿和梯度上升等确定性优化方法的非线性滤波器相继提出. 在随机性优化方面, MCMC (Markov chain Monte Carlo)[22−23] 和序贯蒙特卡罗 (Sequential Monte Carlo, SMC) 利用随机样本来逼近后验概率, 并以样本分布近似未知变量后验分布. 随机优化方法的优点是在大量样本的条件下能够保证较高精度的估计, 但要承担较大的计算负担[24]. 在确定性优化方面, 文献[25] 以最小二乘准则为目标函数, 采用牛顿法推导给出一种迭代卡尔曼滤波器. 文献[26] 综合梯度法和牛顿法提出一种无噪声条件下的滚动时域估计器, 并证明其渐进收敛性和稳定性. 文献[27] 针对非线性状态空间的状态估计及其估计误差协方差的迭代更新问题, 利用正则化的非线性最小二乘思想提出一种随机增量近端梯度算法. VB 方法通过求解参数化的目标函数, 利用参数优化结果逼近后验分布, 是一种确定性近似方法. 因此, VB 具有确定性优化方法特有的计算量较小的优势, 同时由于 VB 便于结合平均场理论近似解耦联合概率分布, 进一步减小了计算代价.
非线性状态估计优化的实质是对多维状态后验 PDF 的近似逼近, 并且其近似程度不能简单地采用欧氏距离进行度量. 因此, 选取合理度量准则将有利于提高后验 PDF 的近似程度, KL 散度作为分布之间“差异”的度量在后验分布近似性衡量中具有天然优势. 文献[17]给出状态估计的变分迭代优化实现形式, 获得$ \alpha $散度和 KL 散度下对后验 PDF 更紧密的近似. 同时, 从信息几何角度出发, 概率分布是统计流形上点, 在一定条件下两概率分布之间的 KL 散度与作为统计流形度规的 Fisher 信息满足一定的数学关系. 基于信息几何理论, Amari[28]利用自然梯度优化方式实现统计流形空间中目标函数的最速梯度下降(或上升). 文献[29] 结合自然梯度策略和卡尔曼滤波框架设计一种非线性状态估计方法, 在文献[30] 中, 该方法进一步推广应用于传感器网络目标跟踪系统. 文献[31] 中作者证明了针对非线性状态估计优化的自然梯度方法在克拉美罗下界意义下是渐进最优的. 考虑自然梯度优化的优势, 以变分置信下界最大化条件下状态估计及其估计误差协方差的自然梯度为切入点, 从信息几何角度实现对状态后验概率密度函数的“紧密”逼近, 进而提高状态估计精度.
在过程噪声先验不准确和由于量测随机丢失导致的量测噪声分布非高斯的情况下, 针对非线性系统状态估计精度和鲁棒性提升问题, 本文提出一种基于自然梯度的噪声自适应变分贝叶斯滤波算法. 本文结构如下: 第 1 节结合IW分布和学生$ t $分布分别实现对状态预测误差协方差和量测似然的参数化建模; 第 2 节结合平均场近似和坐标上升方法给出了变分贝叶斯框架下未知变量变分分布的迭代更新方式; 第 3 节推导给出 ELBO 关于系统状态估计及其误差协方差的自然梯度, 进而构建并设计一种基于自然梯度的噪声自适应非线性变分贝叶斯滤波器; 第 4 节给出仿真验证与分析; 第 5 节总结全文, 并展望了后续研究方向.
1. 预备知识
假设动态系统隐变量$ {\boldsymbol{\Xi}}_{k} $的量测为$ {\boldsymbol{z}}_k $, 根据变分贝叶斯理论可知, 在变分贝叶斯框架下以变分分布 $ q\left({\boldsymbol{\Xi}}_{k}|{\boldsymbol{\psi}}_k\right) $作为桥梁可将难以积分的问题转化为 ELBO 的优化问题.
$$ \begin{split} \log p&\left({\boldsymbol{z}}_k\right) = \int q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right) \log \frac{p\left({\boldsymbol{\Xi}}_k, {\boldsymbol{z}}_k\right)} {q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right)}{\rm{d}}{\boldsymbol{\Xi}}_k \;- \\ &\int q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right) \log \frac{p\left({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k\right)} {q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right)}{\rm{d}}{\boldsymbol{\Xi}}_k = \\ & {{\cal{L}}}\left({\boldsymbol{\psi}}_k\right) + {\cal{D}}_{\rm {KL}}\left[q({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k)\|p({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k)\right]\geq \\ &{{\cal{L}}}\left({\boldsymbol{\psi}}_k\right)\\[-10pt] \end{split} $$ (1) 其中, $ {{\cal{L}}}\left({\boldsymbol{\psi}}_k\right) $表示具有单调递增特性的变分 ELBO, ${\cal{D}}_{\rm {KL}}\left[q({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k)\|p({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k)\right]$表示变分分布$ q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right) $与后验分布 $ p({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k) $之间的 KL 散度, 其表达式分别为
$$ {{\cal{L}}}\left({\boldsymbol{\psi}}_{k}\right) = \int q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right) \log \frac{p\left({\boldsymbol{\Xi}}_k, {\boldsymbol{z}}_k\right)} {q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right)}{\rm{d}}{\boldsymbol{\Xi}}_k $$ (2) $$ \begin{split} & {\cal{D}}_{\rm {KL}} \left[q({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k)\|p({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k)\right] = \\ &\qquad-\int q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right) \log \frac{p\left({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k\right)} {q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right)}{\rm{d}}{\boldsymbol{\Xi}}_k\quad \end{split} $$ (3) 其中, $ q\left({\boldsymbol{\Xi}}_k|{\boldsymbol{\psi}}_k\right) $表示以$ {\boldsymbol{\psi}}_k $为参数的变分分布.
未知变量的估计实际是对状态后验分布 $ p({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k) $的近似逼近, 当非线性状态后验分布难以获取时, 变分贝叶斯方法能够通过构建简单的变分分布实现对$ p({\boldsymbol{\Xi}}_k|{\boldsymbol{z}}_k) $的近似逼近. 变分分布选取需考虑先验分布和后验分布具有相同的函数形式, 一般情况下具有共轭特性的指数族分布满足这一条件. 本文考虑系统噪声未知情况, 根据标准卡尔曼滤波实现结构可知, 系统噪声的统计特性仅影响状态预测误差协方差, 因此可直接对状态预测误差协方差进行先验建模. 并且当系统噪声假设服从均值已知但方差未知的多变量高斯分布假设时, 可选取IW分布作为其分布方差矩阵的共轭先验分布; 此外, 针对量测缺失导致量测噪声出现重尾问题, 考虑选取能够有效表征这一现象的学生$ t $分布对量测似然函数进行建模.
1.1 IW分布共轭先验模型
在贝叶斯滤波框架下, 对于系统噪声高斯分布参数未知问题的处理, 可转化为状态预测误差协方差的估计问题. 这里选取IW分布作为共轭先验分布, 以保证后验分布与先验分布具有相同的函数形式. 一个服从IW分布的$ n\times n $维随机对称正定矩阵$ {\boldsymbol{A}} $的概率密度函数可以表示为
$$ \begin{split} &{\cal{IW}}({\boldsymbol{A}}; t, {{\boldsymbol{T}}}) = \frac{\mid{{\boldsymbol{T}}}\mid^{\frac{t}{2}}\mid {\boldsymbol{A}}\mid^{\frac{-(t+n+1)}{2}}} {2^{\frac{nt}{2}}{\boldsymbol{\Gamma}}_{n}(\frac{t}{2})}\;\times\\ &\qquad{\rm exp}(-0.5{\rm tr}({\boldsymbol{T}}{\boldsymbol{A}}^{-1})) \end{split} $$ (4) 其中, $ t $和$ {{\boldsymbol{T}}} $分别表示IW分布的自由度参数和$ n\times n $维逆尺度矩阵, $ {\rm tr}(\cdot) $表示矩阵的迹, $ {\boldsymbol{\Gamma}}_{n}(\cdot) $表示$ n $维的伽马函数. 进而状态预测误差协方差$ {\boldsymbol{P}}_{k|k-1} $的先验分布表示为
$$ p({\boldsymbol{P}}_{k|k-1}|{\boldsymbol{z}}_{k-1}) = {\cal{IW}}({\boldsymbol{P}}_{k|k-1}; {t}_{k}, {{\boldsymbol{{T}}}}_{k}) $$ (5) 对于逆威沙特分布, 当$ t_{k}\geq n+1 $时, 其变量期望即状态预测误差协方差的均值 ${\rm{E}}[{\boldsymbol{P}}_{k|k-1}]$表示为
$$ {\rm{E}}\left[{{{\boldsymbol{P}}}}_{k|k-1}\right] = \frac{{\boldsymbol{{T}}}_{k}}{{t}_{k}-n-1} $$ (6) 令先验分布参数$ t_{k} $表示为
$$ {t}_{k} = \tau + n+1 $$ (7) 式 (6) 进一步转化为
$$ {\boldsymbol{{T}}}_{k} = \tau {\rm{E}}\left[{{{\boldsymbol{P}}}}_{k|k-1}\right] $$ (8) 其中, $ \tau $表示调谐参数[7]. 根据式 (5) ~ (8) 计算得到状态预测协方差的先验分布及其分布参数.
1.2 学生$ t $分布量测似然模型
当量测噪声分布由于量测值随机异常或缺失呈现重尾特征时, 传统高斯噪声分布模型难以对噪声分布特性进行有效刻画. 考虑学生$ t $分布能够更好地体现这一特征, 并且对异常值具有较好的鲁棒性. 当量测噪声$ {\boldsymbol{\upsilon}}_k $满足均值为$ 0 $、方差为$ {{\boldsymbol{R}}_k} $的分布参数时, 建立参数化学生$ t $分布模型, 即
$$ p({\boldsymbol\upsilon}_k) = {{\cal{S}}t}({\boldsymbol{\upsilon}}_k; 0, {{\boldsymbol{R}}_k}, v) $$ (9) 其中, $ v $表示学生$ t $分布的自由度参数, 噪声方差$ {{\boldsymbol{R}}_k} $是该分布的尺度矩阵. 在此条件下量测似然函数可表示为
$$ p({\boldsymbol{z}}_k|{\boldsymbol{x}}_k) = {{\cal{S}}t}({\boldsymbol{z}}_k; {\boldsymbol{h}}_k({\boldsymbol{x}}_k), {{\boldsymbol{R}}_k}, v) $$ (10) 通常学生$ t $分布的概率密度函数难以求得封闭解, 为了解决这个问题, 引入服从伽马分布的辅助随机变量$ \lambda_k $, 从而进一步将学生$ t $分布转化为服从参数化高斯分布的伽马分布的积分, 式 (10) 中量测似然函数可转化为
$$ \begin{split} p({\boldsymbol{z}}_k|{\boldsymbol{x}}_k) =\;& \int\Bigg( {\rm{N}}\left({\boldsymbol{z}}_k; {\boldsymbol{h}}_k({\boldsymbol{x}}_k), \frac{{{\boldsymbol{R}}_k}}{\lambda_k}\right)\times \\ &{\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right)\Bigg){\rm{d}}\lambda_k \end{split} $$ (11) 其中, $ {\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right) $表示辅助变量$ \lambda_k $服从伽马分布, $ \alpha_k $和$ \beta_k $分别为形状参数和逆尺度参数[20]. 综合上述特点, 在传感器量测存在随机异常值情况下量测似然函数表示为如下结构化形式:
$$ p({\boldsymbol{z}}_k|{\boldsymbol{x}}_k, \lambda_k) = {\rm{N}}\left({\boldsymbol{z}}_k; {\boldsymbol{h}}_k({\boldsymbol{x}}_k), \frac{{{\boldsymbol{R}}_k}}{\lambda_k}\right) $$ (12) $$ p(\lambda_k) = {\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right) $$ (13) 并且$ {\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right) $及其均值${\rm{E}}\left[\lambda_k\right]$分别表示为
$$ {\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right) = \frac{\beta_k^{\alpha_k}}{\Gamma_k(\alpha_k)} \lambda_k^{\alpha_k-1}{\rm{e}}^{-\beta_k\lambda_k} $$ (14) $$ { \rm{E}}\left[\lambda_k\right] = \frac{\alpha_k}{\beta_k} $$ (15) 最后, 通过对辅助变量$ \lambda_k $及其超参数$ \alpha_k $和$ \beta_k $更新确定量测似然的表达式.
1.3 平均场近似
根据以上分析建模, 式 (2) 中的系统隐变量$ {\boldsymbol{\Xi}}_{k} $定义为${\boldsymbol{\Xi}}_{k}=({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k)$. 考虑到联合后验分布形式的复杂性以及参数相互耦合的问题, 无法直接求得其解析解. 在假设各未知变量相互独立的前提下, 通过引入平均场近似策略实现联合变分分布的近似解耦, 即
$$ q({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k) \approx q({{{\boldsymbol{x}}}}_k)q({{{\boldsymbol{P}}}}_{k|k-1})q(\lambda_k) $$ (16) 其中, $ q({{{\boldsymbol{x}}}}_k) $, $ q({{{\boldsymbol{P}}}}_{k|k-1}) $和$ q(\lambda_k) $分别表示状态 $ {{{\boldsymbol{x}}}}_k $, 状态一步预测协方差$ {{{\boldsymbol{P}}}}_{k|k-1} $和辅助变量$ \lambda_k $的变分分布, 通过平均场理论对待估计变量联合分布进行分解, 有助于在变分贝叶斯迭代框架结合坐标上升和各类优化方法实现多变量的联合优化. 结合上述第1.1节和第1.2节中的共轭先验模型和量测似然模型, 相应的变分分布分别表示为
$$ q(\lambda_k) = {\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right) $$ (17) $$ q({\boldsymbol{P}}_{k|k-1}) ={\cal{IW}}({\boldsymbol{P}}_{k|k-1}; {t}_{k|k-1}, {{{\boldsymbol{T}}}}_{k|k-1}) $$ (18) $$ q({{{\boldsymbol{x}}}}_k) = {\rm{N}}({{{\boldsymbol{x}}}}_{k}; {{{\boldsymbol{x}}}}_{k|k}, {{{\boldsymbol{P}}}}_{k|k}) $$ (19) 其中, $ \alpha_k $和$ \beta_k $为辅助随机变量$ \lambda_k $的超参数, 同理, $ {t}_{k|k-1} $和$ {{{\boldsymbol{T}}}}_{k|k-1} $为随机未知变量$ {\boldsymbol{P}}_{k|k-1} $的超参数; $ {{{\boldsymbol{x}}}}_{k|k} $和$ {{{\boldsymbol{P}}}}_{k|k} $为隐变量$ {{{\boldsymbol{x}}}}_k $的超参数. 在变分贝叶斯优化过程中, 可结合平均场理论将联合后验的变分分布近似解耦为多个单变量变分分布, 并利用坐标上升方法分别对每个变量进行迭代更新.
2. 未知变量的变分分布更新
根据上述构建的共轭先验模型和量测似然模型, 变分置信下界中隐变量$ {\boldsymbol{\Xi}}_{k} $包括状态$ {{{\boldsymbol{x}}}}_k $, 状态一步预测协方差$ {{{\boldsymbol{P}}}}_{k|k-1} $和辅助变量$ \lambda_k $, $ {\boldsymbol{\Xi}}_{k} $可定义为$ {\boldsymbol{\Xi}}_{k}=({{\boldsymbol{x}}_k}, {{\boldsymbol{P}}_{k|k-1}}, {\lambda_k}) $. 采用变分分布近似上述多未知变量的联合后验分布, 即
$$ p\left({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k|{{{\boldsymbol{z}}}}_k\right) \approx q({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k) $$ (20) 进而, 相应变分置信下界转化为
$$ \begin{split} {{\cal{L}}}\left({\boldsymbol{\psi}}_{k}\right)= \;&\iiint\Bigg( q({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k) \;\times \\ &\log \frac{p\left({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k, {\boldsymbol{z}}_k\right)} {q({{{\boldsymbol{x}}}}_k, {{{\boldsymbol{P}}}}_{k|k-1}, \lambda_k)} \Bigg){\rm{d}}{\boldsymbol{x}}_k {\rm{d}}{\boldsymbol{P}}_{k|k-1} {\rm{d}}{\lambda}_k \end{split} $$ (21) 在变分置信下界最大化约束下, 结合坐标上升方法, 变量$ {{{\boldsymbol{x}}}}_k $, $ {\boldsymbol{P}}_{k|k-1} $和$ \lambda_k $概率分布的对数形式的更新分别表示为
$$ \begin{split} \log q(\lambda_k) = \;& {\rm{E}}_{{\boldsymbol{x}}_{k}, {{\boldsymbol{P}}_{k|k-1}}} \left[\log p({\boldsymbol{x}}_k, {\boldsymbol{P}}_{k|k-1}, \lambda_k, {\boldsymbol{z}}_k)\right] + \\ &c_{{\boldsymbol{x}}_{k}, {{\boldsymbol{P}}_{k|k-1}}} \\[-10pt] \end{split} $$ (22) $$ \begin{split} \log q({\boldsymbol{P}}_{k|k-1}) =\;& {\rm{E}}_{{\boldsymbol{x}}_k, \lambda_k}\left[\log p({\boldsymbol{x}}_k, {\boldsymbol{P}}_{k|k-1}, \lambda_k, {\boldsymbol{z}}_k)\right] + \\ &c_{{\boldsymbol{x}}_{k}, \lambda_k} \\[-10pt] \end{split} $$ (23) $$ \begin{split} \log q({{\boldsymbol{x}}_{k}}) = \;& {\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k} \left[\log p({\boldsymbol{x}}_k, {\boldsymbol{P}}_{k|k-1}, \lambda_k, {\boldsymbol{z}}_k)\right] + \\&c_{{\boldsymbol{P}}_{k|k-1}, \lambda_k} \\[-10pt] \end{split} $$ (24) 其中, $ c_{{\boldsymbol{P}}_{k|k-1}, \lambda_k} $, $ c_{{\boldsymbol{x}}_{k}, \lambda_k} $和$ c_{{\boldsymbol{x}}_{k}, {{\boldsymbol{P}}_{k|k-1}}} $分别表示在迭代过程中产生的实常数.
结合建模信息和各变量变分分布的特点, 联合后验分布及其对数形式分别表示为
$$ \begin{split} & p({\boldsymbol{x}}_k, {\boldsymbol{P}}_{k|k-1}, \lambda_k|{\boldsymbol{z}}_k) = p({\boldsymbol{z}}_{k}|{\boldsymbol{x}}_{k}, \lambda_k)\;\times\\ &\quad p({\boldsymbol{x}}_k|{\boldsymbol{P}}_{k|k-1}, {\boldsymbol{z}}_{k-1}) p({\boldsymbol{P}}_{k|k-1})p(\lambda_k) \approx \\ &\quad {\rm{N}}\left({\boldsymbol{z}}_{k}; {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k}), \frac{{\boldsymbol{R}}_{k}}{\lambda_k}\right) {\rm{N}}\left({\boldsymbol{x}}_{k}; {\boldsymbol{x}}_{k|k-1}, {\boldsymbol{P}}_{k|k-1}\right) \times\\ &\quad {\cal{IW}}\left({\boldsymbol{P}}_{k|k-1}; {t}_{k|k-1}, {{{\boldsymbol{T}}}}_{k|k-1}\right) {\cal{G}}\left(\lambda_k; \alpha_k, \beta_k\right) \end{split} $$ (25) $$ \begin{split} &\log p({\boldsymbol{x}}_k, {\boldsymbol{P}}_{k|k-1}, \lambda_k|{\boldsymbol{z}}_k) =\\ &\;\;\;-\frac{1}{2}\Big\{ \lambda_k({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k}))^{\rm T}{\boldsymbol{R}}_{k}^{-1}({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k})) \;+\\ &\;\;\;({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1})^{\rm T}{\boldsymbol{P}}_{k|k-1}^{-1}(\cdot)\; + \\ &\;\;\;({t}_{k|k-1}+n+2)\log|{\boldsymbol{P}}_{k|k-1}| + {\rm tr}({{{\boldsymbol{T}}}}_{k|k-1}{\boldsymbol{P}}_{k|k-1}^{-1}) \;+\\ &\;\;\;2\beta_k\lambda_k + (2-2\alpha_k-D_z)\log\lambda_k \Big\} + c_{\Xi_k} \\[-10pt]\end{split} $$ (26) 其中, $ c_{\Xi_k} $表示迭代过程中与变量相关的实常数, $ D_z $和$ n $分别表示向量$ {\boldsymbol{z}}_{k} $和矩阵$ {\boldsymbol{P}}_{k|k-1} $的维数, $({\boldsymbol{x}}_k- {\boldsymbol{x}}_{k|k-1})^{\rm T}{\boldsymbol{P}}_{k|k-1}^{-1}(\cdot)$ 表示 $({\boldsymbol{x}}_k\;-\;{\boldsymbol{x}}_{k|k-1})^{\rm T}{\boldsymbol{P}}_{k|k-1}^{-1}({\boldsymbol{x}}_k\;-$ ${\boldsymbol{x}}_{k|k-1}) $的缩写形式(下文中类似情况不再一一说明). 根据式(26)可知, 由于高斯分布、学生$ t $分布和伽马分布均属于指数分布族而具有简单的对数形式, 因此便于采用迭代更新的方式计算其解析解. 以下给出$ {\lambda}_k $、$ {\boldsymbol{P}}_{k|k-1} $以及$ {\boldsymbol{x}}_{k} $的迭代估计的具体实现原理和步骤.
2.1 $ {\lambda}_k $迭代更新
根据式 (22)和式 (26), 在第$ i+1 $次变分迭代更新过程中, $ \lambda_k $的变分分布对数形式可表示为
$$ \begin{split} &\log q^{(i+1)}(\lambda_k) = \\ &\;\;\;\;\;\;-\frac{1}{2}\Bigl\{ {\rm{E}}_{{\boldsymbol{x}}_{k}, {{\boldsymbol{P}}_{k|k-1}}}^{i} [\lambda_k\left({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k})\right)^{\rm T} {\boldsymbol{R}}_{k}^{-1} \left(\cdot\right)]\;\times\\ &\;\;\;\;\;\;2\beta_k\lambda_k + (2-2\alpha_k-D_z)\log\lambda_k \Bigr\} + c_{\lambda_k} = \\ &\;\;\;\;\;\;-\frac{1}{2}\Bigl\{\lambda_k(2\beta_k+ {\rm{tr}}\left({\boldsymbol{R}}_{k}^{-1}\right) {\rm{E}}_{{\boldsymbol{x}}_{k}, {{\boldsymbol{P}}_{k|k-1}}}^{i}\;\times\\ &\;\;\;\;\;\;\bigl[\left({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k})\right)^{\rm T} \left(\cdot\right)\bigr] ) \;+\\ &\;\;\;\;\;\;(2-2\alpha_k-D_z)\log\lambda_k\Bigr\} + c_{\lambda_k}\\[-10pt] \end{split} $$ (27) 其中, $ {c}_{\lambda_k} $表示$ {\boldsymbol{x}}_{k} $和$ {{\boldsymbol{P}}_{k|k-1}} $积分后相关的实常数. 根据贝叶斯无偏估计理论, $ {\boldsymbol{x}}_{k} = {\boldsymbol{x}}_{k|k}^{i} + {\tilde{{{\boldsymbol{x}}}}}_{k} $, 其中$ {\boldsymbol{x}}_{k|k}^{i} $和$ {\tilde{{{\boldsymbol{x}}}}}_{k} $分别表示第$ i $次估计值及其估计偏差. 同时, 结合局部线性化技术, 式 (27)中期望部分可进一步改写为
$$ \begin{split} &{\rm{E}}_{{\boldsymbol{x}}_{k}, {{\boldsymbol{P}}_{k|k-1}}}^{i} \bigl[\left({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k})\right)^{\rm T} \left(\cdot\right)\bigr]\approx \\ &\qquad \bigl({\boldsymbol{z}}_k - {\boldsymbol{H}}_{k}^{i}({\boldsymbol{x}}_{k|k}^{i} + {\tilde{{{\boldsymbol{x}}}}}_{k})\bigr)^{\rm T} \left(\cdot\right) = \\ &\qquad\bigl({\boldsymbol{z}}_k - {\boldsymbol{H}}_{k}^{i}{\boldsymbol{x}}_{k|k}^{i}\bigr)^{\rm T}\left(\cdot\right) + \bigl({\boldsymbol{H}}_{k}^{i}\bigr)^{\rm T}{\boldsymbol{P}}_{k|k}^{i}{\boldsymbol{H}}_{k}^{i} \end{split} $$ (28) 其中, $ {\boldsymbol{H}}_{k}^{i} = \frac{\partial {\boldsymbol{h}}_k({\boldsymbol{x}}_k)}{\partial{\boldsymbol{x}}_k}\vert_{{\boldsymbol{x}}_k = {\boldsymbol{x}}_{k|k}^{i}} $表示量测函数在${\boldsymbol{x}}_k = {\boldsymbol{x}}_{k|k}^{i}$处的雅克比矩阵.
计算式 (14) 中伽马分布的对数表达式, 并结合式 (27)和式 (28), 不难看出, 第$ i+1 $次更新后参数$ \lambda_k^{i+1} $的变分分布中超参数$ {\alpha}_k^{i+1} $和$ {\beta}_k^{i+1} $的更新过程可表示为
$$ {\alpha}_k^{i+1} = {\alpha}_k^{i} + \frac{1}{2}D_z\;\; \;\;\;\; \qquad $$ (29) $$ {\beta}_k^{i+1} = {\beta}_k^{i}+ \frac{1}{2}{\rm tr}\left({\boldsymbol{R}}_{k}^{-1}{\boldsymbol{\Upsilon}}_k^{i}\right) $$ (30) 其中, ${\boldsymbol{\Upsilon}}_k^{i} \;= \;({\boldsymbol{z}}_k\; -\; {\boldsymbol{H}}_{k}^{i}{\boldsymbol{x}}_{k|k}^{i})^{\rm T} \;\;({\boldsymbol{z}}_k \;- \;{\boldsymbol{H}}_{k}^{i}{\boldsymbol{x}}_{k|k}^{i})\; +$ $({\boldsymbol{H}}_{k}^{i})^{\rm T}{\boldsymbol{P}}_{k|k}^{i}{\boldsymbol{H}}_{k}^{i}. $
根据式 (15)中伽马分布的均值表达式, 同时结合式 (29) 和式 (30), 参数$ \lambda_k $的第$ i+1 $次变分迭代更新$ \lambda_k^{i+1} $可表示为
$$ \lambda_k^{i+1} = \frac{{\alpha}_k^{i+1}}{{\beta}_k^{i+1}} = \frac{{\alpha}_k^{i} + \frac{1}{2}D_z} {{\beta}_k^{i}+\frac{1}{2}{\rm tr}\left({\boldsymbol{R}}_{k}^{-1}{\boldsymbol{\Upsilon}}_k^{i}\right)} $$ (31) 由式 (31) 可以清晰地看出, 辅助随机变量$ {\lambda}_k $的更新与$ {\boldsymbol{\Upsilon}}_k^{i} $负相关, 并且在迭代过程中$ {\boldsymbol{\Upsilon}}_k^{i} $的变化与上一次迭代的状态估计值及量测函数在此估计值处的局部线性化矩阵有关. 根据$ {\boldsymbol{\Upsilon}}_k^{i} $表达式, 第$ i $次迭代的估计值(可理解为第$ i+1 $次迭代估计的先验) 越接近真实状态, $ {\boldsymbol{\Upsilon}}_k^{i} $值越小, 根据式 (31) 可知$ \lambda_k^{i+1} $则越大; 相反, 第$ i $次的估计状态距真实状态较远, $ {\boldsymbol{\Upsilon}}_k^{i} $值越大, $ \lambda_k^{i+1} $则越小. 其物理意义是, 当发生量测随机缺失时导致滤波更新的新息$ ({\boldsymbol{z}}_k - {\boldsymbol{H}}_{k}^{i}{\boldsymbol{x}}_{k|k}^{i}) $增大, 同时$ {\boldsymbol{\Upsilon}}_k^{i} $值将随之增大, 从而造成式 (11) 中实际量测噪声方差增大, 这种情况将反馈于滤波过程中状态估计的自适应更新. 为了直观地解释这种现象, 结合式 (11) 和式 (13), 采用一维高斯分布和伽马分布说明其相关关系. 图1 给出伽马分布参数对量测似然的影响示意图. 在图1 中, 假设高斯分布的均值和方差分别为$ 25 $和${2}/{{\rm{E}}[\lambda]}$, 并且${\rm{E}}[\lambda] ={\alpha}/{\beta}$, 伽马分布参数$ \alpha $和$ \beta $的值分别在图1(a)和图1(b)中注明. 当量测未发生丢失时, $ \beta $的值较小, 高斯分布比较集中(如图1(a)所示); 当量测发生随机丢失, $ \beta $的值增大, 高斯分布比较分散(如图1(b)所示).
图 1 伽马分布参数对量测似然函数的影响示意图Fig. 1 The diagram of the influence of Gamma distribution parameters on likelihood2.2 $ {\boldsymbol{P}}_{k|k-1} $迭代更新
假设第$ i $次迭代状态估计值已知, 根据式 (23) 和式 (26), 在第$ i+1 $次变分贝叶斯迭代更新过程中, $ {\boldsymbol{P}}_{k|k-1} $的变分分布的对数形式表示为
$$ \begin{split} &\log q^{(i+1)}({\boldsymbol{P}}_{k|k-1}) = \\ &\quad-\frac{1}{2}\Bigl\{ {\rm{E}}_{{\boldsymbol{x}}_k, \lambda_k}^{i} \left[ ({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1})^{\rm T}{\boldsymbol{P}}_{k|k-1}^{-1}(\cdot)\right] +\\ &\quad({t}_{k|k-1}+n+2)\log|{\boldsymbol{P}}_{k|k-1}|\;+ \\ &\quad{\rm tr}({{{\boldsymbol{T}}}}_{k|k-1}{\boldsymbol{P}}_{k|k-1}^{-1}) \Bigr\} + {c}_{{\boldsymbol{P}}_{k|k-1}}= \\ &\quad-\frac{1}{2}\Bigl\{ {\rm tr}\left( {\rm{E}}_{{\boldsymbol{x}}_k, \lambda_k}^{i} \left[({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1})(\cdot)^{\rm T}\right]{\boldsymbol{P}}_{k|k-1}^{-1}\right)\;+\\ &\quad ({t}_{k|k-1}+n+2)\log|{\boldsymbol{P}}_{k|k-1}| \;+ \\ &\quad{\rm tr}({{{\boldsymbol{T}}}}_{k|k-1}{\boldsymbol{P}}_{k|k-1}^{-1}) \Bigr\} + {c}_{{\boldsymbol{P}}_{k|k-1}}\\[-12pt] \end{split} $$ (32) 其中, 参数$ {c}_{{\boldsymbol{P}}_{k|k-1}} $表示$ {\boldsymbol{x}}_{k} $和$ \lambda_k $积分后的相关性实常数. 式 (32)中的期望${ \rm{E}}_{{\boldsymbol{x}}_k, \lambda_k}^{i} [({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1}) ({\boldsymbol{x}}_k- {\boldsymbol{x}}_{k|k-1})^{\rm T}]$可转化为
$$ \begin{split} &{\rm{E}}_{{\boldsymbol{x}}_k, \lambda_k}^{i}\left[({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1}) (\cdot)^{\rm T}\right]= \\ &\quad{\boldsymbol{P}}_{k|k}^{i}+ ({\boldsymbol{x}}_{k|k}^{i}- {\boldsymbol{x}}_{k|k-1}) (\cdot)^{\rm T} = \\ &\quad{\boldsymbol{P}}_{k|k}^{i} + {\boldsymbol{K}}_k^{i}({\boldsymbol{z}}_{k}-{\boldsymbol{h}}_k({\boldsymbol{x}}_{k|k-1})) (\cdot)^{\rm T} \left({\boldsymbol{K}}_k^{i}\right)^{\rm T}= {\boldsymbol{\Omega}}_k \end{split} $$ (33) 其中, $ {\boldsymbol{K}}_k^{i} $表示在第$ i $次变分贝叶斯迭代中卡尔曼滤波的增益. 令第$ i $次迭代中IW分布参数分别为$ {t}_{k}^{i} $和$ {{{\boldsymbol{T}}}}_{k}^{i} $, 根据IW分布表达式 (4), 同时结合式 (32) 和式 (33), 则状态预测误差协方差$ {\boldsymbol{P}}_{k|k-1} $更新后变分分布的超参数分别为
$$ {t}_{k}^{i+1} = {t}_{k}^{i} +1\;\;\;\; \; $$ (34) $$ {{{\boldsymbol{T}}}}_{k}^{i+1} = {{{\boldsymbol{T}}}}_{k}^{i} + {\boldsymbol{\Omega}}_k $$ (35) 因此, 根据式 (6)中$ {\boldsymbol{P}}_{k|k-1} $均值表达式, 同时结合式 (34) 和式 (35), 状态预测误差协方差的第$ i+1 $次变分迭代更新$ {\boldsymbol{P}}_{k|k-1}^{i+1} $表示为
$$ {\boldsymbol{P}}_{k|k-1}^{i+1}= \frac{{{{\boldsymbol{T}}}}_{k}^{i+1}} {{t}_{k}^{i+1}-n-1} = \frac{{{{\boldsymbol{T}}}}_{k}^{i} + {\boldsymbol{\Omega}}_k}{{t}_{k}^{i} -n } $$ (36) 根据卡尔曼滤波实现原理可知, 当过程噪声方差建模不准确时, 状态预测误差协方差较大, 并且导致量测预测不精确而产生较大的量测新息$ {\boldsymbol{z}}_{k}- {\boldsymbol{h}}_k({\boldsymbol{x}}_{k|k-1}) $. 同时, 由式 (33)可知, $ {\boldsymbol{P}}_{k|k-1}^{i+1} $的迭代更新与量测新息正相关, 进而从物理意义上说明状态预测协方差建模的合理性.
2.3 $ {\boldsymbol{x}}_{k|k} $迭代更新
根据式 (24) 和式 (26), 在第$ i+1 $次迭代更新过程中, 系统状态的变分分布对数形式表示为
$$ \begin{split} &\log q^{(i+1)}({\boldsymbol{x}}_{k}) = -\frac{1}{2} \Bigl\{ {\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1}\times\\ &\;\;\left[ \lambda_k ({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k}))^{\rm T}{\boldsymbol{R}}_{k}^{-1} (\cdot) \right]\Bigr. \;+ \\ &\;\;\Bigl. {\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1}\left[ ({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1})^{\rm T}{\boldsymbol{P}}_{k|k-1}^{-1}(\cdot) \right] \Bigr\} +c_{{\boldsymbol{x}}_{k}}\;= \\ &\;\;- \frac{1}{2} \Bigl\{ {\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1} \left[\lambda_k \right] ({\boldsymbol{z}}_k - {\boldsymbol{h}}_{k}({\boldsymbol{x}}_{k}))^{\rm T}{\boldsymbol{R}}_{k}^{-1} (\cdot)\; +\\ &\;\; ({\boldsymbol{x}}_k-{\boldsymbol{x}}_{k|k-1})^{\rm T} {\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1}\left[{\boldsymbol{P}}_{k|k-1}^{-1}\right](\cdot) \Bigr\} + c_{{\boldsymbol{x}}_{k}} \\[-12pt] \end{split} $$ (37) 其中, $ c_{{\boldsymbol{x}}_{k}} $表示与$ {\boldsymbol{P}}_{k|k-1} $和$ \lambda_k $积分后相关性的实常数. 在第$ i+1 $次迭代中, 依据式 (31) 和式 (36) 已经获取状态预测误差协方差$ {\boldsymbol{P}}_{k|k-1} $和辅助变量$ \lambda_k $的估计值
$$ {\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1} \left[\lambda_k\right]= \lambda_k^{i+1} $$ (38) $$ \begin{aligned}[b] &{\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1} \bigl[{\boldsymbol{P}}_{k|k-1}^{-1}\bigr]= \\ &\quad \bigl({\rm{E}}_{{\boldsymbol{P}}_{k|k-1}, \lambda_k}^{i+1} \left[{\boldsymbol{P}}_{k|k-1}\right]\bigr)^{-1} = \bigl({\boldsymbol{P}}_{k|k-1}^{i+1}\bigr)^{-1} \\[-15pt]\end{aligned} $$ (39) 由贝叶斯滤波理论可知, 状态后验分布的近似变分分布应具有以下形式
$$ \begin{split} q^{(i+1)}({\boldsymbol{x}}_{k}) = \;& \frac{1}{{\boldsymbol{C}}_k^{i+1}} p^{(i+1)}({\boldsymbol{z}}_{k}|{\boldsymbol{x}}_{k}) p^{(i+1)}({\boldsymbol{x}}_{k}|{\boldsymbol{z}}_{k-1})= \\ &\frac{1}{{\boldsymbol{C}}_k^{i+1}}{\rm{N}}\left({\boldsymbol{z}}_{k}; {\boldsymbol{h}}_k\left({\boldsymbol{x}}_{k}\right), \frac{{\boldsymbol{R}}_{k}}{\lambda_k^{i+1}}\right)\times\\ &{\rm{N}}\left({\boldsymbol{x}}_{k};{\boldsymbol{x}}_{k|k-1}, {\boldsymbol{P}}_{k|k-1}^{i+1}\right)\\[-12pt] \end{split} $$ (40) 其中, $ {\boldsymbol{C}}_k^{i+1} $表示归一化常数
$$ {\boldsymbol{C}}_k^{i+1} = \int p^{(i+1)}({\boldsymbol{z}}_{k}|{\boldsymbol{x}}_{k}) p^{(i+1)}({\boldsymbol{x}}_{k}|{\boldsymbol{z}}_{k-1}){\rm{d}}{\boldsymbol{x}}_{k} $$ (41) 结合高斯分布表达式, 式 (40) 可进一步改写为
$$ \begin{split} &\log q^{(i+1)}({\boldsymbol{x}}_{k}) = \frac{1}{{\boldsymbol{C}}_k^{i+1}} \sqrt{\frac{|\lambda_k^{i+1}|}{(2\pi)^{D_z+D_x}|{{{\boldsymbol{R}}}}_k||{{\boldsymbol{P}}}_{k|k-1}^{i+1}|}}\;\;\times\\ &\;\;\;\;\exp \Biggr(-\frac{1}{2}\left( \left[\begin{array}{c} {\boldsymbol{z}}_k\\ {\boldsymbol{x}}_k \end{array}\right]- \left[\begin{array}{c} {\boldsymbol{h}}_k\left({\boldsymbol{x}}_k\right)\\ {\boldsymbol{x}}_{k|k-1} \end{array}\right] \right)^{\rm T}\times \\ &\;\;\;\;\left[\begin{array}{cc} \dfrac{{{{\boldsymbol{R}}}}_k}{\lambda_k^{i+1}} & 0\\ 0 & {{\boldsymbol{P}}}_{k|k-1}^{i+1} \end{array} \right]^{-1} \left( \left[\begin{array}{c} {\boldsymbol{z}}_k\\ {\boldsymbol{x}}_k \end{array}\right]- \left[\begin{array}{c} {\boldsymbol{h}}_k\left({\boldsymbol{x}}_k\right)\\ {\boldsymbol{x}}_{k|k-1} \end{array}\right] \right)\Biggr) \end{split} $$ (42) 综合式 (37) ~ (42), 目标状态后验分布的近似变分分布分别是以$ {\boldsymbol{x}}_{k|k}^{i+1} $和$ {\boldsymbol{P}}_{k|k}^{i+1} $为期望和协方差的高斯分布, 即
$$ q^{(i+1)}({\boldsymbol{x}}_{k}) = {\rm{N}}\left({\boldsymbol{x}}_{k}; {\boldsymbol{x}}_{k|k}^{i+1}, {\boldsymbol{P}}_{k|k}^{i+1}\right) $$ (43) 针对非线性系统状态估计问题, 现有贝叶斯滤波方法在实现式 (43) 中状态估计及其误差协方差的迭代更新过程中, 往往难以获得与真实后验“紧密”近似的变分分布. 虽然在多个变量坐标迭代优化过程中能够弥补部分由于线性化和参数不精确带来的误差, 但估计精度提升效果受限, 有时难以满足实际工程需求. 因此, 考虑在获得$ \lambda^{i+1} $和$ {\boldsymbol{P}}_{k|k}^{i+1} $估计值的基础上, 在置信下界最大化条件下结合自然梯度方法和 Fisher 信息矩阵, 推导给出$ {\boldsymbol{x}}_{k|k}^{i+1} $和$ {\boldsymbol{P}}_{k|k}^{i+1} $迭代更新解析表达式的具体实现.
3. 基于自然梯度的噪声自适应变分贝叶斯滤波器
3.1 变分置信下界的自然梯度
第$ i $次变分迭代后, 在分别获取参数$ \lambda_k $和$ {\boldsymbol{P}}_{k|k-1} $的估计值$ \lambda_k^i $和$ {\boldsymbol{P}}_{k|k-1}^i $的基础上, 式 (2) 可转化为仅包含未知变量$ {\boldsymbol{x}}_k $的形式
$$ {{\cal{L}}}\left({\boldsymbol{\psi}}_{k}\right) = \int q\left({\boldsymbol{x}}_k|{\boldsymbol{\psi}}_k\right) \log \frac{p\left({\boldsymbol{x}}_k, {\boldsymbol{z}}_k\right)} {q\left({\boldsymbol{x}}_k|{\boldsymbol{\psi}}_k\right)}{\rm{d}}{\boldsymbol{x}}_k $$ (44) 定义此时变分参数${\boldsymbol{\psi}}_{k}= ({\boldsymbol{x}}_{k|k}, {\boldsymbol{P}}_{k|k})$, 通过最大化式(44)中的置信下界即可获得状态估计及其误差协方差. 自然梯度通过信息几何方法寻找统计流形空间的目标函数最速下降/上升方向, 给出了一种实现式 (44)置信下界求解的可行性方案[25]. 下面以置信下界$ {\cal{L}}(\boldsymbol \psi_k) $最大化为目标函数, 同时结合Fisher 信息矩阵推导关于变分参数$ {\boldsymbol \psi}_{k} $的自然梯度. 式 (44)的置信下界可以进一步表示为
$$ {{\cal{L}}}\left({\boldsymbol{\psi}}_{k}\right) = {\rm{E}}_{q}\left[\log p\left({\boldsymbol{z}}_{k}|{\boldsymbol{x}}_{k}\right)\right] - {\cal{D}}_{\text{KL}}\left[q\left({\boldsymbol{x}}_{k}|{\boldsymbol{\psi}}_{k}\right)\| p\left({\boldsymbol{x}}_{k}\right)\right] $$ (45) 假设第$ i $ 次迭代是第$ i+1 $次迭代的先验, 即式(45)中的先验信息$ p\left({\boldsymbol{x}}_{k}\right) $可以近似为 $ p\left({\boldsymbol{x}}_{k}\right)\approx q\left({\boldsymbol{x}}_{k}|{\boldsymbol{\psi}}_{k}^i\right) ,$ 则变分参数$ {\boldsymbol{\psi}}_{k} $的最优估计值表示为
$$ \begin{split} {\boldsymbol \psi}_{k}^{*} = \;&\arg \underset{{\boldsymbol \psi}_{k}}{\max} \Bigl\{ {\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] -\\ &{\cal{D}}_{\text{KL}}\left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right) {\| q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right) } \right] \Bigr\} \end{split} $$ (46) 通过计算式 (46)等号右边关于$ {\boldsymbol \psi}_{k} $线性化函数, 可获取置信下界的自然梯度以及相应的最优$ {\boldsymbol \psi}_{k}^* $.
定理 1. 假设系统状态演化满足高斯分布特性, 并且对数似然期望$ {\rm{E}}_{q}\left[\log p\left({\boldsymbol{z}}_{k}|{\boldsymbol{x}}_{k}\right)\right] $在$ {\boldsymbol \psi}_{k} = {\boldsymbol \psi}_{k}^i $的邻域内一阶可导, 同时, $ {\cal{D}}_{\text{KL}}\left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right)\| q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right)\right] $在$ {\boldsymbol \psi}_{k} ={\boldsymbol \psi}_{k}^i $的邻域内二阶可导, 则第$ i+1 $次迭代过程中变分置信下界的自然梯度为
$$ \widetilde{\nabla}_{{\boldsymbol \psi}_{k}}^{i+1} = {{\boldsymbol{F}}}_{{{\boldsymbol{\psi}}}_{k}^{i}}^{-1}\nabla_{{\boldsymbol \psi}_{k}} {{\rm{E}}_{q}\left[\log p({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k|k}^{i})\right]} $$ (47) 证明. 根据变分参数的最优表达式 (46)可知, 通过对其求关于 $ {\boldsymbol \psi}_{k} $的偏导数即可获得置信下界最大化条件下的极值点. 由于对数似然期望一阶可导, 令$\Delta {\boldsymbol \psi}_{k} = {\boldsymbol \psi}_{k}- {\boldsymbol \psi}_{k}^{i}\to 0$, 根据泰勒展开上式对数似然期望线性化后可以近似表示为
$$ \begin{split} &{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] ={\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right]_{{\boldsymbol \psi}_{k} = {\boldsymbol \psi}_{k}^{i}} + \\ &\qquad \nabla_{{{\boldsymbol{\psi}}}_{k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right]_{{\boldsymbol \psi}_{k} = {\boldsymbol \psi}_{k}^{i}}\Delta {\boldsymbol \psi}_{k} + {\cal{O}}({\Delta{{\boldsymbol \psi}_{k}}}^{2}) \end{split} $$ (48) 其中, $\nabla_{{{\boldsymbol{\psi}}}_{k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right]_{{\boldsymbol \psi}_{k} = {\boldsymbol \psi}_{k}^{i}}$表示对数似然期望在$ {\boldsymbol \psi}_{k}^{i} $处的雅克比矩阵, $ {\cal{O}}({\Delta{{\boldsymbol \psi}_{k}}}^{2}) $表示$ {\Delta{{\boldsymbol \psi}_{k}}}^{2} $的高阶无穷小.
同时, 由于$ {\cal{D}}_{\text{KL}}\left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right)\|q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right)\right] $二阶可导, 式 (46) 中第2项在$ {{\boldsymbol \psi}_{k} = {\boldsymbol \psi}_{k}^{i}} $点处进行泰勒展开后可表示为
$$ \begin{split} &{\cal{D}}_{\text{KL}}\left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right)\|q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right)\right] =\\ &\quad\frac{1}{2}\Delta {\boldsymbol \psi}_{k}^{\rm T}{{\boldsymbol{F}}}_{{\boldsymbol \psi}_{k}}\Delta {\boldsymbol \psi}_{k}+ {\cal{O}}({\Delta{{\boldsymbol \psi}_{k}}}^{3}) \end{split} $$ (49) 其中, $ {{\boldsymbol{F}}}_{{\boldsymbol \psi}_{k}} $为具有半正定性 Fisher 信息矩阵, 即
$$ {{\boldsymbol{F}}}_{{\boldsymbol \psi}_{k}}\approx\nabla_{{{\boldsymbol{\psi}}}_{k}}^{2} {\cal{D}}_{\text{KL}}\left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right)\| q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right)\right] $$ (50) 此时, 式 (46) 进一步转化为
$$ \begin{split} {\boldsymbol \psi}_{k}^{*} = \;&\arg\underset{\Delta{{\boldsymbol \psi}_{k}} \to 0}{ \max} \Bigl\{ \nabla_{{{\boldsymbol{\psi}}}_{k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right]\Delta {\boldsymbol \psi}_{k}\;-\\ &\frac{1}{2}\Delta {\boldsymbol \psi}_{k}^{\rm T}{{\boldsymbol{F}}}_{{\boldsymbol \psi}_{k}}\Delta{\boldsymbol \psi}_{k} \Bigr\} \end{split} $$ (51) 计算式(51)关于$ {\boldsymbol \psi}_{k} $的偏导, 并令其取值为零, 可获得在第$ i+1 $次变分迭代中ELBO 关于$ {\boldsymbol \psi}_{k} $的自然梯度
$$ \widetilde{\nabla}_{{\boldsymbol \psi}_{k}}^{i+1} = {{\boldsymbol{F}}}_{{{\boldsymbol{\psi}}}_{k}^{i}}^{-1}\nabla_{{\boldsymbol \psi}_{k}} {{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k|k}^{i}\right)\right]} $$ (52) 因此, 在置信下界最大化条件下变分参数$ {\boldsymbol \psi}_{k} $的更新表达式为
$$ \begin{split} &{\boldsymbol \psi}_{k}^{i+1} = {\boldsymbol \psi}_{k}^{i} + \widetilde{\nabla}_{{\boldsymbol \psi}_{k}}^{i+1} = \\ &\qquad{\boldsymbol \psi}_k^{i} + {{\boldsymbol{F}}}_{{\boldsymbol \psi}_k^{i}}^{-1}\nabla_{{\boldsymbol\psi}_k} {{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_k|{{{\boldsymbol{x}}}}_k\right)\right]} \end{split} $$ (53) □ 需要说明的是, VB 迭代更新等价于在统计流形空间沿以 Fisher 信息矩阵作为权重的自然梯度方向移动, 从而获得后验分布的最佳近似, 并且具有以下性质:
1) 自然梯度推导中, ${{\boldsymbol{F}}}_{{{\boldsymbol{\psi}}}_k^i}$和$ \nabla_{{\boldsymbol \psi}_k}{{\rm{E}}_{q}[\log p ({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k|k}^{i})]} $ 的计算依赖于参数化的变分分布$ q\left({{{\boldsymbol{x}}}}_k|{\boldsymbol \psi}_k\right) $. 但是KL 散度作为变分分布选择的度量是不变的.
2)$ \Delta{\boldsymbol \psi}_k \to 0 $时, 式 (46) 中的第2项KL 散度表达式转化为式 (49), 因此从局部意义上迭代过程中的变分分布与后验分布的近似误差是正定二次型.
3) 自然梯度是Fisher有效的[25], 因此上述优化过程是 Fisher 有效的估计器. 特别地, 上述$ {\boldsymbol \psi}_k $的迭代估计是无偏的.
4) 在自然梯度的推导过程中, 在无限小的邻域内 (即$ \Delta{\boldsymbol \psi}_k \to 0 $) 给出 Fisher 信息矩阵与 KL 散度之间的近似关系, 并且${{\boldsymbol{F}}}_{{{\boldsymbol{\psi}}}_k^i}$和$ \nabla_{{\boldsymbol \psi}_k}{{\rm{E}}_{q}[\log p({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k|k}^{i})]} $的计算依赖于参数化的变分分布$ q({{{\boldsymbol{x}}}}_k|{\boldsymbol \psi}_k) $, 因此, 随着变分迭代的进行, Fihser 信息不断变化.
3.2 基于自然梯度的滤波更新
系统状态的变分分布包含两个超参数, 即当前时刻状态估计$ {\boldsymbol{x}}_{k|k} $及其误差协方差$ {\boldsymbol{P}}_{k|k} $, 为实现其迭代更新需分别计算相应的自然梯度. 定理 2 及其证明给出了变分置信下界分别关于$ {\boldsymbol{x}}_{k|k} $和$ {\boldsymbol{P}}_{k|k} $的自然梯度及其推导过程.
定理 2. 假设第$ i $次迭代更新的状态估计及其估计误差协方差分别为$ {{{\boldsymbol{x}}}}_{k|k}^i $和$ {{{\boldsymbol{P}}}}_{k|k}^i $, 在满足定理 1 的条件下, 对数似然期望 $ {\rm{E}}_{q}\left[\log p\left({\boldsymbol{z}}_{k}|{\boldsymbol{x}}_{k}\right)\right] $分别在${{{\boldsymbol{x}}}}_{k} = {{{\boldsymbol{x}}}}_{k|k}^i$和$ {{{\boldsymbol{P}}}}_{k|k} = {{{\boldsymbol{P}}}}_{k|k}^i $的邻域内一阶可导; 同时, KL 散度 $ {\cal{D}}_{\text{KL}} \left[q({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k})\|q({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i})\right] $ 分别在 $ {{{\boldsymbol{x}}}}_{k} = {{{\boldsymbol{x}}}}_{k|k}^i $ 和$ {{{\boldsymbol{P}}}}_{k|k} = {{{\boldsymbol{P}}}}_{k|k}^i $的邻域内二阶可导; 并且在已经分别获取第$ i $次迭代状态预测误差协方差以及辅助变量估计值$ {{{\boldsymbol{P}}}}_{k|k-1}^i $和 $ \lambda_k^i $的前提下, 变分置信下界关于$ {{{\boldsymbol{x}}}}_{k} $和$ {{{\boldsymbol{P}}}}_{k|k} $的自然梯度分别表示为
$$ \begin{split} \widetilde{\nabla}_{{{{\boldsymbol{x}}}}_{k}}^{i+1} = \;&{{\boldsymbol{F}}}_{{{{\boldsymbol{x}}}}_{k|k}^{i}}^{-1} \nabla_{{{{\boldsymbol{x}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}^{i}\right)\right]=\\ &\lambda_k^i{{{\boldsymbol{P}}}}_{k|k}^{i}\left(({{{\boldsymbol{H}}}}_k^{i})^{\rm{T}} {{{\boldsymbol{R}}}}_{k}^{-1} ({{{\boldsymbol{z}}}}_{k}-{{{\boldsymbol{h}}}}({{{\boldsymbol{x}}}}_{k|k}^{i}))\right) \end{split} $$ (54) $$ \begin{split} \widetilde{\nabla}_{{{{\boldsymbol{P}}}}_{k|k}}^{i+1} =\;& {{\boldsymbol{F}}}_{{{{\boldsymbol{P}}}}_{k|k}^{i}}^{-1} \nabla_{{{{\boldsymbol{P}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}^{i}\right)\right]= \\ &{{{\boldsymbol{P}}}}_{k|k}^{i}\left({{{\boldsymbol{I}}}}-\lambda_k^i\left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm{T}} {{{\boldsymbol{R}}}}_{k}^{-1}{{{\boldsymbol{H}}}}_k^{i}{{{\boldsymbol{P}}}}_{k|k}^{i}\right) \end{split} $$ (55) 其中, $ {{{\boldsymbol{H}}}}_k^{i} = \frac{\partial {{{\boldsymbol{h}}}}({{{\boldsymbol{x}}}}_k)}{\partial {{{\boldsymbol{x}}}}_{k}}\big|_{{{{\boldsymbol{x}}}}_{k}={{{\boldsymbol{x}}}}_{k|k}^i} $表示量测函数的雅克比矩阵.
证明. 首先推导在定理 2 中假设条件下 KL散度$ {\cal{D}}_{\rm{KL}} \left[q({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k})\|q({{{\boldsymbol{x}}}}_{k}|{\boldsymbol\psi}_{k}^{i})\right] $关于$ {{{\boldsymbol{x}}}}_{k|k} $和$ {{{\boldsymbol{P}}}}_{k|k} $的二阶导数. 若高斯分布$ q_{1}\sim{\rm{N}}\left({\boldsymbol\xi}_{1} ; \, {\boldsymbol\mu}_{1}, {{{\boldsymbol{C}}}}_{1}\right) $和 $q_{2}\sim {\rm{N}}({\boldsymbol\xi}_{2}; \, {\boldsymbol\mu}_{2}, {{{\boldsymbol{C}}}}_{2})$具有相同维数$ d $, 则两分布之间的KL 散度表示为
$$ \begin{split} &{\cal{D}}_{\mathrm{KL}}[q_{1}\|q_{2}]= \frac{1}{2}\{\ln (|{{{\boldsymbol{C}}}}_{2}||{{{\boldsymbol{C}}}}_{1}|^{-1}) \;+\\ &\qquad\operatorname{tr}({{{\boldsymbol{C}}}}_{2}^{-1} {{{\boldsymbol{C}}}}_{1}) + ({\boldsymbol \mu}_{2}-{\boldsymbol \mu}_{1})^{\mathrm{T}} \times \\ &\qquad{{{\boldsymbol{C}}}}_{2}^{-1}({\boldsymbol \mu}_{2}-{\boldsymbol \mu}_{1})-d\} \end{split} $$ 因此, 迭代过程中变分分布之间的 KL 散度表示为
$$ \begin{split} &{\cal{D}}_{\rm{KL}} \left[q({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k})\|q({{{\boldsymbol{x}}}}_{k}|{\boldsymbol\psi}_{k}^{i})\right] =\\ &\;\;\;\;\;\;\frac{1}{2}\Bigl\{\ln \left(\bigl\vert {{{\boldsymbol{P}}}}_{k|k}^{i}\bigr\vert\bigl\vert {{{\boldsymbol{P}}}}_{k|k}\bigr\vert^{-1}\right) + \; \Bigr. {\rm{tr}}\left(({{{\boldsymbol{P}}}}_{k|k}^{i})^{-1}{{{\boldsymbol{P}}}}_{k|k}\right)+\\ &\;\;\;\;\;\;\Bigl.({{{\boldsymbol{x}}}}_{k|k}^{i}-{{{\boldsymbol{x}}}}_{k|k})^{\rm{T}} ({{{\boldsymbol{P}}}}_{k|k}^{i})^{-1}(\cdot) - d_{{{{\boldsymbol{x}}}}_k}\Bigr\} \\[-12pt] \end{split} $$ (56) 结合式 (50), 关于$ {{{\boldsymbol{x}}}}_{k|k} $的 Fisher 信息矩阵表示为
$$ \begin{split} {{\boldsymbol{F}}}_{{{{\boldsymbol{x}}}}_{k|k}} \approx\;& \nabla_{{{{\boldsymbol{x}}}}_{k|k}}^{2} {\cal{D}}_{\text{KL}} \left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right)\| q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right)\right] = \\ &\frac{\partial^2\left(({{{\boldsymbol{x}}}}_{k|k}^{i}-{{{\boldsymbol{x}}}}_{k|k})^{\rm{T}} ({{{\boldsymbol{P}}}}_{k|k}^{i})^{-1}({{{\boldsymbol{x}}}}_{k|k}^{i}-{{{\boldsymbol{x}}}}_{k|k})\right)} {2\partial {{{\boldsymbol{x}}}}_{k|k}\partial {{{\boldsymbol{x}}}}_{k|k}} = \\ &({{{\boldsymbol{P}}}}_{k|k}^{i})^{-1} \\[-12pt] \end{split} $$ (57) 关于$ {{{\boldsymbol{P}}}}_{k|k} $的 Fisher 信息矩阵的推导如下:
$$ \begin{split} {{\boldsymbol{F}}}_{{{{\boldsymbol{P}}}}_{k|k}} = \;& \nabla_{{{{\boldsymbol{P}}}}_{k|k}}^{2} {\cal{D}}_{\text{KL}}\left[q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}\right)\| q\left({{{\boldsymbol{x}}}}_{k}|{\boldsymbol \psi}_{k}^{i}\right)\right]= \\ &-\frac{\partial}{2\partial {{{\boldsymbol{P}}}}_{k|k}}\left(\frac{\partial \ln{\left\vert {{{\boldsymbol{P}}}}_{k|k}\right\vert}}{\partial {{{\boldsymbol{P}}}}_{k|k}}\right)= \\ &-\frac{\partial {{{\boldsymbol{P}}}}_{k|k}^{-1}}{\partial {{{\boldsymbol{P}}}}_{k|k}} + \frac{\partial {{{\boldsymbol{P}}}}_{k|k}^{-1}}{2\partial {{{\boldsymbol{P}}}}_{k|k}}\circ {{{\boldsymbol{I}}}} \end{split} $$ (58) 其中, 哈达玛积$ \circ $表示取矩阵的对角线元素构成单位阵. 忽略误差协方差矩阵非对角线元素的影响, 式(58)可以近似简化为
$$ {{\boldsymbol{F}}}_{{{{\boldsymbol{P}}}}_{k|k}} \approx -\frac{\partial {{{\boldsymbol{P}}}}_{k|k}^{-1}}{2\partial {{{\boldsymbol{P}}}}_{k|k}} $$ (59) 由矩阵求导知识可知, 矩阵$ {{{\boldsymbol{X}}}} $逆函数$ {{{\boldsymbol{X}}}}\to {{{\boldsymbol{X}}}}^{-1} $的导数表示为 $\frac{\partial ({{{\boldsymbol{X}}}}^{-1})_{pl}}{{\partial{{\boldsymbol{X}}}}_{mn}} = -({{{\boldsymbol{X}}}}^{-1})_{pm}({{{\boldsymbol{X}}}}^{-1})_{nl}$, 其中, $ m $和$ p $分别表示矩阵$ {{{\boldsymbol{X}}}} $的不同行, $ n $和$ l $分别表示其不同列. 因此, 在迭代优化过程中关于估计误差协方差的 Fisher 信息矩阵表示为
$$ {{\boldsymbol{F}}}_{{{{\boldsymbol{P}}}}_{k|k}} = \frac{1}{2}({{{\boldsymbol{P}}}}_{k|k}^{i})^{-1}\otimes({{{\boldsymbol{P}}}}_{k|k}^{i})^{-1} $$ (60) 其中, 符号$ \otimes $表示矩阵克罗内克积运算.
其次, 推导$ {\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] $关于$ {{{\boldsymbol{x}}}}_{k|k} $和$ {{{\boldsymbol{P}}}}_{k|k} $的一阶偏导数. 根据 Bonnet 定理, $ {\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] $关于$ {{{\boldsymbol{x}}}}_{k|k} $的梯度可以表示为
$$ \nabla_{{{{\boldsymbol{x}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] = {\rm{E}}_{q}\left[\nabla_{{{{\boldsymbol{x}}}}_{k}}\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] $$ (61) 结合服从高斯分布的似然函数表达和局部线性化方法, 式(61)可转化为
$$ \begin{split} & \nabla_{{{{\boldsymbol{x}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] =\\ &\qquad{\rm{E}}_{q}\left[\lambda_k^i{{{\boldsymbol{h}}}}_{k}^{\rm T}\left({{{\boldsymbol{x}}}}_{k}\right){{{\boldsymbol{R}}}}_{k}^{-1}\left({{{\boldsymbol{z}}}}_{k}-{{{\boldsymbol{h}}}}_{k}\left({{{\boldsymbol{x}}}}_{k}\right)\right)\right]\approx \\ &\qquad\lambda_k^i\left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm{T}}{{{\boldsymbol{R}}}}_{k}^{-1}\left({{{\boldsymbol{z}}}}_{k}- {{{\boldsymbol{h}}}}({{{\boldsymbol{x}}}}_{k|k}^{i})\right) \end{split} $$ (62) 同时, 根据 Price 定理可知
$$ \begin{split} & \nabla_{{{{\boldsymbol{P}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right]=\\ &\qquad \frac{1}{2}{\rm{E}}_{q}\left[\nabla_{{{{\boldsymbol{x}}}}_{k}, {{{\boldsymbol{x}}}}_{k}}^{2}\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] \end{split} $$ (63) 其中, $ {\rm{E}}_{q}\left[\nabla_{{{{\boldsymbol{x}}}}_{k}, {{{\boldsymbol{x}}}}_{k}}^{2}\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] $可表示为
$$ \begin{split} & \nabla_{{{{\boldsymbol{x}}}}_{k}, {{{\boldsymbol{x}}}}_{k}}^{2} \log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right) = \lambda_k^i\left(\nabla_{{{{\boldsymbol{x}}}}_{k}}^{2}{{{\boldsymbol{h}}}}_{k}^{\rm T}({{{\boldsymbol{x}}}}_{k})\right){{{\boldsymbol{R}}}}_{k}^{-1}\times \\ &\quad\left({{{\boldsymbol{z}}}}_{k}-{{{\boldsymbol{h}}}}_{k}({{{\boldsymbol{x}}}}_{k})\right) - \lambda_k^i(\nabla_{{{{\boldsymbol{x}}}}_{k}}{{{\boldsymbol{h}}}}_{k}^{\rm T} ({{{\boldsymbol{x}}}}_{k})){{{{\boldsymbol{R}}}}_{k}^{-1}}(\cdot) \end{split} $$ (64) 由于$ {\rm{E}}_{q}\left[{{{\boldsymbol{z}}}}_{k}-{{{\boldsymbol{h}}}}_{k}\left({{{\boldsymbol{x}}}}_{k}\right)\right]=0 $, 局部线性化处理后, 式 (63) 可表示为
$$ \nabla_{{{{\boldsymbol{P}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}\right)\right] \approx - \frac{1}{2}\lambda_k^i\left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm T}{{{\boldsymbol{R}}}}_{k}^{-1}{{{\boldsymbol{H}}}}_k^{i} $$ (65) 综合上述过程, 在第$ i+1 $次迭代中变分置信下界关于 $ {{{\boldsymbol{x}}}}_{k|k} $和$ {{{\boldsymbol{P}}}}_{k|k} $的自然梯度分别表示为
$$ \begin{split} \widetilde{\nabla}_{{{{\boldsymbol{x}}}}_{k}}^{i+1} =\;& {{\boldsymbol{F}}}_{{{{\boldsymbol{x}}}}_{k|k}^{i}}^{-1} \nabla_{{{{\boldsymbol{x}}}}_{k|k}} {\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}^{i}\right)\right]= \\ &\lambda_k^i{{{\boldsymbol{P}}}}_{k|k}^{i}\left(\left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm{T}}{{{\boldsymbol{R}}}}_{k}^{-1}({{{\boldsymbol{z}}}}_{k}-{{{\boldsymbol{h}}}}({{{\boldsymbol{x}}}}_{k|k}^{i}))\right) \end{split} $$ (66) $$ \begin{split} \widetilde{\nabla}_{{{{\boldsymbol{P}}}}_{k}}^{i+1} = \;& {{{\boldsymbol{P}}}}_{k|k}^{i} + {{\boldsymbol{F}}}_{{{{\boldsymbol{P}}}}_{k|k}^{i}}^{-1} \nabla_{{{{\boldsymbol{P}}}}_{k|k}}{\rm{E}}_{q}\left[\log p\left({{{\boldsymbol{z}}}}_{k}|{{{\boldsymbol{x}}}}_{k}^{i}\right)\right] = \\ &{{{\boldsymbol{P}}}}_{k|k}^{i}\left({{{\boldsymbol{I}}}}-\lambda_k^i\left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm{T}}{{{\boldsymbol{R}}}}_{k}^{-1}{{{\boldsymbol{H}}}}_k^{i}{{{\boldsymbol{P}}}}_{k|k}^{i}\right) \end{split} $$ (67) □ 结合式 (53)中变分参数迭代更新以及式 (66)和式(67)中自然梯度的具体表达式, 第$ i+1 $次迭代时状态估计值及其误差协方差表示为
$$ {{{\boldsymbol{x}}}}_{k|k}^{i+1} = {{{\boldsymbol{x}}}}_{k|k}^{i} + \lambda_k^i{{{\boldsymbol{P}}}}_{k|k}^{i}\left(({{{\boldsymbol{H}}}}_k^{i})^{\rm{T}}{{{\boldsymbol{R}}}}_{k}^{-1}({{{\boldsymbol{z}}}}_{k}-{{{\boldsymbol{h}}}}({{{\boldsymbol{x}}}}_{k|k}^{i}))\right) $$ (68) $$ {{{\boldsymbol{P}}}}_{k|k}^{i+1} = {{{\boldsymbol{P}}}}_{k|k}^{i}\left({{{\boldsymbol{I}}}}-\lambda_k^i\left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm{T}}{{{\boldsymbol{R}}}}_{k}^{-1}{{{\boldsymbol{H}}}}_k^{i}{{{\boldsymbol{P}}}}_{k|k}^{i}\right) $$ (69) 综合上述式(68)和式 (69) 的构建过程, 本文给出了一种基于自然梯度的噪声自适应变分贝叶斯滤波算法, 为便于理解算法整体实现流程, 下面以伪代码的形式给出算法具体实现步骤, 详见算法1.
算法1. 基于自然梯度的噪声自适应变分贝叶斯滤波算法
1: 初始化状态估计$ {\boldsymbol{x}}_{1|1} $, 估计误差协方差$ {\boldsymbol{P}}_{1|1} $, 迭代次数$ {Iter} $, 伽马分布参数$ \alpha_0 $和$ \beta_0 $;
2: for$ k = 2:K $
3: 计算状态一步预测 $ {\boldsymbol{x}}_{k|k-1} = {\boldsymbol{f}}_{k}\left({\boldsymbol{x}}_{k-1|k-1}\right) $;
4: for$ i = 1:{Iter} $且$ e\leq\epsilon $
5: 根据式 (29) ~ (31), 计算 $ \lambda_{k}^{i} $的估计值;
6: 根据式 (34) ~ (36), 计算未知变量状态预测误差协方差$ {\boldsymbol{P}}_{k|k-1}^{i} $的估计值;
7: 根据式 (7) 和式 (8), 计算IW分布参数$ t_{k|k-1} $和$ {{\boldsymbol{T}}}_{k|k-1} $;
8: 计算自适应变分贝叶斯滤波器的增益 ${{{\boldsymbol{K}}}}^i_k ={\boldsymbol{P}}_{k|k-1}^{i}({{{\boldsymbol{H}}}}_k^i)^{\rm T}({{{\boldsymbol{H}}}}_k^i{\boldsymbol{P}}_{k|k-1}^{i}({{{\boldsymbol{H}}}}_k^i)^{\rm T} + \frac{{\boldsymbol{R}}_{k}}{\lambda_k^i})^{-1}$;
9: 计算非线性状态估计的迭代更新值 $\hat{{\boldsymbol{x}}}_{k|k} = {\boldsymbol{x}}_{k|k-1} + {{{\boldsymbol{K}}}}^i_k\left({\boldsymbol{z}}_{k}-{\boldsymbol{h}}_k\left({\boldsymbol{x}}_{k|k-1} \right) \right) $;
10: 计算非线性状态估计误差协方差的迭代更新值 $ \hat{{\boldsymbol{P}}}_{k|k} = \left({{{\boldsymbol{I}}}}-{{{\boldsymbol{K}}}}^i_k{{{\boldsymbol{H}}}}_k^i\right){\boldsymbol{P}}_{k|k-1}^{i} $;
11: 令$ {\boldsymbol{x}}_{k|k}^{i} = \hat{{\boldsymbol{x}}}_{k|k} $, $ {\boldsymbol{P}}_{k|k}^{i} = \hat{{\boldsymbol{P}}}_{k|k} $;
12: 根据式 (66) 和式 (67), 更新第$ i+1 $次迭代后的非线性状态估计值$ {\boldsymbol{x}}_{k|k}^{i+1} $和其估计误差协方差$ {\boldsymbol{P}}_{k|k}^{i+1} $;
13: 根据式 (70), 计算相对误差$ {\boldsymbol{e}}_r $;
14: end for
15: 输出$ {\boldsymbol{x}}_{k|k}= {\boldsymbol{x}}_{k|k}^{i+1} $和$ {\boldsymbol{P}}_{k|k}= {\boldsymbol{P}}_{k|k}^{i+1} $.
16: end for
由算法1中状态估计以及估计误差协方差的更新过程可知, 算法1具有以下特点和优势.
1) 状态估计$ {{{\boldsymbol{x}}}}_{k|k}^{i} $的更新以$ {{{\boldsymbol{P}}}}_{k|k}^{i} $为先决条件, 使得$ {{{\boldsymbol{x}}}}_{k|k}^{i} $沿自然梯度方向自适应地向真实状态移动. 在滤波初始阶段, $ {{{\boldsymbol{P}}}}_{k|k}^{i} $较大, $ {{{\boldsymbol{x}}}}_{k|k}^{i} $向真实位置移动的速度较快, 从而能够较快地到达真实值附近, 即估计器能够较快地收敛; 当滤波迭代达到收敛时, $ {{{\boldsymbol{P}}}}_{k|k}^{i} $较小, $ {{{\boldsymbol{x}}}}_{k|k}^{i} $的移动速度减缓, 从而使估计器能够更好地接近真实值.
2) 从式 (67) 可以明显地看出, $ {{{\boldsymbol{P}}}}_{k|k}^{i+1} \leq {{{\boldsymbol{P}}}}_{k|k}^{i} $, 并且由于$ \left({{{\boldsymbol{H}}}}_k^{i}\right)^{\rm{T}}{\boldsymbol{R}}_{k}^{-1}{{{\boldsymbol{H}}}}_k^{i} $是海塞阵的期望, 表明$ {{{\boldsymbol{P}}}}_{k|k}^{i} $具有二阶收敛性.
3) 自然梯度的方向是目标函数在统计流形空间中 ELBO 的最速上升方向, 即 ELBO 沿基于 Fisher 信息矩阵的自然梯度向使其增大的方向移动, 以从信息几何角度获得状态后验分布的最优近似.
为实现迭代优化过程的自适应, 采用以下相对估计误差$ {\boldsymbol{e}}_r $作为判断迭代是否终止的条件.
$$ {\boldsymbol{e}}_r = \Bigg\vert\frac{{\boldsymbol{x}}_{k|k}^{i+1}-{\boldsymbol{x}}_{k|k}^{i}} {{\boldsymbol{x}}_{k|k}^i}\Bigg\vert \leq \epsilon $$ (70) 其中, $ \epsilon $表示一个较小的正实数, 其取值大小可根据实际应用场景的精度需求合理设定.
4. 实验验证
4.1 仿真算例
为验证本文提出算法的可行性和有效性, 采用二维笛卡尔坐标系下三雷达量测的目标跟踪系统进行$ 500 $次 Monte Carlo 仿真实验, 目标做转弯率未知的匀速转弯运动, 并假设过程噪声先验信息不准确且缓慢变化[13], 同时三个雷达传感器均存在量测随机缺失现象. 本文所提算法记为 VBAKF-NG, 其三个传感器分布式估计误差协方差融合方法记为DVBAKF-NG. 仿真实验中选取对比算法分别为: 结合本文构建的逆威沙特分布的共轭先验分布模型和服从学生$ t $分布的量测似然模型, 分别利用扩展卡尔曼滤波器 (Extended Kalman filter, EKF)和无迹卡尔曼滤波 (Unscented Kalman filter, UKF) 滤波器实现状态的预测与更新, 其中变分扩展卡尔曼自适应滤波算法记为 EKF-VB, 其对应的三个传感器分布式估计误差协方差融合方法记为DEKF-VB, 变分无迹卡尔曼自适应滤波方法记为 UKF-VB, 采用传统自适应结构的无迹卡尔曼滤波方法记为 UKF-FN, 基于虚拟量测采样的集合卡尔曼滤波器记为 EnKF, 其对应的三个传感器分布式估计误差协方差融合方法分别记为 DUKF-VB, DUKF-FN 和 DEnKF; 同时, 将采用真实量测噪声和过程噪声的 EKF 和 UKF 实现方式分别记为 DEKF-TN 和 DUKF-TN.
4.1.1 场景设置
在$ k $时刻目标状态表示为${\boldsymbol{x}}_k = [x_k \;\;\;y_k\;\;\; \dot{x}_k\;\;\; \dot{y}_k \;\;\; \omega_k^a ]^{\rm T}$, 其中, $\left[ x_{k}\;\;\; y_k\right]^{\rm T}$和 $\left[\dot{x}_k \;\;\;\dot{y}_k \right]^{\rm T}$分别表示$ k $时刻目标在水平方向和竖直方向的位置与速度, $ \omega_k^a= -0.052\;{\rm{ rad/s }} $表示转弯率. 相应地, 线性化后的状态转移矩阵$ {\boldsymbol{F}}_k $表示为
$$ {\boldsymbol{F}}_k = \left[\begin{array}{ccccc} 1 & 0 & \dfrac{\sin(\omega_k^aT)}{\omega_k^a} & -\dfrac{1-\cos(\omega_k^aT)}{\omega_k^a} & 0 \\ 0 & 1 & \dfrac{1-\cos(\omega_k^aT)}{\omega_k^a} & \dfrac{\sin(\omega_k^aT)}{\omega_k^a} & 0\\ 0 & 0 & \cos(\omega_k^aT) & -\sin(\omega_k^aT) & 0\\ 0 & 0 & \sin(\omega_k^aT) & \cos(\omega_k^aT) & 0\\ 0 & 0 & 0 & 0 & 1 \end{array}\right]\nonumber $$ 其中, $ T=1 $表示采样周期. 过程噪声$ {\boldsymbol{\omega}}_k $随时间缓慢变化且先验知识不准确, 假设系统噪声协方差的变化满足方程
$$ {\boldsymbol{Q}}_k = \left(6.5 + 0.5\cos\left(\frac{\pi k}{t_s}\right)\right) {\rm blkdiag}\left\{q_1{\bar{{{\boldsymbol{Q}}}}}_k, q_2T\right\} $$ 其中, $q_1=0.3 \;{\rm{m}}^2/{\rm{s}}^2$和$q_2=1\times 10^{-4}\; {\rm{m}}^2/ {\rm{s}}^2$分别为噪声方差的参数, $ t_s $表示仿真的总时长, ${\bar{{{\boldsymbol{Q}}}}}_k = $ $\left[\begin{aligned} \frac{T^{3}}{3}{{{\boldsymbol{I}}}}_{2} \;\; \frac{T^{2}}{2}{{{\boldsymbol{I}}}}_{2} \\ \frac{T^{2}}{2}{{{\boldsymbol{I}}}}_{2} \;\;\; T{{{\boldsymbol{I}}}}_{2}\; \end{aligned}\right]$, $ {{{\boldsymbol{I}}}}_{2} $表示$ 2 $维单位阵. 初始估计值和其误差协方差分别为${\boldsymbol{x}}_{0|0} = [2\;100\; {\rm{m}}, 0\; {\rm{m/s}}, 1\;800\; {\rm{m}}, 30 \;{\rm{m/s}}, -\pi/50\; {\rm{rad/s}}]$ 和 ${\boldsymbol{P}}_{0|0}\;=\;\rm {diag}\{150,\, 10, \,150, 10, 0.0001\}$. 直角坐标系内三个雷达的位置分别为$(0 \;{\rm{m}}, 0 \; {\rm{m}})$、$(500 \; {\rm{m}}, 0\;{\rm{m}})$和$(0\;{\rm{m}}, 500\; {\rm{m}})$, 其量测方程表示为
$$ {{{\boldsymbol{z}}}}_k = \left[({{{\boldsymbol{z}}}}_k^1)^{\rm T}, ({{{\boldsymbol{z}}}}_k^2)^{\rm T}, ({{{\boldsymbol{z}}}}_k^3)^{\rm T}\right]^{\rm T}\qquad\qquad\qquad $$ $$ {{{\boldsymbol{z}}}}_k^s = \left[\begin{array}{c} \sqrt{(x_{k}-x_{k}^s)^2+(y_k-y_k^s)^2} \\ \arctan\left(\dfrac{y_k-y_k^s}{x_{k}-x_{k}^s}\right) \end{array}\right] + {\boldsymbol\upsilon}_{k}^{s} $$ 其中, $ x_k^s $和$ y_k^s $分别表示$ k $时刻传感器$ s $在水平方向和竖直方向的位置, 且$ s\in(1, 2, 3) $. 三个传感器量测随机丢失的概率相同且服从参数为$ p = 0.1 $的伯努利分布, 相应量测噪声$ {\boldsymbol\upsilon}_{k}^{s} $及其方差$ {{{\boldsymbol{R}}}}_k^{s} $的参数取值设定为
$$ {\boldsymbol\upsilon}_{k}^{s}\sim \left\{ \begin{array}{lll} &{\rm{N}}(0, {{{\boldsymbol{R}}}}_k^{s}), & 1-p\\ &{\rm{N}}(0, 100{{{\boldsymbol{R}}}}_k^{s}), & p \end{array} \right. \qquad\qquad\;\;\; \;\;$$ $$ \left\{ \begin{array}{ll} {{{\boldsymbol{R}}}}_k^{1} = {\rm blkdiag}\left\{10^2\;{{\rm{m}}}^2, 10^{-5}\;{{\rm{rad}}}^2\right\}\\ {{{\boldsymbol{R}}}}_k^{2} = {\rm blkdiag}\left\{13^2\;{{\rm{m}}}^2, 1.2\times 10^{-5}\;{{\rm{rad}}}^2\right\}\\ {{{\boldsymbol{R}}}}_k^{3} = {\rm blkdiag}\left\{16^2\;{{\rm{m}}}^2, 1.4\times 10^{-5}\;{{\rm{rad}}}^2\right\} \end{array} \right. $$ 仿真实验中, 迭代次数$ N_{\rm iter} = 5 $, 传感器扫描周期$ T=1 $ s, 其余仿真实验参数取值如表1所示.
表 1 仿真参数Table 1 Simulation parameters参数 参数值 $ \alpha_0 $ 1 $ \beta_0 $ 2 $ t_s $ 100 $ {\tau} $ 10 $ v $ 5 $ n $ 4 4.1.2 结果与分析
图2 和图3 分别给出基于传感器 1 量测的状态预测误差协方差对角线上位置分量和速度分量的估计值, 从图2和图3可以看出, 基于本文建模方法的3种滤波算法均能对预测误差协方差进行有效估计, 避免了因过程噪声先验建模不准对状态估计精度造成的不利影响. 同时, VBAKF-NG 在更新阶段采用自然梯度优化获得更高精度的估计和相应的误差协方差, 并作为下一时刻的先验信息, 因而其预测误差协方差最小, 并且更有利于在时序上形成一个“当前时刻高精度估计$ \rightleftarrows $下一时刻高精度预测”的良性循环.
图 2 传感器1 位置状态预测误差协方差的估计值Fig. 2 The expectation of the position state prediction error covariance from Sensor 1
图 3 传感器1 速度状态预测误差协方差的估计值Fig. 3 The expectation of the velocity state prediction error covariance from Sensor 1图4 ~ 7 分别给出传感器 2 和传感器 3 的状态预测误差协方差对角线上位置量和速度量的估计值, 可以看出, 基于传感器 2 和传感器 3 量测的3种算法对状态预测误差协方差估计的曲线与传感器 1 相似, 即VBAKF-NG 预测误差协方差位置分量和速度分量的估计值曲线始终处于最优的位置且更加平滑, 体现出相对于 EKF-VB 和 UKF-VB 能够获取更高的估计精度和鲁棒性.
图 4 传感器2 位置状态预测误差协方差的估计值Fig. 4 The expectation of the position state prediction error covariance from Sensor 2
图 7 传感器3 速度状态预测误差协方差的估计值Fig. 7 The expectation of the velocity state prediction error covariance from Sensor 3
图 5 传感器2 速度状态预测误差协方差的估计值Fig. 5 The expectation of the velocity state prediction error covariance from Sensor 2
图 6 传感器3 位置状态预测误差协方差的估计值Fig. 6 The expectation of the position state prediction error covariance from Sensor 3图8 给出传感器 1 在径向距和方位角上的量测噪声方差. 图8中黑色跳变表示该传感器此时出现量测丢失情况, 从图8中可以看出, UKF-VB 和 VBAKF-NG 能够准确地识别出量测噪声方差的跳变, 以便及时调整分布参数更新. 同时, 从图8中可以清晰地看出, VBAKF-NG 对噪声方差跳变的识别精度更高. 图9 和图10 分别给出传感器 2 和传感器 3 在径向距和方位角上的量测噪声方差. 结果同样表明与 UKF-VB 和 EKF-VB 相比, VBAKF-NG 对噪声方差跳变具有更高的识别精度. 实际上, 图2 ~ 7 中 VBAKF-NG 之所以在滤波精度和鲁棒性上优于 EKF-VB和UKF-VB, 其原因正是由于 VBAKF-NG 可以有效辨识量测随机缺失现象产生, 从而使得算法具有较好的容错特性.
图 8 传感器1 量测噪声方差的估计值Fig. 8 The expectation of the measurement noise variance from Sensor 1
图 9 传感器2 量测噪声方差的估计值Fig. 9 The expectation of the measurement noise variance from Sensor 2
图 10 传感器3 量测噪声方差的估计值Fig. 10 The expectation of the measurement noise variance from Sensor 3图11 和图12 分别给出上述6种方法状态估计的径向距均方根误差 (Root mean squared error, RMSE)和径向速度均方根误差的对比, 从图11和图12中可以看出, 采用IW分布和学生$ t $分布分别建模预测误差协方差和量测噪声的 DEKF-VB, DUKF-VB 和 DVBAKF-NG 三种方法均优于传统自适应滤波算法 DUKF-FN 和集合卡尔曼滤波算法 DEnKF. 同时, 由于 DVBAKF-NG 在更新过程中采用自然梯度的方法更新目标状态, 与 DEKF-VB, DUKF-VB 相比, 取得了相对更优的估计精度. 需要说明的是, DUKF-TN 在滤波过程中采用真实的过程噪声, 因此具有最高的精度.
4.2 毫米波雷达数据验证
实验场景为采用 ARS408-21 毫米波雷达对地面沿直线运动的行人目标进行跟踪. 在量测过程中考虑量测噪声非高斯特点, 行人分别在 17 时刻和 27 时刻被遮挡, 导致出现两次量测丢失. 同时考虑系统噪声方差的未知时变情况, 行人的速度大小不断变化. 在不考虑杂波和数据关联的情况下, 假设一个时刻返回一个量测, 量测数据和滤波结果如图13所示.
需要说明的是, 由于真实系统噪声未知, 在此实验中采用真实系统噪声方差的 UKF-TN 算法未参与对比. 从图13 中可以看出, 由于系统噪声未知, 在滤波初始阶段, EnKF 和 UKF-FN 算法的估计结果与量测偏离较大, 采用IW分布建模的 EKF-VB、UKF-VB 和 VBAKF-NG 与量测偏离较小. 当发生量测缺失时, 对比算法均存在不同程度的发散现象, VBAKF-NG 能够较好地抑制滤波发散. 因此, 毫米波雷达地面行人跟踪实验验证了本文所提 VBAKF-NG 算法的优越性.
5. 结束语
针对过程噪声时变且先验信息建模不准确和量测噪声非高斯问题, 首先, 引入参数化的IW分布作为状态预测误差协方差矩阵的共轭分布建立先验信息模型, 同时构建参数化学生$ t $分布模型以近似由于量测随机丢失造成的非高斯量测似然函数; 继而, 结合平均场理论近似解耦状态和多参数的联合概率密度函数以获取相应的边缘变分分布, 并采用坐标上升的迭代方法更新状态和各参数的边缘变分分布; 在此基础上, 结合 Fisher 信息矩阵推导给出置信下界关于状态估计及其误差协方差的自然梯度, 进而给出一种基于自然梯度的噪声自适应变分贝叶斯滤波算法, 实现过程噪声时变和由于量测随机缺失导致的量测噪声非高斯条件下非线性系统的高精度估计. 本文算法处理的对象局限在单目标跟踪系统, 如何将其拓展到机动多目标跟踪系统, 在变分贝叶斯框架下, 结合自然梯度策略有效地解决模型自适应选取以及多目标跟踪中数据关联问题是下一步将开展的工作.
-
图 1 伽马分布参数对量测似然函数的影响示意图
Fig. 1 The diagram of the influence of Gamma distribution parameters on likelihood
图 2 传感器1 位置状态预测误差协方差的估计值
Fig. 2 The expectation of the position state prediction error covariance from Sensor 1
图 3 传感器1 速度状态预测误差协方差的估计值
Fig. 3 The expectation of the velocity state prediction error covariance from Sensor 1
图 4 传感器2 位置状态预测误差协方差的估计值
Fig. 4 The expectation of the position state prediction error covariance from Sensor 2
图 7 传感器3 速度状态预测误差协方差的估计值
Fig. 7 The expectation of the velocity state prediction error covariance from Sensor 3
图 5 传感器2 速度状态预测误差协方差的估计值
Fig. 5 The expectation of the velocity state prediction error covariance from Sensor 2
图 6 传感器3 位置状态预测误差协方差的估计值
Fig. 6 The expectation of the position state prediction error covariance from Sensor 3
图 8 传感器1 量测噪声方差的估计值
Fig. 8 The expectation of the measurement noise variance from Sensor 1
图 9 传感器2 量测噪声方差的估计值
Fig. 9 The expectation of the measurement noise variance from Sensor 2
图 10 传感器3 量测噪声方差的估计值
Fig. 10 The expectation of the measurement noise variance from Sensor 3
表 1 仿真参数
Table 1 Simulation parameters
参数 参数值 $ \alpha_0 $ 1 $ \beta_0 $ 2 $ t_s $ 100 $ {\tau} $ 10 $ v $ 5 $ n $ 4 
下载: 导出CSV
-
[1] Shnitzer T, Talmon R, Slotine J J. Diffusion maps Kalman filter for a class of systems with gradient flows. IEEE Transactions on Signal Processing, 2020, 68: 2739-2753 doi: 10.1109/TSP.2020.2987750 [2] 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 [3] Copp B, Subbarao K. Nonlinear adaptive filtering in terrain-referenced navigation. IEEE Transactions on Aerospace and Electronic Systems, 2015, 51(4): 3461-3469 doi: 10.1109/TAES.2015.140826 [4] 潘泉, 胡玉梅, 兰华, 孙帅, 王增福, 杨峰. 信息融合理论研究进展: 基于变分贝叶斯的联合优化. 自动化学报, 2019, 45(7): 1207-1223Pan Quan, Hu Yu-Mei, Lan Hua, Sun Shuai, Wang Zeng-Fu, Yang Feng. Information fusion progress: Joint optimization based on variational Bayesian theory. Acta Automatica Sinica, 2019, 45(7): 1207-1223 [5] Huang Y L, Zhang Y G, Zhao Y X, Chambers J A. A novel robust Gaussian-Student's t mixture distribution based Kalman filter. IEEE Transactions on Signal Processing, 2019, 67(13): 3606-3620 doi: 10.1109/TSP.2019.2916755 [6] Särkkä S, Nummenmaa A. Recursive noise adaptive Kalman filtering by variational Bayesian approximations. IEEE Transactions on Automatic Control, 2009, 54(3): 596-600 doi: 10.1109/TAC.2008.2008348 [7] Särkkä S, Hartikainen J. Non-linear noise adaptive Kalman filtering via variational Bayes. In: Proceedings of the IEEE International Workshop on Machine Learning for Signal Processing. Southampton, UK: IEEE, 2013. 1−6 [8] 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 [9] Li W L, Sun S H, Jia Y M, Du J P. Robust unscented Kalman filter with adaptation of process and measurement noise covariances. Digital Signal Processing, 2016, 48: 93-103 doi: 10.1016/j.dsp.2015.09.004 [10] Dong P, Jing Z L, Leung H, Shen K, Li M Z. The labeled multi-Bernoulli filter for multitarget tracking with glint noise. IEEE Transactions on Aerospace and Electronic Systems, 2019, 55(5): 2253-2268 doi: 10.1109/TAES.2018.2884183 [11] Zhang W Y, Liang Y, Yang F, Xu L F. A robust Student＇s t-based labeled multi-Bernoulli filter. In: Proceedings of the 22th International Conference on Information Fusion. Ottawa, Canada: IEEE, 2019. 1−6 [12] Ardeshiri T, Özkan E, Orguner U, Gustafsson F. Approximate Bayesian smoothing with unknown process and measurement noise covariances. IEEE Signal Processing Letters, 2015, 22(12): 2450-2454 doi: 10.1109/LSP.2015.2490543 [13] Huang Y L, Zhang Y G, Wu Z M, Li N, Chambers J. A novel adaptive Kalman filter with inaccurate process and measurement noise covariance matrices. IEEE Transactions on Automatic Control, 2018, 63(2): 594-601 doi: 10.1109/TAC.2017.2730480 [14] Turner R, Bottone S, Stanek C. Online variational approximations to non-exponential family change point models: With application to radar tracking. In: Proceedings of the 26th International Conference on Neural Information Processing Systems. Lake Tahoe, USA: Curran Associates Inc., 2013. 306−314 [15] Tronarp F, García-Fernández Á F, Särkkä S. Iterative filtering and smoothing in nonlinear and non-Gaussian systems using conditional moments. IEEE Signal Processing Letters, 2018, 25(3): 408-412 doi: 10.1109/LSP.2018.2794767 [16] Nurminen H, Ardeshiri T, Piché R, Gustafsson F. Skew- t filter and smoother with improved covariance matrix approximation. IEEE Transactions on Signal Processing, 2018, 66(21): 5618-5633 doi: 10.1109/TSP.2018.2865434 [17] Gultekin S, Paisley J. Nonlinear Kalman filtering with divergence minimization. IEEE Transactions on Signal Processing, 2017, 65(23): 6319-6331 doi: 10.1109/TSP.2017.2752729 [18] 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 IEEE International Workshop on Machine Learning for Signal Processing. Santander, Spain: IEEE, 2012. 1−6 [19] 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 [20] Huang Y L, Zhang Y G, Li N, Wu Z M, Chambers J A. A novel robust Student's t-based Kalman filter. IEEE Transactions on Aerospace and Electronic Systems, 2017, 53(3): 1545-1554 doi: 10.1109/TAES.2017.2651684 [21] 胡振涛, 杨诗博, 胡玉梅, 周林, 金勇, 杨琳琳. 基于变分贝叶斯的分布式融合目标跟踪. 电子学报, 2022, 50(5): 1058-1065Hu Zhen-Tao, Yang Shi-Bo, Hu Yu-Mei, Zhou Lin, Jin Yong, Yang Lin-Lin. Distributed fusion target tracking based on variational Bayes. Acta Electronica Sinica, 2022, 50(5): 1058-1065 [22] Brooks S. Markov chain Monte Carlo method and its application. Journal of the Royal Statistical Society: Series D (The Statistician), 1998, 47(1): 69-100 [23] Geyer C J. Practical Markov chain Monte Carlo. Statistical Science, 1992, 7(4): 473-483 [24] Salimans T, Kingma D P, Welling M. Markov chain Monte Carlo and variational inference: Bridging the gap. In: Proceedings of the 32nd International Conference on International Conference on Machine Learning. Lille, France: JMLR.org, 2015. 1218−1226 [25] Humpherys J, West J. Kalman filtering with Newton's method[Lecture Notes]. IEEE Control Systems Magazine, 2010, 30(6): 101-106 doi: 10.1109/MCS.2010.938485 [26] Alessandri A, Gaggero M. Moving-horizon estimation for discrete-time linear and nonlinear systems using the gradient and Newton methods. In: Proceedings of the 55th IEEE Conference on Decision and Control. Las Vegas, USA: IEEE, 2016. 2906−2911 [27] Akyildiz Ö D, Chouzenoux É, Elvira V, Míguez J. A probabilistic incremental proximal gradient method. IEEE Signal Processing Letters, 2019, 26(8): 1257-1261 doi: 10.1109/LSP.2019.2926926 [28] Amari S I. Natural gradient works efficiently in learning. Neural Computation, 1998, 10(2): 251-276 doi: 10.1162/089976698300017746 [29] Ollivier Y. Online natural gradient as a Kalman filter. Electronic Journal of Statistics, 2018, 12(2): 2930-2961 [30] Cheng Y Q, Wang X Z, Morelande M, Moran B. Information geometry of target tracking sensor networks. Information Fusion, 2013, 14(3): 311-326 doi: 10.1016/j.inffus.2012.02.005 [31] Schmitt L, Fichter W. Globally valid posterior Cramér-Rao bound for three-dimensional bearings-only filtering. IEEE Transactions on Aerospace and Electronic Systems, 2018, 55(4): 2036-2044 期刊类型引用(3)
1. 胡玉梅，潘泉，邓豹，郭振，陈立峰. 基于自然梯度的非线性变分贝叶斯滤波算法. 自动化学报. 2025(02): 427-444 . 
本站查看2. 王毅坤. 中波广播发射中的脉冲干扰类信道噪声及其抑制方法. 电声技术. 2025(01): 106-108 . 百度学术3. 胡玉梅，潘泉，邓豹. 基于Fisher信息的传感器航迹自适应滤波算法. 航空学报. 2024(20): 104-120 . 百度学术其他类型引用(0)
-
下载:
计量
- 文章访问数: 670
- HTML全文浏览量: 189
- PDF下载量: 253
- 被引次数: 3
