-
非线性滤波即是通过观测某一非线性变化的随机变量,对这一随机变量或与之关联的其他随机变量进行状态估计,被广泛应用于雷达目标跟踪、声纳测距等领域。
粒子滤波(particle filter, PF)是一种重要的非线性递归贝叶斯滤波技术,适用于任意非线性和非高斯噪声滤波场景[1-3]。但PF也存在粒子退化、重要性密度函数(importance density function, IDF)的选取及计算复杂性等问题[1]。高斯粒子滤波[4](Gaussian particle filter, GPF)是一种重要的PF改进算法,其递归传递目标状态概率密度的均值、方差或更高阶矩,从而有效避免重采样,降低PF的计算复杂度。而最优的IDF应与当前量测之间条件相关[5],且好的IDF可有效避免粒子退化现象[3]。
近年来出现许多针对IDF选取的改进算法[6-8],这些算法均利用非线性滤波方法,如扩展卡尔曼滤波器(extended Kalman filter, EKF)、无迹卡尔曼滤波器(unscented Kalman filter, UKF)、容积卡尔曼滤波器(cubature Kalman filter,CKF)等,结合当前量测获取IDF,使得状态转移密度朝着高似然区域转移,从而有效提高滤波精度。但是,这些非线性滤波方法都是基于Kalman滤波框架,而Kalman滤波仅在线性最小均方误差准则下是最优的,这使得上述方法并不能得到准确的后验估计。文献[9]提出一种基于EKF的递推更新滤波方法,依据非线性测量函数的梯度对目标状态进行渐进式的更新,可有效克服线性最小均方误差准则的限制,实现目标状态的有效更新。但是EKF泰勒级数截断产生的线性化误差使得上述递推更新滤波方法并不准确。针对这一问题,文献[10]将递推更新思想推广至非线性高斯滤波器中,提出RUGF利用数值计算方法(如UKF、CKF等)近似高斯积分,可有效避免由EKF线性化误差引起的滤波发散问题。但在实际计算中,基于有限字长数字计算机的数值计算极易导致协方差矩阵非正定,从而出现递推中断问题。
对此,本文首先分析SR-RUGF的实现策略,并利用CKF对其进行具体实现,有效避免了递推中断问题;然后,利用SR-RUGF为GPF构建重要性密度函数,进而得到基于平方根递推更新的高斯粒子滤波算法(square-root recursive update Gaussian particle filter, SRRU-GPF)。
HTML
-
本文利用CKF对RUGF和SR-RUGF进行具体实现,同时对所提算法的计算复杂度进行对比分析。表 1给出了CKF-SR-RUGF算法单次迭代计算中求取各变量所需的计算量。
计算项 计算量 $\mathit{\boldsymbol{\chi}}_{k + 1|k}^{(i - 1)}$ $3{n^2}$ $\mathit{\boldsymbol{Z}}_{k + 1|k}^{(i - 1)}$ $4{n^2}m - 2mn$,$i = 1$ $4{n^2}m$,$i \ne 1$ ${\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)}$ $2mn$ $\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)}$ $4{m^2}n + 4mn$ $\mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)}$ $4{n^2}m + 2mn + 2{n^2}$ $\mathit{\boldsymbol{K}}_{k + 1}^{(i - 1)}$ $3{m^3} + 2{m^2}n - {m^2}$ ${\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i)}$ $2mn + m$ $\mathit{\boldsymbol{S}}_{k + 1|k + 1}^{(i)}$ $4{n^3} + 4{n^2}m + 4{n^2} + 2mn$ 其中,$i$表示递推更新的次数,$n$表示状态变量维数,$m$表示量测维数。对于维数为$l \times p$的矩阵,QR分解和Cholesky分解的计算量分别为$2l{p^2}$和。
CKF算法的单次计算量为[14]:
CKF-RUGF算法的计算量分析在文献[10]中给出,但其重复计算$\mathit{\boldsymbol{D}}_{k + 1}^{(i - 1)}$、$\boldsymbol{P}_{z, k+1 | k}^{(i-1)}+\boldsymbol{D}_{k+1}^{(i-1)}+\boldsymbol{D}_{k+1}^{(i-1) \mathrm{T}}$和$\boldsymbol{P}_{x z, k+1 | k}^{(i-1)}+\boldsymbol{C}_{k+1}^{(i-1)}$,且忽略了产生样本点所需的计算量。对此,这里将CKF-RUGF算法的计算量更正为:
根据表 1,本文CKF-SR-RUGF的计算量为:
对比式(17)~(19)可以得出,3种算法的计算复杂度由高到低依次为CKF-SR-RUGF算法、CKF-RUGF算法、CKF算法。但不难发现,本文提出的SR-RUGF算法的计算复杂度主要取决于参数$N$的选取。故在工程应用中,应选取合适的参数$N$来权衡计算量和滤波精度。
-
单变量非平稳增长模型是经典的粒子滤波仿真数据模型[4, 10, 15],其模型描述如下:
式中,$\alpha = 0.5$;$\beta = 25$;$\gamma = 8$;${w_{t - 1}} \sim {\rm{N}}(0, 1)$;${v_t} \sim {\rm{N}}(0, 0.01)$。
将CKF-SRRU-GPF、EKRU-GPF、CKF-GPF和CKRU-GPF 4种GPF算法的估计性能进行对比,其中4者的重要性密度函数分别由CKF-SR-RUGF算法、文献[9]中基于EKF的递推更新方法、CKF算法和文献[10]中的CKF-RUGF算法构建。
这里将均方根误差(RMSE)作为评价指标:
式中,${x_t}$表示$t$时刻的真实状态值,${{{\hat x}}}_t^i$表示$t$时刻第$i$次仿真的状态估计值;MC表示Monte Carlo仿真次数。真实初始状态值为${x_0} = 0.1$,并取初始估计值${{{{\hat x}}}_0} = 0$,${\hat {{P}}_0} = 1$。采样粒子数$M = 500$,递推更新次数$N = 20$,测量周期$T = 1{\rm{ }}s$,仿真次数${\rm{MC}} = 100$。
图 1和表 2分别给出CKF-SRRU-GPF、CKF- GPF、EKRU-GPF和CKRU-GPF的RMSE与RMSE均值比较。不难发现,相比于其他3种利用递推更新方法的滤波器,CKF-GPF的性能最差,这说明了递推更新方法的优越性。本文CKF-SRRU-GPF的估计性能优于EKRU-GPF,说明本文改进算法的有效性,但其性能差于CKRU-GPF,可能的原因是在SR-RUGF实现过程中(2.2节),将测量误差嵌入测量值计算各协方差矩阵,而有限次的高斯分布采样无法准确近似真实分布。
滤波器 RMSE均值 CKF-GPF 8.486 2 EKRU-GPF 5.145 2 CKF-SRRU-GPF 4.725 2 CKRU-GPF 4.296 2 表 3给出递推更新次数$N = 20$时各滤波器的单次运行时间比较。可以看出,CKF-GPF的运行时间最短,原因是其他3种滤波器均运用了递推更新方法,故运行时间较长。CKF-SRRU-GPF的运行时间最长。
滤波器 时间/s CKF-GPF 0.580 1 EKRU-GPF 0.629 0 CKF-SRRU-GPF 0.664 8 CKRU-GPF 0.639 9 图 2和表 4给出了当N=2, 5, 10, 20时,CKF-SRRU- GPF的估计性能对比。不难看出,随着N的增大,估计性能不断提升,但当N达到一定值时,估计性能几乎不再提升。故在工程应用中,应选取合适的参数$N$来权衡计算量和估计精度。
CKF-SRRU-GPF RMSE均值 N=2 9.648 0 N=5 6.044 4 N=10 5.301 4 N=20 4.808 9 -
式中:$\mathit{\boldsymbol{x}} = {[{p_x}\;\;{p_y}]^{\rm{T}}} = {[s\;\;t]^{\rm{T}}}$;${\mathit{\boldsymbol{w}}_{k - 1}} \sim {\rm{N}}(0, \mathit{\boldsymbol{Q}})$,且$\mathit{\boldsymbol{Q}} = [2\;\;0.05;0.05\;\;2]$。传感器位于$(\cos k, \sin k)$,则测量模型为:
式中,${v_k} \sim {\rm{N}}(0, R)$,且$R = 0.001$。
初始状态值设定为${\mathit{\boldsymbol{x}}_0} = {[20\;\;5]^{\rm{T}}}$,并取初始估计值为${{\mathit{\boldsymbol{\hat x}}}_0} = {[20\;\;5]^{\rm{T}}}$,${{\mathit{\boldsymbol{\hat P}}}_0} = [0.1\;\;0;0\;\;0.1]$。采样粒子数$M = 300$,递推更新次数$N = 20$,测量周期$T = 1{\rm{ }}s$,仿真次数${\rm{MC}} = 100$,并利用RMSE(式(21))作为性能评价指标。
由于CKRU-GPF容易出现协方差矩阵非正定而导致递推中断的问题,故这里只对CKF-GPF、EKRU-GPF、CKF-SRRU-GPF这3种滤波器的估计性能进行比较。
图 3、图 4分别给出状态$s$和$t$的RMSE比较。不难发现,CKF-SRRU-GPF的估计性能优于其他两种算法,再次说明本文改进算法的有效性。值得一提的是,EKRU-GPF在较大部分时刻估计性能优于CKF-GPF,但也存在极大异常估计值的情况,可能的原因是强非线性时EKF的线性化计算引入了较大的近似误差,从而导致异常估计值。
表 5给出递推更新次数$N = 20$时各滤波器的单次运行时间对比。同样,由于利用递推更新方法,CKF-SRRU-GPF和EKRU-GPF的运行时间高于CKF-GPF。CKF-SRRU-GPF的运行时间最长,但其估计性能最好。
滤波器 时间/s CKF-GPF 0.865 0 EKRU-GPF 0.876 3 CKF-SRRU-GPF 1.015 5 图 5、图 6和表 6给出当N=2, 5, 10, 20时,CKF-SRRU-GPF对状态$s$和$t$的RMSE及RMSE均值对比。可以看出,当N=2时CKF-SRRU-GPF估计性能很不理想,存在较多异常估计值。与4.1节结果相同,随着N的增大,其性能不断提升,但当N达到一定值时,估计性能几乎不再提升。因此,实际工程应用时应同时考虑计算量和所需精度来选取参数N。
CKF-SRRU-GPF $s$RMSE均值 $t$RMSE均值 N=2 8.627 3 11.471 8 N=5 1.146 5 6.034 9 N=10 1.089 4 5.556 1 N=20 0.932 7 4.872 6