基于多层重要性采样的高斯滤波方法和高斯滤波器

摘要

本发明提供一种基于多层重要性采样的高斯滤波方法和滤波器，属于非线性滤波技术领域，包括步骤一：根据实际工程应用，建立非线性系统的状态方程和测量方程；步骤二：初始状态：确定系统初始状态，即初始状态的随机分布特征，包括其均值、协方差以及高阶矩，噪声的分布特征，以及初始测量值；步骤三：根据计算复杂度、滤波精度以及稳定性的要求，初始化样本层数和样本类别数，计算每层样本点的权重和矩匹配参数；接着再进行一步状态预测和一步量测预测，最后进行状态滤波更新，使用卡曼增益融合状态预测以及测量数据计算最优状态的分布特征，完成非线性系统一步估计任务，并迭代回到步骤四，进行下一时刻估计任务，本发明具有更好的性能和实用性。

说明书

技术领域

本发明涉及非线性滤波、数字信号处理、目标定位跟踪等的信息融合技术领域，特别是 涉及一种基于多层重要性采样的高斯滤波方法和滤波器。

背景技术

几乎所有的现实系统都是非线性的，尤其在飞行器导航、目标跟踪及工业控制等领域。 例如：在目标定位跟踪过程中，利用雷达对空中目标进行观测时，雷达能够获得空中目标相 对自身的方位角，但该观测含有噪声，观测方程中雷达的方位观测量是待估计目标位置参数 的非线性函数，不能直接利用线性滤波方法获取目标的运动状态，其本质为非线性滤波问题， 是目标跟踪、数字信号处理等研究领域的共同难题。

针对非线性滤波问题，常采用两类滤波方法：一类是对非线性函数进行线性化近似,对 高阶项采用忽略或逼近的措施，其中最广泛使用的是扩展卡尔曼滤波器(英文全称：Extended KalmanFilter，简称：EKF)，其基本思路是对非线性函数的Taylor展开式进行一阶线性化截 断,从而将非线性问题转化为线性；另一类是采用采样方法近似非线性分布，常用的有粒子 滤波器(英文全称：ParticleFiler，简称：PF)、无迹卡尔曼滤波器(英文全称：UnscentedKalman Filter，简称：UKF)和容积滤波(英文全称：CubatureKalmanFilte，简称：CKF),其基本原 理是使用样本点结合其权重逼近非线性函数的随机变量的分布。

上述EKF，PF、UKF、CKF虽然在某些方面具有优秀的性能，但它们都有自身难以克服 的缺点。

首先，EKF具有以下三点不足：(1)当非线性函数Taylor展开式的高阶项无法忽略时, 线性化会使系统产生较大的误差，甚至于滤波器难以稳定；(2)在许多实际问题中很难得 到非线性函数的雅克比矩阵，甚至不存在；(3)EKF需要求导，所以必须清楚了解非线性 函数的具体形式,无法做到黑盒封装,从而难以模块化应用。目前,虽然对EKF有众多的改 进方法,如高阶截断EKF,迭代EKF等,但这些缺陷仍然难以克服。

第二，相比较于EKF，UKF能达到更高阶的计算精度，且计算量与EFK同阶次。而且， UKF采用确定性采样，故其所需的样本点相对较少。然而，UKF面临一个参数k的选择问题。 如果k选择最优值，必须满足k＝3-n。在高维情况下，原点样本的权重可为负数。最终导致滤 波发散或不稳定。

第三，与UKF相对比，粒子滤波器(PF)采用的随机样本点，需要的数量非常大，而且其 样本点数量随着问题的维数呈几何级数地增长，其计算代价十分昂贵。但是，PF采用了重要 性采样，能有效地获取重要样本以及非负权重。因此，PF具有良好的稳定性。

最后，CKF相对于UKF和EKF，具有良好的稳定性以及精度。然而，当采用高阶CKF 时，比如5-degreeCKF，通过高阶矩匹配计算样本权重就变得非常困难。而且样本点数也会 快速增加。另外，高阶CKF的权重中负权重，这也会导致系统的发散或不稳定性。

