ISSN 
-
非线性滤波即是通过观测某一非线性变化的随机变量，对这一随机变量或与之关联的其他随机变量进行状态估计，被广泛应用于雷达目标跟踪、声纳测距等领域。
粒子滤波(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)。
-
GPF假设先验和后验概率密度均为高斯分布，利用Monte Carlo采样递归传递它们的均值和方差，从而避免重采样，可有效降低PF的计算复杂度。此外，利用采样技术可容易获得概率分布的更高阶矩，故GPF同样适用于非高斯系统。
假设非线性高斯状态方程和测量方程分别为：
$$f\left(\boldsymbol{x}_{k+1} | \boldsymbol{x}_{k}\right)=N\left(\boldsymbol{x}_{k+1} ; \varphi\left(\boldsymbol{x}_{k}\right), \boldsymbol{Q}_{k}\right)$$ (1) $$g\left(\mathit{\boldsymbol{z}}_{k+1} | \boldsymbol{x}_{k+1}\right)=N\left(\mathit{\boldsymbol{z}}_{k+1} ; h\left(\boldsymbol{x}_{k+1}\right), \boldsymbol{R}_{k+1}\right)$$ (2) 式中，$N( \cdot {\rm{ }};m, P)$表示均值为$m$，方差为$P$的高斯分布；$\varphi ( \cdot )$和$h( \cdot )$分别为非线性状态转移函数和测量函数；$\boldsymbol{Q}_{k}$和$\boldsymbol{R}_{k+1}$分别为过程噪声和测量噪声的协方差矩阵。
假设$k$时刻后验概率密度函数为：
$$p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1 : k}\right)=N\left(\boldsymbol{x}_{k} ; \boldsymbol{m}_{k}, \boldsymbol{P}_{k}\right)$$ (3) 1) 预测
首先，将后验概率密度$p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{\mathrm{l}, k}\right)$、状态转移密度$f\left(\boldsymbol{x}_{k+1} | \boldsymbol{x}_{k}\right)$带入Bayesian预测方程[11]，可得：
$$p\left(\boldsymbol{x}_{k+1} | \boldsymbol{z}_{1 : k}\right)=\int N\left(\boldsymbol{x}_{k+1} ; \varphi\left(\boldsymbol{x}_{k}\right), \boldsymbol{Q}_{k}\right) N\left(\boldsymbol{x}_{k} ; \boldsymbol{m}_{k}, \boldsymbol{P}_{k}\right) \mathrm{d} \boldsymbol{x}_{k}$$ (4) 然后从高斯量$N\left(\cdot ; \boldsymbol{m}_{k}, \boldsymbol{P}_{k}\right)$采样$M$个粒子$\boldsymbol{x}_{k}^{i}$，$i = 1, 2, \cdots , M$，则预测方程(4)可近似为：
$$\frac{1}{M}\sum\limits_{i = 1}^M f \left( {{\mathit{\boldsymbol{x}}_{k + 1}}|\mathit{\boldsymbol{x}}_k^{(i)}} \right) $$ (5) 最后由$f\left(\boldsymbol{x}_{k+1} | \boldsymbol{x}_{k}^{(i)}\right)$计算$\boldsymbol{x}_{k+1 | k}^{(i)}$，即可得到$p\left(\boldsymbol{x}_{k+1} | \boldsymbol{z}_{1 : k}\right)$，其均值和协方差分别为：
$$\boldsymbol{m}_{k+1 | k}=\frac{1}{M} \sum\limits_{i=1}^{M} \boldsymbol{x}_{k+1 | k}^{(i)} $$ (6) $$\boldsymbol{P}_{k+1 | k}=\frac{1}{M} \sum\limits_{i=1}^{M}\left(\boldsymbol{m}_{k+1 | k}-\boldsymbol{x}_{k+1 k}^{(i)}\right)\left(\boldsymbol{m}_{k+1 k}-\boldsymbol{x}_{k+1 k}^{(i)}\right)^{\mathrm{T}} $$ (7) 2) 更新
将预测概率密度$p\left(\boldsymbol{x}_{k+1} | \boldsymbol{z}_{1 : k}\right)$与测量密度$g\left(\mathit{\boldsymbol{z}}_{k+1} | \boldsymbol{x}_{k+1}\right)$带入Bayesian更新方程[11]，可得：
$$\begin{array}{c}{p\left(\boldsymbol{x}_{k+1} | \boldsymbol{z}_{1: k+1}\right)=} \\ {\mathrm{CN}\left(\boldsymbol{z}_{k+1} ; h\left(\boldsymbol{x}_{k+1}\right), \boldsymbol{R}_{k+1}\right) N\left(\boldsymbol{x}_{k+1} ; \boldsymbol{m}_{k+1 |k}, \boldsymbol{P}_{k+1| k}\right)}\end{array}$$ (8) 其中
$$C=\int N\left(\boldsymbol{z}_{k+1} ; h\left(\boldsymbol{x}_{k+1}\right), \boldsymbol{R}_{k+1}\right) N\left(\boldsymbol{x}_{k+1} ; \boldsymbol{m}_{k+1 | k}, \boldsymbol{P}_{k+1 | k}\right) \mathrm{d} \boldsymbol{x}_{k+1}$$ (9) 基于量测$\mathit{\boldsymbol{z}}$及预测量$N\left(\cdot ; \boldsymbol{m}_{k+1 | k}, \boldsymbol{P}_{k+1 | k}\right)$，构建重要性密度函数${\pi _{k + 1}}\left( { \cdot |{\mathit{\boldsymbol{z}}_{1:k + 1}}} \right)$，并采样得到粒子$\boldsymbol{x}_{k+1}^{(i)}$，$i = 1, 2, \cdots , M$。进而计算每个粒子的权重为：
$$\bar w_{k + 1}^{(i)} = \frac{{N\left( {{\mathit{\boldsymbol{z}}_{k + 1}};h\left( {\mathit{\boldsymbol{x}}_{k + 1}^{(i)}} \right), {\mathit{\boldsymbol{R}}_{k + 1}}} \right)N\left( {\mathit{\boldsymbol{x}}_{k + 1}^{(i)};{\mathit{\boldsymbol{m}}_{k + 1|k}}, {\mathit{\boldsymbol{P}}_{k + 1|k}}} \right)}}{{{\pi _{k + 1}}\left( {\mathit{\boldsymbol{x}}_{k + 1}^{(i)}|{\mathit{\boldsymbol{z}}_{1:k + 1}}} \right)}}$$ (10) 权重归一化为：
$$w_{k + 1}^{(i)} = {{\bar w_{k + 1}^{(i)}} \mathord{\left/ {\vphantom {{\bar w_{k + 1}^{(i)}} {\sum\limits_{i = 1}^M {\bar w_{k + 1}^{(i)}} }}} \right. } {\sum\limits_{i = 1}^M {\bar w_{k + 1}^{(i)}} }}$$ (11) 则可近似得到$p\left(\boldsymbol{x}_{k+1} | \boldsymbol{z}_{1 : k+1}\right)$，其均值和协方差分别为：
$$\boldsymbol{m}_{k+1}=\sum\limits_{i=1}^{M} \boldsymbol{w}_{k+1}^{(i)} \boldsymbol{x}_{k+1}^{(i)} $$ (12) $$\boldsymbol{P}_{k+1}=\sum\limits_{i=1}^{M} \sum\limits_{i=1}^{M} w_{k+1}^{(i)}\left(\boldsymbol{m}_{k+1}-\boldsymbol{x}_{k+1}^{(i)}\right)\left(\boldsymbol{m}_{k+1}-\boldsymbol{x}_{k+1}^{(i)}\right)^{\mathrm{T}} $$ (13) -
RUGF的每次迭代计算均是依据测量函数梯度对目标状态的一次微小的更新，这使得算法的线性化假设总是合理的。RUGF可克服线性最小均方误差准则的限制且很好地利用非线性量测信息，实现对目标状态的有效更新。
首先定义：
$$\boldsymbol{C}_{k+1}=\mathrm{E}\left[\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1 | k}\right) \boldsymbol{V}_{k+1}^{\mathrm{T}}\right]$$ (14) $$\boldsymbol{D}_{k+1}=\mathrm{E}\left[\left(h\left(\boldsymbol{x}_{k+1}\right)-\hat{\boldsymbol{z}}_{k+1 | k}\right) \boldsymbol{V}_{k+1}^{\mathrm{T}}\right]$$ (15) 式中，$\boldsymbol{V}_{k+1}$为测量噪声。Kalman滤波器假设测量噪声与目标状态之间是相互独立的，从而得到式(14)和(15)的值均为零。但在RUGF中二者均不为0，参与递推更新过程，RUGF具体实现步骤如下：
已知$\hat{\boldsymbol{x}}_{k+1 | k}$和$\boldsymbol{P}_{k+1 | k}$，量测${\mathit{\boldsymbol{z}}_{k + 1}}$；
1) 初始化
设$\boldsymbol{C}_{k+1}^{(0)}=0, \quad \hat{\boldsymbol{x}}_{k+1 | k+1}^{(0)}=\hat{\boldsymbol{x}}_{k+1 k}, \quad \boldsymbol{P}_{k+1 | k+1}^{(0)}=\boldsymbol{P}_{k+1 | k}$
2) 递推更新
for $ i = 1:N$
$$\mathit{\boldsymbol{\hat z}}_{k + 1|k}^{(i - 1)} = \int h \left( {{\mathit{\boldsymbol{x}}_{k + 1}}} \right)N\left( {{\mathit{\boldsymbol{x}}_{k + 1}};\mathit{\boldsymbol{\widehat x}}_{k + 1|k + 1}^{(i - 1)}, \mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(i - 1)}} \right){\rm{d}}{\mathit{\boldsymbol{x}}_{k + 1}}$$ $$\begin{array}{l} \mathit{\boldsymbol{P}}_{z,k + 1|k}^{(i - 1)} = \\ \int {h({\mathit{\boldsymbol{x}}_{k + 1}})} {h^T}({\mathit{\boldsymbol{x}}_{k + 1}})N({\mathit{\boldsymbol{x}}_{k + 1}};\mathit{\boldsymbol{\hat x}}_{k + 1|k + 1}^{(i - 1)},\mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(i - 1)}){\rm{d}}{\mathit{\boldsymbol{x}}_{k + 1}} - \\ \mathit{\boldsymbol{\hat z}}_{k + 1|k}^{(i - 1)}\mathit{\boldsymbol{\hat z}}_{k + 1|k}^{{{(i - 1)}^{\rm{T}}}} + {\mathit{\boldsymbol{R}}_{k + 1}} \end{array} $$ $$\begin{array}{l} \mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)} = \\ \int {{\mathit{\boldsymbol{x}}_{k + 1}}} {h^T}({\mathit{\boldsymbol{x}}_{k + 1}})N({\mathit{\boldsymbol{x}}_{k + 1}};{\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)}, \mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(i - 1)}){\rm{d}}{\mathit{\boldsymbol{x}}_{k + 1}} - \\ {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)}{\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)} \\ \end{array}$$ $$\mathit{\boldsymbol{H}}_{k + 1}^{(i - 1)} = \frac{{\partial h}}{{\partial x}}\left| {x = } \right.{\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)}$$ $$\mathit{\boldsymbol{D}}_{k + 1}^{(i - 1)} = \mathit{\boldsymbol{H}}_{k + 1}^{(i - 1)}\mathit{\boldsymbol{C}}_{k + 1}^{(i - 1)}$$ $$\begin{array}{l} \mathit{\boldsymbol{K}}_{k + 1}^{(i - 1)} = \frac{1}{{N - i + 1}}(\mathit{\boldsymbol{P}}_{xz,k + 1|k}^{(i - 1)} + \mathit{\boldsymbol{C}}_{k + 1}^{(i - 1)}) \times \\ {(\mathit{\boldsymbol{P}}_{z,k + 1|k}^{(i - 1)} + \mathit{\boldsymbol{D}}_{k + 1}^{(i - 1)} + \mathit{\boldsymbol{D}}_{k + 1}^{{{(i - 1)}^{\rm{T}}}})^{ - 1}} \end{array}$$ $${\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i)} = {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)} + K_{k + 1}^{(i - 1)}({\mathit{\boldsymbol{z}}_{k + 1}} - {\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)})$$ $$\begin{array}{l} \mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(i)} = \mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(i - 1)} - (\mathit{\boldsymbol{P}}_{xz,k + 1|k}^{(i - 1)} + \mathit{\boldsymbol{C}}_{k + 1}^{(i - 1)})\mathit{\boldsymbol{K}}_{k + 1}^{{{(i - 1)}^{\rm{T}}}} + \\ + \mathit{\boldsymbol{K}}_{k + 1}^{(i - 1)}(\mathit{\boldsymbol{P}}_{z,k + 1|k}^{(i - 1)} + \mathit{\boldsymbol{D}}_{k + 1}^{(i - 1)} + \mathit{\boldsymbol{D}}_{k + 1}^{{{(i - 1)}^{\rm{T}}}})\mathit{\boldsymbol{K}}_{k + 1}^{{{(i - 1)}^{\rm{T}}}} - \\ \mathit{\boldsymbol{K}}_{k + 1}^{(i - 1)}{(\mathit{\boldsymbol{P}}_{xz,k + 1|k}^{(i - 1)} + \mathit{\boldsymbol{C}}_{k + 1}^{(i - 1)})^{\rm{T}}} \end{array} $$ $$\mathit{\boldsymbol{C}}_{k + 1}^{(i)} = (I - K_{k + 1}^{(i - 1)}\mathit{\boldsymbol{H}}_{k + 1}^{(i - 1)})\mathit{\boldsymbol{C}}_{k + 1}^{(i - 1)} - K_{k + 1}^{(i - 1)}{R_{k + 1}}$$ end for
3) $\hat{\boldsymbol{x}}_{k+1 | k+1}=\hat{\boldsymbol{x}}_{k+\| k+1}^{(N)}, \quad \boldsymbol{P}_{k+1 | k+1}=\boldsymbol{P}_{k+1 | k+1}^{(N)}$
其中，$N$代表递推次数，$\boldsymbol{H}_{k+1}^{(i-1)}$表示雅可比矩阵，$\mathit{\boldsymbol{R}}$为测量噪声协方差。由于$\boldsymbol{D}_{k+1}^{(i-1)}$与$\boldsymbol{D}_{k+1}^{(i)}$之间无法建立直接意义上的解析式，故这里利用$\boldsymbol{H}_{k+1}^{(i-1)}$对其进行近似计算。
但在利用UKF、CKF等数值计算方法基于有限字长数字计算机近似高斯积分时，极易导致协方差矩阵$\boldsymbol{P}_{k+1 | k+1}^{(i)}$非正定而使得递推过程中断。针对这一问题，本文对RUGF的平方根实现方法进行研究。
-
由2.1节RUGF的实现步骤可知，RUGF的本质是：1)假设目标状态$\boldsymbol{x}_{k+1}$与测量噪声之间是相关的，即二者的互协方差不为0；2)将当前量测${\mathit{\boldsymbol{z}}_{k + 1}}$分为$N$等份，对目标状态预测进行渐进式的更新。
基于上述两点，并结合平方根实现思想[12-13]，可得SR-RUGF的实现方法。下面给出基于CKF的SR-RUGF(CKF-SR-RUGF)的实现步骤。
已知${{\mathit{\boldsymbol{\hat x}}}_{k + 1|k}}$和${\mathit{\boldsymbol{P}}_{k + 1|k}}$，量测${\mathit{\boldsymbol{z}}_{k + 1}}$：
1) 初始化
设${\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(0)} = {{\mathit{\boldsymbol{\hat x}}}_{k + 1|k}}$，$\mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(0)} = {\mathit{\boldsymbol{P}}_{k + 1|k}}$，
$$\mathit{\boldsymbol{S}}_{k + 1|k + 1}^{(0)} = {\rm{chol(}}\mathit{\boldsymbol{P}}_{k + 1|k + 1}^{(0)}{{\rm{)}}^{\rm{T}}}$$ 2) 递推更新
for $ i = 1:N$
$$\mathit{\boldsymbol{\chi}}_{j, k + 1|k}^{(i - 1)} = {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)} + \mathit{\boldsymbol{S}}_{k + 1|k + 1}^{(i - 1)}{\mathit{\boldsymbol{\xi}}_j}, j = 1, 2, \cdots , m$$ if $i = 1 $
$$\mathit{\boldsymbol{Z}}_{k + 1|k}^{(i - 1)} = h(\mathit{\boldsymbol{\chi}}_{k + 1|k}^{(i - 1)})$$ else
$$\mathit{\boldsymbol{Z}}_{k + 1|k}^{(i - 1)} = h(\mathit{\boldsymbol{\chi}}_{k + 1|k}^{(i - 1)}) + {V_{k + 1}}$$ end
$${\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)} = \frac{1}{m}\sum\nolimits_{j = 1}^m {h(\mathit{\boldsymbol{\chi}}_{k + 1|k}^{(i - 1)})} $$ $$\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)} = {\rm{qr}}\left\{ {\frac{1}{{\sqrt m }}(\mathit{\boldsymbol{Z}}_{1:m, k + 1|k}^{(i - 1)} - {\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)})} \right\}$$ $$\mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)} = \frac{1}{m}\sum\limits_{j = 1}^m {\left[ {\mathit{\boldsymbol{\chi}}_{j, k + 1|k}^{(i - 1)} - {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)}} \right]{{\left[ {\mathit{\boldsymbol{Z}}_{j, k + 1|k}^{(i - 1)} - {\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)}} \right]}^T}} $$ $$K_{k + 1}^{(i - 1)} = \frac{1}{{N - i + 1}}{{{{\mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)}} \mathord{\left/ {\vphantom {{\mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)}} {\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)\;\;T}}}} \right. } {\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)\;\;T}}}} \mathord{\left/ {\vphantom {{{{\mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)}} \mathord{\left/ {\vphantom {{\mathit{\boldsymbol{P}}_{xz, k + 1|k}^{(i - 1)}} {\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)\;\;T}}}} \right. } {\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)\;\;T}}}} {\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)}}}} \right. } {\mathit{\boldsymbol{S}}_{z, k + 1|k}^{(i - 1)}}}$$ $${\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i)} = {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)} + K_{k + 1}^{(i - 1)}({\mathit{\boldsymbol{z}}_{k + 1}} - {\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)})$$ $$\begin{gathered} \mathit{\boldsymbol{S}}_{k + 1|k + 1}^{(i)} = {\rm{qr}}\left\{ {\frac{1}{{\sqrt m }}\left[ {(\mathit{\boldsymbol{\chi}}_{1:m, k + 1|k}^{(i - 1)} - {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(i - 1)}) - } \right.} \right. \\ \left. {\left. {{\rm{ }}K_{k + 1}^{(i - 1)}(\mathit{\boldsymbol{Z}}_{1:m, k + 1|k}^{(i - 1)} - {\mathit{\boldsymbol{\hat z}}}_{k + 1|k}^{(i - 1)})} \right]} \right\} \\ \end{gathered} $$ end for
3) ${{\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}} = {\mathit{\boldsymbol{\hat x}}}_{k + 1|k + 1}^{(N)}$，${\mathit{\boldsymbol{P}}_{k + 1|k + 1}} = \mathit{\boldsymbol{S}}_{k + 1|k + 1}^{(N)}\mathit{\boldsymbol{S}}_{k + 1|k + 1}^{(N)\;\;T}$
其中，$m = 2{n_x}$，${n_x}$为状态向量的维数。${\rm{chol}}$(A)为返回矩阵$A$的Cholesky上三角矩阵；${\rm{qr}}\{ \mathit{\boldsymbol{A}}\} $表示先对矩阵${\mathit{\boldsymbol{A}}^T}$进行QR分解，得到正交矩阵Q和上三角矩阵R，然后只返回R的上三角部分的转置矩阵。另外：
$${\mathit{\boldsymbol{\xi}}_i} = \sqrt {{m \mathord{\left/ {\vphantom {m 2}} \right. } 2}} \cdot {[1]_i}$$ (16) 式中，${[1]_i}$的具体表达式见文献[13]。
-
对于粒子滤波来说，选取一个好的重要性密度函数至关重要，可有效避免粒子退化现象。递推更新方法能够有效实现非线性量测对目标状态的有效更新，并且获取更接近于真实分布的后验状态估计。而其平方根实现又可解决在递推过程中因目标状态协方差矩阵非正定而出现的递推中断问题。因此，本文利用SR-RUGF方法为GPF构建重要性密度函数，进而得到SRRU-GPF，具体步骤如下所示。
已知$({\mathit{\boldsymbol{m}}_k}, {\mathit{\boldsymbol{P}}_k})$，量测${\mathit{\boldsymbol{z}}_{k + 1}}$：
1) 预测
for $ i = 1, 2, \cdots , M$
采样$ \mathit{\boldsymbol{x}}_k^{(i)} \sim N( \cdot {\rm{ }};{\mathit{\boldsymbol{m}}_k}, {\mathit{\boldsymbol{P}}_k})$
计算$ \mathit{\boldsymbol{x}}_{k + 1|k}^{(i)} = {f_{k + 1|k}}( \cdot \left| {\mathit{\boldsymbol{x}}_k^{(i)}} \right.)$
end
利用式(6)、式(7)计算$\mathit{\boldsymbol{m}}_{k + 1|k}^{}$和$\mathit{\boldsymbol{P}}_{k + 1|k}^{}$
2) 更新
设计重要性密度函数：
$[\hat{\boldsymbol{m}}, \hat{\boldsymbol{P}}] = $SR-RUGF$\left[\boldsymbol{m}_{k+1 | k}, \boldsymbol{P}_{k+1 | k}, \boldsymbol{z}_{k+1}\right]$
for $i = 1, 2, \cdots , M $
采样$\mathit{\boldsymbol{x}}_{k + 1}^{(i)} \sim N( \cdot ;\mathit{\boldsymbol{\hat m}}, \mathit{\boldsymbol{\hat P}}) $
利用式(10)计算权重$\bar w_{k + 1}^{(i)}$
end
利用式(11)归一化权重，得到$w_{k + 1}^{(i)}$
由式(12)、(13)计算$\boldsymbol{m}_{k + 1}^{}$和$\mathit{\boldsymbol{P}}_{k + 1}^{}$
输出 $({\boldsymbol{m}_{k + 1}}, {\mathit{\boldsymbol{P}}_{k + 1}}) $
另外，利用其他数值计算方法，如UKF等，结合2.2节的平方根递推更新实现思路，可获得不同的SR-RUGF算法，进而得到不同的SRRU-GPF算法。
-
本文利用CKF对RUGF和SR-RUGF进行具体实现，同时对所提算法的计算复杂度进行对比分析。表 1给出了CKF-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]：
$$\begin{gathered} {C_1} = \frac{4}{3}{n^3} + 3{n^2} + 10{n^2}m + 8{m^2}n + \\ \quad \quad \;\;\;{m^3} + 3{m^2} + 2mn + m \\ \end{gathered} $$ (17) 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算法的计算量更正为：
$$\begin{gathered} {C_2} = N(\frac{4}{3}{n^3} + 3{n^2} + 18{n^2}m + 12{m^2}n + \\ \quad \quad \quad \;\;{m^3} + 4{m^2} + 4mn + m) \\ \end{gathered} $$ (18) 根据表 1，本文CKF-SR-RUGF的计算量为：
$$\begin{gathered} {C_3} = \frac{4}{3}{n^3} - {n^2} - 2mn + N(4{n^3} + 9{n^2} + 3{m^3} + \\ \quad \quad 12{n^2}m + 6{m^2}n - {m^2} + 12mn + m) \\ \end{gathered} $$ (19) 对比式(17)~(19)可以得出，3种算法的计算复杂度由高到低依次为CKF-SR-RUGF算法、CKF-RUGF算法、CKF算法。但不难发现，本文提出的SR-RUGF算法的计算复杂度主要取决于参数$N$的选取。故在工程应用中，应选取合适的参数$N$来权衡计算量和滤波精度。
-
单变量非平稳增长模型是经典的粒子滤波仿真数据模型[4, 10, 15]，其模型描述如下：
$$\left\{ {\begin{array}{*{20}{c}} {{x_t} = \alpha {x_{t - 1}} + \beta \frac{{{x_{t - 1}}}}{{1 + x_{t - 1}^2}} + \gamma \cos (1.2(t - 1)) + {w_{t - 1}}} \\ {{\mathit{\boldsymbol{z}}_t} = \frac{{x_t^2}}{{20}} + {v_t}\quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad } \end{array}} \right.$$ (20) 式中，$\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)作为评价指标：
$$\operatorname{RMSE} = \sqrt {\frac{1}{{{\rm{MC}}}}\sum\limits_{i = 1}^{{\rm{MC}}} {{{({x_t} - {{{\hat x}}}_t^i)}^2}} } $$ (21) 式中，${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节)，将测量误差嵌入测量值计算各协方差矩阵，而有限次的高斯分布采样无法准确近似真实分布。
图 1 各滤波器状态估计均方根误差比较
表 2 各滤波器RMSE均值比较
滤波器 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的运行时间最长。
表 3 各滤波器单次运行时间比较
滤波器 时间/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$来权衡计算量和估计精度。
图 2 CKF-SRRU-GPF状态估计RMSE比较
表 4 CKF-SRRU-GPF状态估计RMSE均值比较
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}}_k} = \left[ {\begin{array}{*{20}{c}} {0.9}&0 \\ 0&1 \end{array}} \right]{\mathit{\boldsymbol{x}}_{k - 1}} + {\mathit{\boldsymbol{w}}_{k - 1}}$$ (22) 式中：$\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)$，则测量模型为：
$${\mathit{\boldsymbol{z}}_k} = {\tan ^{ - 1}}\left( {\frac{{{t_k} - \sin k}}{{{s_k} - \cos k}}} \right) + {v_k}$$ (23) 式中，${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的线性化计算引入了较大的近似误差，从而导致异常估计值。
图 3 各滤波器状态s估计RMSE比较
图 4 各滤波器状态t估计RMSE比较
表 5给出递推更新次数$N = 20$时各滤波器的单次运行时间对比。同样，由于利用递推更新方法，CKF-SRRU-GPF和EKRU-GPF的运行时间高于CKF-GPF。CKF-SRRU-GPF的运行时间最长，但其估计性能最好。
表 5 各滤波器单次运行时间比较
滤波器 时间/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。
图 5 CKF-SRRU-GPF状态s估计RMSE比较
图 6 CKF-SRRU-GPF状态t估计RMSE比较
表 6 CKF-SRRU-GPF状态s和t估计RMSE均值比较
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 -
对于GPF重要性密度函数的构建，本文提出一种SR-RUGF算法。该算法可有效克服线性最小均方误差准则的限制，对目标状态进行渐进式的更新，从而获取更接近于真实分布的后验状态估计，又可解决RUGF因协方差矩阵非正定而出现的递推中断难题。仿真结果表明本文算法的有效性。
Square-Root Recursive Update Gaussian Particle Filter
-
摘要: 对于高斯粒子滤波器重要性密度函数（IDF）的构建，递推更新高斯滤波器（RUGF）依据非线性测量函数梯度对目标运动状态进行渐进式的更新，可以有效克服线性最小均方误差准则的限制，从而得到更接近于真实分布的后验状态估计，但在递推过程中目标状态协方差矩阵易非正定而出现递推中断。针对这一问题，该文首先分析了RUGF的平方根实现策略，并借助容积卡尔曼滤波对平方根（SR）RUGF进行具体实现，然后利用SR-RUGF为高斯粒子滤波器选取IDF，进而得到平方根递推更新高斯粒子滤波器。仿真实验表明，本文算法可有效解决递推中断问题，并获得较高精度的估计结果。Abstract: For the construction of importance density function (IDF) of Gaussian particle filter, recursive update Gaussian filter (RUGF) which can effectively overcome the limitation of linear minimum mean square error criterion, updates the target state incrementally based on the gradient of nonlinear measurement function. Consequently, the posterior state estimation that is closer to the real distribution is obtained, but non-positive definite state covariance matrix will lead to recursive interruption. To solve this problem, the square-root implementation strategy of RUGF is firstly analyzed and then square-root recursive update Gaussian filter (SR-RUGF) is implemented by using cubature Kalman filter. Based on that, SR-RUGF is used to construct IDF for Gaussian particle filter. Simulation results demonstrate that the proposed algorithm can effectively solve the recursive interruption problem and obtain estimation result with higher accuracy.
-
表 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$ 
下载: 导出CSV
表 2 各滤波器RMSE均值比较
滤波器 RMSE均值 CKF-GPF 8.486 2 EKRU-GPF 5.145 2 CKF-SRRU-GPF 4.725 2 CKRU-GPF 4.296 2
下载: 导出CSV
表 3 各滤波器单次运行时间比较
滤波器 时间/s CKF-GPF 0.580 1 EKRU-GPF 0.629 0 CKF-SRRU-GPF 0.664 8 CKRU-GPF 0.639 9
下载: 导出CSV
表 4 CKF-SRRU-GPF状态估计RMSE均值比较
CKF-SRRU-GPF RMSE均值 N=2 9.648 0 N=5 6.044 4 N=10 5.301 4 N=20 4.808 9
下载: 导出CSV
表 6 CKF-SRRU-GPF状态s和t估计RMSE均值比较
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
下载: 导出CSV
-
[1] 杨小军, 潘泉, 王睿, 等.粒子滤波进展与展望[J].控制理论与应用, 2006, 23(2):261-267. doi: 10.3969/j.issn.1000-8152.2006.02.019 YANG Xiao-jun, PAN Quan, WANG Rui, et al. Development and prospect of particle filtering[J]. Control Theory & Application, 2006, 23(2):261-267. doi: 10.3969/j.issn.1000-8152.2006.02.019 [2] CAPPE O, GODSILL S J, MOULINES E. An overview of existing methods and recent advances in sequential Monte Carlo[J]. Proceedings of the IEEE, 2007, 95(5):899-924. doi: 10.1109/JPROC.2007.893250 [3] 李天成, 范红旗, 孙树栋.粒子滤波理论、方法及其在多目标跟踪中的应用[J].自动化学报, 2015, 41(12):1981-2002. LI Tian-chen, FAN Hong-qi, SUN Su-dong. Particle filtering:Theory, approach, and application for multitarget tracking[J]. Acta Automatica Sinica, 2015, 41(12):1981-2002. [4] KOTECHA J H, DJURIC P M. Gaussian particle filtering[J]. IEEE Transactions on Signal Processing, 2003, 51(10):2592-2601. doi: 10.1109/TSP.2003.816758 [5] DJURIC P M, KOTECHA J H, ZHANG J Q, et al. Particle filtering[J]. IEEE Signal Processing Magazine, 2003, 20(5):19-38. doi: 10.1109/MSP.2003.1236770 [6] MOVAGHATI S, MOGHADDAMJOO A, TAVAKOLI A. Road extraction from satellite images using particle filtering and extended Kalman filtering[J]. IEEE Transactions on Geoscience and Remote Sensing, 2010, 48(7):2807-2817. doi: 10.1109/TGRS.2010.2041783 [7] ZUO J Y, JIA Y N, GAO Q X. Simplified unscented particle filter for nonlinear/non-Gaussian Bayesian estimation[J]. Journal of Systems Engineering and Electronics, 2013, 24(3):537-544. doi: 10.1109/JSEE.2013.00062 [8] LU C G, FENG X X, LEI Y, et al. A novel particle filter for nonlinear non-Gaussian estimation[C]//Proceedings of 3rd International Workshop on Intelligent Systems and Applications. Wuhan, China: IEEE, 2011: 1-5. [9] ZANETI R. Recursive update filtering for nonlinear estimation[J]. IEEE Transactions on Automatic Control, 2012, 57(6):1481-1490. doi: 10.1109/TAC.2011.2178334 [10] 张勇刚, 王刚, 黄玉龙, 等.递推更新高斯粒子滤波[J].控制理论与应用, 2016, 33(3):353-360. http://d.old.wanfangdata.com.cn/Periodical/kzllyyy201603011 ZHANG Yong-gang, WANG Gang, HUANG Yu-long, et al. Recursive update Gaussian particle filter[J]. Control Theory & Application, 2016, 33(3):353-360. http://d.old.wanfangdata.com.cn/Periodical/kzllyyy201603011 [11] 何友, 修建娟, 关欣.雷达数据处理及应用[M].北京:电子工业出版社, 2013. HE You, XIU Jian-juan, GUAN Xin. Radar data processing with applications[M]. Beijing:Publishing House of Electronics Industry, 2013. [12] VAN DER MERWE R, WAN E A. The square-root unscented Kalman filter for state and parameter-estimation[C]//Proceedings of IEEE International Conference on Acoustics, Speech, and Signal Processing. Sate Lake City, UT, USA: IEEE, 2001: 3461-3464. [13] ARASARATNAM I, HAYKIN S. Cubature Kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6):1254-1269. doi: 10.1109/TAC.2009.2019800 [14] 张召友, 郝燕玲, 吴旭. 3种确定性采样非线性滤波算法的复杂度分析[J].哈尔滨工业大学学报, 2013, 45(12):111-115. doi: 10.11918/j.issn.0367-6234.2013.12.020 ZHANG Zhao-you, HAO Yan-ling, WU Xu. Complexity analysis of there deterministic sampling nonlinear filtering algorithms[J]. Journal of Harbin Institute of Technology, 2013, 45(12):111-115. doi: 10.11918/j.issn.0367-6234.2013.12.020 [15] 黄玉龙, 张勇刚, 李宁, 等.带不确定量测和未知虚警概率的粒子滤波器[J].控制理论与应用, 2015, 32(8):1012-1022. http://d.old.wanfangdata.com.cn/Periodical/kzllyyy201508003 HUANG Yu-long, ZHANG Yong-gang, LI Ning, et al. Particle filter with uncertain measurement and unknown false alarm probability[J]. Control Theory & Application, 2015, 32(8):1012-1022. http://d.old.wanfangdata.com.cn/Periodical/kzllyyy201508003 -
点击查看大图
图(6) / 表(6)
计量
- 文章访问数: 4318
- HTML全文浏览量: 1449
- PDF下载量: 101
- 被引次数: 0
