一种基于Gauss von Mises分布模型的非线性量测更新方法

一种基于Gauss von Mises分布模型的非线性量测更新方法

陈慕羿，王洪源

（沈阳理工大学 信息科学与工程学院，沈阳 110159）

基于状态空间模型的许多传统滤波算法都基于 Rn空间中的高斯分布模型，但当状态向量中包含角变量或方向变量时，难以达到理想的效果。针对J. T. Horwood等提出的 S ×Rn流形上的Gauss Von Mises (GVM)多变量概率密度分布，扩展了狄拉克混合逼近方法，给出了联合分布的GVM逼近方法，推导了后验分布的GVM参数计算公式，设计了量测更新状态估计算法。将J. T. Horwood等的时间更新算法与所提出的量测更新算法相结合，可实现基于GVM分布的递推贝叶斯滤波器(GVMF)。仿真结果表明，当状态向量符合GVM概率分布模型时，GVMF对角变量的估计明显优于传统的扩展卡尔曼滤波器。

Gauss von Mises分布；狄拉克混合逼近；递推贝叶斯滤波；量测更新

基于状态空间模型的传统滤波器，例如卡尔曼滤波器、扩展卡尔曼滤波器等大多都采用了欧式空间Rn中的高斯模型假设，而许多现实应用中，状态表示是定义在更复杂的非欧拓扑空间上的，例如在机器人感知、增强现实、传感器辅助导航等领域，经常需要同时估计运动物体的位置和姿态（朝向）。对于这些应用，所选择的状态向量需要同时包含平移量和旋转量，虽然平移量是定义于Rn空间中的，但旋转量是角度或方向数据，具有周期性，它们的值本质上是定义在S1、 S3等流形上的，这些流形不属于向量空间，也没有等价的线性模型。因此对于一般流形，如平面或三维空间中的刚体运动群SE(2)和SE(3)等，传统滤波方法[1-2]难以获得理想的结果，例如，当角度不确定性很大时，滤波器甚至可能发散。

于浛等[3]给出了具有随机时滞和异步相关噪声的非线性系统的高斯滤波器设计，对于含有观测时滞和相关噪声系统的状态估计问题给出了更高的精度和数值稳定性。对于包含方向变量的状态估计，许多学者利用方向统计学中的相关理论及研究成果，针对一些特殊流形建立了概率分布模型。Mardia等[4]给出了多变量von Mises分布及其参数估计方法，G. Kurz等[5]也提出了定义于刚体运动群SE(2)上的PWN（Partially Wrapped Normal）概率分布， 但这些研究中都没有提出相应的滤波算法。 除此之外，G. Stienne等[6]提出了多传感器圆融合滤波器，J. Glover等[7]引入了四元数Bingham滤波器跟踪高速3D旋转，但这些研究中，状态向量中只包含圆变量或四元数表示的三维旋转量，不包含任何线性变量。

J. T. Horwood等[8]为研究空间态势感知（Space Situational Awareness，SSA）中的不确定性传播问题，提出了Gauss von Mises（GVM）分布。SSA的主要研究目标是近地空间环境中的轨道空间对象，需要尽可能准确地获得空间对象的不确定性特征，来表示统计误差。而在这样的空间环境中，由于测量更新的时间间隔长、误检测率高、非线性、不守恒动力学和非高斯现象等原因，采用高斯分布和局部线性假设无法得到理想的结果，因此提出了GVM分布，在此基础上研究了测量更新间隔时间段内的不确定性传播问题，并得到了很好的结果。但文中只考虑了时间更新问题（即预测步骤），没有给出融合测量数据的量测更新步骤，且给出的参数估计方法具有一定的特殊性，难以通用。

本文针对 GVM分布[8]，扩展了狄拉克混合逼近方法，给出了递推滤波过程中联合密度的GVM逼近方法，在此基础上，推导了后验状态分布的GVM参数计算公式，设计了量测状态更新算法，通过将[8]中的时间更新（预测）方法与本文的量测更新方法相结合，可实现完整的GVM滤波器(GVM filter, GVMF)。

1 Gauss Von Mises分布的定义

定义1 GVM分布[8]：随机变量(x,θ)∈ Rn×S的联合分布满足Gauss von Mises（GVM）分布，当且仅当它们的联合概率密度函数具有形式：

记为：(x,θ)~GVM(μ, P,α,β,Γ,κ)。

式(1)中：

P =LLT；参数集(μ, P,α,β,Γ,κ)必须满足下列限制：μ ∈ Rn，P是n× n对称正定矩阵，α∈ R，β ∈Rn，Γ是n× n对称矩阵，κ ≥ 0；L是参数矩阵P的下三角 Cholesky因子；参数β和Γ用于对x和θ的相关性进行建模。

通过变换：