综上所述，PF的重要性采样能提供良好的样本以及非负权重；CKF和UKF的确定性采 样能通过少量的样本较好地逼近非线性分布。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于多层重要性采样的高斯 滤波方法和滤波器，用于解决解决非线性滤波器在实际应用过程中稳定性、精度和计算效率 的问题。

为实现上述目的及其他相关目的，本发明提供以下技术方案：

一种基于多层重要性采样的高斯滤波方法，包含以下步骤：

步骤一：根据实际工程应用，建立非线性系统的状态方程和测量方程；

步骤二；初始状态：确定系统初始状态，即初始状态的随机分布特征，包括其均值、协 方差以及高阶矩，噪声的分布特征，以及初始测量值；

步骤三：根据计算复杂度、滤波精度以及稳定性的要求，初始化样本层数和样本类别数。 然后基于样本层数和样本类别数，计算每层样本点的权重和矩匹配参数。

步骤四：一步状态预测：基于上一时刻的状态估计和状态方程，使用多层重要性采样方 法(英文全称：Multi-layerImportanceSampling，简称：MIS)计算一步状态预测的随机变量 的分布特征；

步骤五：一步量测预测：基于步骤四的状态预测和测量方程，使用MIS计算状态预测的 量测的分布特征；

步骤六：状态滤波更新：使用卡曼增益(KalmanGain)融合状态预测以及测量数据计算 最优状态的分布特征，完成非线性系统一步估计任务，并迭代回到步骤四，进行下一时刻估 计任务。

进一步地，步骤一所述的根据实际工程应用，建立非线性系统的状态方程和测量方程为：

其中，k表示第k步，x_k为第k步的n维状态向量，z_k为第k步的m维量测向量，f及h 为非线性函数，wk为n维随机系统噪声，v_k为m维的随机测量噪声，其中系统噪声服从均值 为零，方差为Q_k的高斯分布，测量噪声服从均值为零，方差为R_k的高斯分布，并且测量噪声 和系统噪声互不相关。这里，函数f(x_k-1)是系统状态变换的数学模型，函数h(x_k)对应系统状 态测量的数学模型。

进一步地，步骤三所述的样本半径、样本权重以及匹配参数。使用重要性采样和二阶矩 匹配计算上述参数可分为以下三个步骤：

步骤(一)：根据滤波精度和计算法复杂度要求，确定采样层数L和样本类别数C。根 据重要性密度函数“累积分布”为均匀分布，使用公式(2)确定L层预样本的半径j＝1,2,…L，

$\begin{array}{c}F\left(\sqrt{{\alpha }_j}\right)={\int }_{{\left(x-\bar{x}\right)}^T{P}_x^{-1}\left(x-\bar{x}\right)\le {\alpha }_j}\frac{1}{{\left(2\pi \right)}^{n/2}}\frac{1}{{\left|P_x\right|}^{1/2}}\exp \left\{-\frac{1}{2}{\left(x-\bar{x}\right)}^T{P}_x^{-1}\left(x-\bar{x}\right)\right\}dx\\ =\frac{1}{2^{n/2-1}}\frac{1}{\Gamma \left(n/2\right)}{\int }_{r^2\le {\alpha }_j}\exp \left\{-\frac{1}{2}{r}^2\right\}{r}^{n-1}dr\\ =\frac{j-1}{L}\end{array}\mathrm{......}\left(2\right);$

步骤(二)：根据步骤(一)的预样本半径用表示第j层的第C类的第ic 个样本点的权重,根据公式(3)计算每层上每个样本对应的权重。

$W_{i_c,c,j}=\frac{\exp \left\{-\frac{1}{2}{\alpha }_j\right\}}{\underset{j=1}{\overset{L}{\Sigma }}\exp \left\{-\frac{1}{2}{\alpha }_j\right\}\underset{c=1}{\overset{C}{\Sigma }}{2}^c\left(\begin{array}{c}n\\ c\end{array}\right)}\mathrm{......}\left(5\right)$

根据式可知同一层上每个样本有相同的权重，即样本权重只与预样本半径有关；

步骤(三)：根据所需匹配阶矩，利用公式(4)计算参数β以确定样本半径，

$1=\underset{j=1}{\overset{L}{\Sigma }}({\alpha }_j+\beta )\underset{c=1}{\overset{C}{\Sigma }}{W}_{i_c,c,j}\frac{1}{c}{2}^c\left(\begin{array}{c}n-1\\ c-1\end{array}\right)\mathrm{......}\left(6\right).$

进一步地，步骤四所述的基于上一时刻的状态估计和状态方程，使用MIS计算一步状态 预测的随机变量的分布特征可分为以下两个步骤：

步骤(一)：根据上一步的状态估计随机变量分布特征，即均值协方差P_k|k，以及 权利要求3中所确定的预样本半径和匹配参数β，根据公式(5)确定对应权重的样 本点j＝1,2,…L,，c＝1,2,…C,$ic=1,2,...Nc,Nc=2c\left(\begin{array}{c}n\\ c\end{array}\right),$

$S_{i_c,c,j}^{k|k},={\bar{x}}_{k|k}+\sqrt{{\alpha }_j+\beta }\sqrt{P_{k|k}}{\eta }_{c,i_c}\mathrm{......}\left(7\right)$

其中，是η_c的第i_c个全排列。η_c为

例如当n＝2，c＝1，有

${\eta }_{1,i_1}=\frac{1}{\sqrt{2}}{\{\left(\begin{array}{c}1\\ 0\end{array}\right),\left(\begin{array}{c}-1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\end{array}\right),\left(\begin{array}{c}0\\ -1\end{array}\right)\}}_{i_1},i_1=1,2,3,4;$

步骤(二)：随机变量状态方程变换的分布特征计算：根据变换函数，计算sigma点经 过状态方程变换后的变换sigma点对应的权重为根据公式系统方程，其计算方 法为：

$S_{i_c,c,j}^{k+1|k}=f\left(S_{i_c,c,j}^{k|k}\right),j=1,2,\mathrm{...}L;c=1,2,\mathrm{...}C;i_c=1,2,\mathrm{...}{N}_c\mathrm{......}\left(8\right)$

然后，根据公式(7)计算变换随机变量x_k+1|k的均值向量：

${\bar{x}}_{k+1|k}=\underset{j=1}{\overset{L}{\Sigma }}\underset{c=1}{\overset{C}{\Sigma }}\underset{i_c=1}{\overset{N_c}{\Sigma }}{W}_{i_c,c,j}{S}_{i_c,c,j}^{k+1|k}\mathrm{......}\left(9\right)$

利用公式的结果根据公式(8)计算变换随机变量x_k+1|k的协方差：

$P_{k+1|k}=\underset{j=1}{\overset{L}{\Sigma }}\underset{c=1}{\overset{C}{\Sigma }}\underset{i_c=1}{\overset{N_c}{\Sigma }}{W}_{i_c,c,j}(S_{i_c,c,j}^{k+1|k}-{\bar{x}}_{k+1|k}){(S_{i_c,c,j}^{k+1|k}-{\bar{x}}_{k+1|k})}^T+Q_k\mathrm{......}\left(10\right)$

进一步地，步骤四所述的基于步骤四的状态预测和测量方程，使用MIS计算状态预测的 量测的分布特征可分为以下二个步骤：

步骤(一)：根据上一步的状态估计随机变量分布特征，即均值协方差P_k+1|k，利 用公式(9)确定sigma样本点对应的权重为c＝1,2,…C,

${\chi }_{i_c,c,j}^{k+1|k}={\bar{x}}_{k+1|k}+\sqrt{{\alpha }_j+\beta }\sqrt{P_{k+1|k}}{\eta }_{c,i_c}\mathrm{......}\left(11\right)$

步骤(二)：随机变量量测方程变换的分布特征计算：根据变换函数，计算sigma点经 过量测方程变换后的变换sigma点对应的权重为根据测量方程，使用公式(10) 计算方变换点：