可将GVM(μ, P,α,β,Γ,κ)简化为规范形式：

2 GVM分布的狄拉克混合逼近

从一个给定的概率密度函数生成样本通常采用随机采样的方式，简单快速，并有许多已有的成熟方法。但在随机采样中，样本是独立随机生成的，因此，矩等统计性质收敛到真实值的速度很慢，需要大量的样本。除此之外，样本之间完全独立也会导致对给定概率密度的非均匀覆盖。

为克服随机采样的缺点，研究提出了许多确定性采样方法，其中，多维高斯分布的狄拉克混合逼近方法[8]无需限制点位于坐标轴上，可以灵活控制采样点个数，在满足某些矩匹配约束的同时，尝试对于某种距离测度，最小化原分布与狄拉克逼近之间的距离，可实现对给定密度较为均匀的覆盖。本节扩展了狄拉克混合逼近方法，使其可用于多维GVM分布，该方法包含4个步骤：

第一步：首先利用狄拉克混合采样方法对 Rn空间中的规范高斯分布进行采样，生成q个采样点

第二步：通过d个n× n维正交旋转矩阵，对q个采样点进行旋转变换，共生成dq个采样点。通过适当选择正交矩阵，这种方法得到的采样点能够更好地满足对称性和均匀覆盖，且对于规范高斯分布,能够保持矩不变。

第三步：得到 Rn空间中的采样点后，对于dq个采样点中的每个点进行分裂，随机生成l个符合VM(0,κ)分布的角度样本点，设分裂系数为c， 那么表示对应于点 y的c个角度样本点，因此，可γi得到cdq个 Rn×S空间中的采样点

3 GVM分布的量测更新算法

考虑如下离散时间随机动力系统：(xk,θk)=，其中表示状态向量，表示测量值，表示测量噪声，f是系统转移函数，g是测量函数。

贝叶斯递推滤波包括预测和量测更新两个步骤。对于GVM分布模型，预测步骤根据上一时刻估计得到的GVM状态分布及系统方程，计算出预测状态分布的GVM逼近。文献[8]已给出预测步的算法流程，本文将主要考虑量测更新步的算法。

3.1 联合分布的GVM逼近

为了计算出后验状态分布，我们首先需要计算出联合分布的GVM逼近，这可看作是求后验概率密度的中间步骤[9]，即：给定预测密度的逼近，我们需要计算联合分布的 GVM 逼近参数，使得：

对于联合分布，有：

1）线性部分参数计算

并有：

对于时间更新（预测步骤）所得到的GVM分布，将利用狄拉克混合逼近方法所求出的采样点记为，则经测量方程变换后可得预测的测量值：。可利用这些点通过最小二乘拟合得到参数的最优估计。令=，对参数的估计可转化为优化问题：

可采用高斯-牛顿、拟牛顿法，结合例如线性搜索、置信域等方法求解式(6)的非线性最小二乘问题，求得参数

3.2 后验概率分布的GVM参数计算

由于：

对比式(10)与(11)，令两式中xk的各次项系数相等，可解出为：

3.3 量测状态更新算法描述

量测更新算法的输入为：

● 给定预测分布的GVM逼近：

● 测量值 zk。

算法输出为k时刻的后验状态密度的GVM逼近

量测更新算法包含以下4个步骤：

第一步：根据给定预测状态密度的GVM逼近，利用式(2)~(5)，计算出、。

第二步：根据给定预测状态密度的GVM逼近生成q个狄拉克采样点，并计算经测量方程变换后的点

该算法中，利用2n+ 3个sigma点及积分逼近方法可保证对线性部分一阶二阶矩的估计精度，但利用这些 sigma点难以保证参数的估计精度，而狄拉克采样点个数可以控制，且覆盖较为均匀，利用该方法可对参数进行更精确的估计。与基于粒子的滤波方法相比，利用狄拉克混合逼近方法所需的采样点个数远小于粒子滤波所需的采样点数目，且不存在粒子退化问题。

将本文提出的量测更新算法与文献[8]中提出的时间更新（预测）算法相结合，可以设计出GVM滤波器（GVMF）。

4 仿真验证

我们采用文献[10]中的仿真模型，该模型是非线性问题的一个典型例子。考虑一个在近圆轨道上运行的近地空间目标，希望利用一系列角测量结果来估计它的位置及速度。为简便，假定所有的运动和测量都在同一平面上，从而可用极坐标r和θ来表示目标的运动。通过牛顿或拉格朗日力学，可推导出运动方程：

测量方程为：

利用MATLAB 2008a，分别对GVMF与扩展卡尔曼滤波器（Extended Kalman filter，EKF）进行仿真。仿真过程中采用的参数为：真实的初始状态为：