$Z_{i_c,c,j}^{k+1|k}=h\left({\chi }_{i_c,c,j}^{k+1|k}\right)\mathrm{......}\left(12\right);$

然后，使用公式(11)计算变换随机变量z_k+1|k的均值向量：

${\bar{z}}_{k+1|k}=\underset{j=1}{\overset{L}{\Sigma }}\underset{c=1}{\overset{C}{\Sigma }}\underset{i_c=1}{\overset{N_c}{\Sigma }}{W}_{i_c,c,j}{Z}_{i_c,c,j}^{k+1|k}\mathrm{......}\left(13\right)$

利用公式结果使用公式(12)计算变换随机变量z_k+1的协方差：

$P_{zz,k+1|k}=\underset{j=1}{\overset{L}{\Sigma }}\underset{c=1}{\overset{C}{\Sigma }}\underset{i_c=1}{\overset{N_c}{\Sigma }}{W}_{i_c,c,j}\left(Z_{i_c,c,j}^{k+1|k}-{\bar{z}}_{k+1|k}\right){\left(z_{i_c,c,j}^{k+1|k}-{\bar{z}}_{k+1|k}\right)}^T+R_k\mathrm{......}\left(14\right)$

根据公式、样本点使用公式(13)计算变换随机变量x_k+1|k与z_k+1|k的协方差：

$P_{xz,k+1|k}=\underset{j=1}{\overset{L}{\Sigma }}\underset{c=1}{\overset{C}{\Sigma }}\underset{i_c=1}{\overset{N_c}{\Sigma }}{W}_{i_c,c,j}\left(S_{i_c,c,j}^{k+1|k}-{\bar{x}}_{k+1|k}\right){\left(Z_{i_c,c,j}^{k+1|k}-{\bar{z}}_{k+1|k}\right)}^T\mathrm{......}\left(15\right).$

进一步地，步骤六所述的使用卡曼增益融合状态预测以及测量数据计算最优状态的分布 特征，完成非线性系统一步估计任务的具体过程为：

分别使用公式和公式(15)计算x_k+1的均值与协方差：

${\bar{x}}_{k+1|k+1}={\bar{x}}_{k+1|k}+K_{k+1}\left(z_{k+1}-{\bar{z}}_{k+1|k}\right)---\left(16\right)$

$P_{k+1|k+1}=P_{k+1|k}-K_{k+1}{P}_{zz,k+1|k}{K}_{k+1}^T---\left(17\right)$

式中为卡曼增益；z_k+1是实际测量数据值。

更进一步地，在上述步骤三、步骤四和步骤五中以及其进一步描述中是使用相同的样本 半径、样本类别数以及权重，即在滤波进程中，样本半径和权重是不变的。因此，该样本半 径和权重可以离线计算存储，以简化滤波复杂度和计算量。

另外，本发明还提供了一种高斯滤波器，其通过上述基于多层重要性采样的高斯滤波方 法来实现滤波。

如上所述，本发明具有以下有益效果：本发明充分利用协方差椭圆的几何特征，将重要 性采样和多层采样结合，获得合理的重要性要本和非负权重，使得滤波稳定性以及滤波精度 显著提高。同时，该方法还可以根据计算复杂度需要，调整采样的层数和样本类别，得到所 需的滤波性能。

附图说明

图1为本发明一种基于多层重要性采样的高斯滤波方法的实现流程图。

图2为基于本发明提供的MISKF方法与基于UKF、CKF方法的目标跟踪应用中对位置 估计的均方误差曲线图。

图3为基于图本发明提供的MISKF方法与基于UKF、CKF方法的目标跟踪应用中对目 标速度估计的均方误差曲线图。

图4为基于图本发明提供的MISKF方法与基于UKF、CKF方法的目标跟踪应用中对目 标角速度估计的均方误差曲线图。

元件标号说明

S101～S11步骤

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露 的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加 以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精 神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征 可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图 式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实 际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复 杂。

下面将结合附图，对本发明的优选实施例进行详细的描述。

实施例一

在目标追踪过程中，观测站能够获得包含噪声的目标方位信息，而方位信息与待估的目 标位置信息间关系为非线性，通常应用EKF、二阶UKF、3阶CKF或5阶CKF非线性滤波 方法来获得目标的运动状态。但在目标定位要求较高的场合上述滤波方法不能满足要求。本 发明提供的方法比现有的方法具备更高的估计精度，能够提高目标跟踪的精度。下面以具体 实施例子来说明本发明的优越性。请参见图1，为具体如下：

S101：依据纯方位目标跟踪问题，建立如下描述目标跟踪系统的状态方程和观测方程：

$x_k=\left(\begin{array}{ccccc}1& \frac{\sin \left({\omega }_{k-1}\Delta t\right)}{{\omega }_{k-1}}& 0& \frac{\cos \left({\omega }_{k-1}\Delta t\right)-1}{{\omega }_{k-1}}& 0\\ 0& \cos \left({\omega }_{k-1}\Delta t\right)& 0& -\sin \left({\omega }_{k-1}\Delta t\right)& 0\\ 0& \frac{1-\cos \left({\omega }_{k-1}\Delta t\right)}{{\omega }_{k-1}}& 1& \frac{\sin \left({\omega }_{k-1}\Delta t\right)}{{\omega }_{k-1}}& 0\\ 0& \sin \left({\omega }_{k-1}\Delta t\right)& 0& \cos \left({\omega }_{k-1}\Delta t\right)& 0\\ 0& 0& 0& 0& 1\end{array}\right)\times x_{k-1}+w_{k-1}---\left(16\right)$

$z_k=\left(\begin{array}{c}\sqrt{x_k^2+y_k^2}\\ {\tan }^{-1}\left(\frac{y_k}{x_k}\right)\end{array}\right)+v_k---\left(17\right)$

其中，第k步的状态方程中x_k和y_k分别表示x坐标轴和y坐标 轴平面内(笛卡尔坐标系)目标的位置，和分别表示x坐标轴和y坐标轴方向的速度；ω_k表示目标的角速度；随机系统噪声向量w_k服从均值为零，方差为Q_k的高斯分布；随机量测 噪声向量v_k服从均值为0，方差为R_k的高斯分布。而且有：

$Q_{k-1}=\left(\begin{array}{ccccc}\frac{{\mathrm{\Delta t}}^3}{3}& \frac{{\mathrm{\Delta t}}^2}{2}& 0& 0& 0\\ \frac{{\mathrm{\Delta t}}^2}{2}& \Delta t& 0& 0& 0\\ 0& 0& \frac{{\mathrm{\Delta t}}^3}{3}& \frac{{\mathrm{\Delta t}}^2}{2}& 0\\ 0& 0& \frac{{\mathrm{\Delta t}}^2}{2}& \Delta t& 0\\ 0& 0& 0& 0& 1.75\times {10}^{-1}\Delta t\end{array}\right)\times 2,R_k=\left(\begin{array}{cc}1000& 0\\ 0& 100\end{array}\right)\times 6$

其中，Δt为采样间隔，本次实例中为1s。

S103：初始状态：确定系统初始状态，即初始状态的随机分布特征，包括其均值、协方 差：

初始状态：x₀＝[1000m,300m/s,1000m,0m/s,-3°/s]^T；

初始协方差矩阵P₀＝diag[100m²,10m²/s²,100m²,10m²/s²,100mrad²/s²]，P₀表征了系统 初始位置的不确定性。

S105：根据计算复杂度、滤波精度以及稳定性的要求，初始化样本层数L＝4和样本类别 数C＝1，然后基于样本层数和样本类别数，计算每层样本点的权重和矩匹配参数。

具体为，根据S105确定的层数L和公式(2)，计算出每层预样本的半径然后根 据公式(3)，将代入求解每层样本的权重根据公式(4)，求解出匹配参数β。

S107：基于上一时刻的状态估计和状态方程，使用MIS计算一步状态预测的随机变量的 分布特征。