仿真结果如图1、图2所示，可以看出，GVMF能够比较快速地收敛到真实值，而EKF对角度的估计误差随时间增加，GVMF对角的估计明显优于EKF。

图1 角速度估计误差Fig.1 Angular velocity estimation error

图2 角估计误差Fig.2 Angular estimation error

5 结 论

本文针对GVM分布，扩展了狄拉克混合逼近方法，给出了联合分布的GVM逼近算法，推导出了后验状态分布的GVM参数计算公式，并在此基础上，给出了GVM滤波中的量测状态更新方法，并对非线性状态方程和测量方程进行了仿真。

仿真结果表明，当状态向量符合GVM分布时，利用本文的所提出的量测更新算法所设计出的GVMF很好地实现了滤波，对角变量的估计明显优于EKF。

（References）：

[1] 张敏虎, 任章, 华春红. UKF在深组合GPS/INS导航系统中的应用[J].中国惯性技术学报, 2009, 17(6): 697-700. Zhang Min-hu, Ren Zhang, Hua Chun-hong. Application of UKF in deeply coupled GPS/INS navigation system[J]. Journal of Chinese Inertial Technology, 2009, 17(6): 697-700.

[2] 熊剑, 郭杭, 熊智, 等. GPS/INS组合导航系统中的高斯粒子滤波混和算法[J]. 中国惯性技术学报, 2012, 20(2): 596-600. Xiong Jian, Guo Hang, Xiong Zhi, et al. Gaussian particle filtering hybrid algorithm for GPS/INS integrated navigation system[J]. Journal of Chinese Inertial Technology, 2012, 20(2): 596-600.

[3] 于浛, 宋申民, 王硕, 等. 具有随机时滞和异步相关噪声的非线性系统的高斯滤波器设计[J]. 中国惯性技术学报, 2015, 23(2): 238-247. Yu Han, Song Shen-min, Wang Shuo, et al. Improved Gaussian filter algorithm for nonlinear system with random delay and asynchronously correlated noises[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 238-247.

[4] Mardia K V, Hughes G, Taylor C C, et al. A multivariate von Mises distribution with applications to bioinformatics[J]. Canadian Journal of Statistics, 2008, 36(1): 99-109. [5] Kurz G, Gilitschenski I, Hanebeck U D. The partially wrapped normal distribution for SE(2) estimation[C]//2014 International Conference on Multisensor Fusion and Information Integration for Intelligent Systems. Beijing, China, 2014: 1-8.

[6] Stienne G, Reboul.S, Azmani M, et al. A multi-temporal multi-sensor circular fusion filter[J]. Information Fusion, 2013, 18(7): 86-100.

[7] Glover J, Kaelbling L P. Tracking 3-D rotations with the quaternion bingham filter [R]. MIT's Computer Science and Artificial Intelligence Laboratory. Mar. 2013.

[8] Joshua T. Horwood, Aubrey B. Poore. Gauss von Mises distribution for improved uncertainty realism in space situational awareness[J]. SIAM/ASA Journal on Uncertainty Quantification, 2014, 2(1): 276-304.

[9] Deisenroth M P and Ohlsson H. A general perspective on gaussian filtering and smoothing: explaining current and deriving new algorithms[C]//Proc. of the American Control Conference. 2011: 1807-1812.

[10] Sorenson H W. Kalman filtering techniques: Advances in control systems[M]. New York: Academic Press, 1966, Vol.3: 219-289.

Nonlinear measurement update based on Gauss von mises distribution

CHEN Mu-yi, WANG Hong-yuan
(School of Information Science and Engineering, Shenyang Ligong University, Shenyang 110159, China)

Many traditional Kalman-based state estimation algorithms assume that the system state is uncertain and the measurements are Gaussian distributed. However, this assumption cannot catch the periodic nature of angular or orientation variables. Based on the Gauss von Mises (GVM) distribution defined on a cylindrical manifold, this paper extends the Dirac mixture approximation method to deal with sampling with GVM. The GVM approximation with joint distribution is proposed to perform recursive Bayesian filtering. The formula for computing the posterior distribution is deduced, and the measurement update algorithm is developed. The GVM filtering can be realized by combined with the time update algorithm proposed by J.T.Horwood. Simulation results demonstrates that, when state variables conform with GVM distributions, the GVM filter can achieve more accurate angular estimates than the traditional extended Kalman filter.

Gauss von Mises distribution; Dirac mixtures approximation; recursive Bayesian filtering; measurement update

V448.2

：A

2016-03-30；

：2016-04-12

辽宁省教育厅科学研究一般项目（L2013083）；国家自然科学青年基金（61501308）

陈慕羿（1981—），女，讲师，从事传感器融合、目标定位跟踪研究。E-mail: camchenm@163.com

1005-6734(2016)03-0361-05

10.13695/j.cnki.12-1222/o3.2016.03.015