假设在k时刻，状态的随机分布特征为均值协方差P_k|k，则将上述参数代入公式(5)， 得到所采集的样本对应权重为

随机变量状态方程变换的分布特征计算：根据变换函数，根据公式(5)计算得到的样本 点经过状态方程变换后的变换样本点对应的权重为然后，根据公式(7)计算 变换随机变量x_k+1|k的均值向量根据公式(8)计算变换随机变量x_k+1|k的协方差P_k+1|k。

S109：一步量测预测：基于S105的状态预测和测量方程，使用MIS计算状态预测的量 测的分布特征。

具体为，根据S105的状态估计随机变量的特征，即协方差P_k+1|k，根据公式(9) 确定样本点对应的权重为

根据公式(10)，计算sigma点经过量测方程变换后的变换sigma点对应的权重 为然后根据公式(11)和(12)，分别计算变换随机变量z_k+1|k的均值向量和协 方差矩阵P_zz,k+1|k。同时，根据公式(13)，计算变换随机变量x_k+1|k与z_k+1|k的互协方差P_xz,k+1|k。

S111：状态滤波更新：使用卡曼增益(KalmanGain)融合状态预测以及测量数据计算最 优状态的分布特征，完成非线性系统一步估计任务，并迭代回到S105，进行下一时刻估计任 务。

具体为：根据公式(14)、(15)计算x_k+1的均值和协方差P_k+1|k+1。，

实施过程：仿真过程中，定义目标位置在k时刻的RMSE：

${\mathrm{RMSE}}_{postion}\left(k\right)=\sqrt{\frac{1}{N}\underset{i=1}{\overset{N}{\Sigma }}{\left({\bar{\xi }}_k^i-{\xi }_k^i\right)}^2+{\left({\bar{\psi }}_k^i-{\psi }_k^i\right)}^2}---\left(18\right)$

其中，和分别为目标位置在第i次模拟中的的估计值和真实值。同时， 我们还将建立速度和角速度的RMSE。

对目标位置估计的均方误差越小则表明目标跟踪精度越高，效果越好。其中，仿真时间 为100步。

实施效果：图2、图3、图4分别给出了纯方向目标跟踪过程中，基于本发明提供的MISKFF 方法与基于UKF、CKF方法的目标跟踪应用中对目标位置、速度和角速度的估计均方误差曲 线。并结合以下表1，表1为计算复杂度与滤波性能RMSE比较：

滤波器 位置 速度 角速度 样本数 MISKF 131.5864 72.1620 7.0195 40 UKF(K＝-1) 123.4109 1570.4919 29.4534 11 UKF(K＝1) 158.6223 67.5632 6.4943 11 UKF(K＝2) 201.2941 1979.4560 43.3447 11 CKF-3 159.9505 1765.9425 27.9980 10 CKF-5 125.8849 68.7983 6.5248 50

表1

根据实验结果可知，MISKF在滤波精度上比3阶CKF和UKF(κ＝-1,1)更高，比5 阶CKF滤波精度略低，与UKF(κ＝2)相当。在稳定性上，UKF(κ＝-1)时，滤波进程 经常因非正定性出现发散或停止状态。3阶CKF比UKF(κ＝-1)要好，但也经常出现滤波 误差太大，导致滤波失效。5阶CKF稳定性相对稳定，但当系统的稳定性降低，滤波进行依 然会出现非稳定性波动，导致滤波发散或停止。而MISKF在整个滤波过程中都非常稳定，不 会出现发散或滤波停止情况。这主要得益于MISKF的非负权重。在算法复杂度上，本实验中 的MISKF低于5阶CKF的复杂度，高于其他算法复杂度。同时，根据滤波要求，通过调整 样本层数L和样本类别书C，可以获得所需的MISKF的滤波精度。这比CKF和UKF更加方 便和灵活。

综上所述，本发明的基于多层重要性采样的高斯滤波方法相对于其他滤波方法，具有更 好的性能和实用性。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技 术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡 所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等 效修饰或改变，仍应由本发明的权利要求所涵盖。