一種基于Gauss von Mises分布模型的非線性量測更新方法

一種基于Gauss von Mises分布模型的非線性量測更新方法

陳慕羿，王洪源

（沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，沈陽 110159）

基于狀態(tài)空間模型的許多傳統(tǒng)濾波算法都基于 Rn空間中的高斯分布模型，但當(dāng)狀態(tài)向量中包含角變量或方向變量時(shí)，難以達(dá)到理想的效果。針對(duì)J. T. Horwood等提出的 S ×Rn流形上的Gauss Von Mises (GVM)多變量概率密度分布，擴(kuò)展了狄拉克混合逼近方法，給出了聯(lián)合分布的GVM逼近方法，推導(dǎo)了后驗(yàn)分布的GVM參數(shù)計(jì)算公式，設(shè)計(jì)了量測更新狀態(tài)估計(jì)算法。將J. T. Horwood等的時(shí)間更新算法與所提出的量測更新算法相結(jié)合，可實(shí)現(xiàn)基于GVM分布的遞推貝葉斯濾波器(GVMF)。仿真結(jié)果表明，當(dāng)狀態(tài)向量符合GVM概率分布模型時(shí)，GVMF對(duì)角變量的估計(jì)明顯優(yōu)于傳統(tǒng)的擴(kuò)展卡爾曼濾波器。

Gauss von Mises分布；狄拉克混合逼近；遞推貝葉斯濾波；量測更新

基于狀態(tài)空間模型的傳統(tǒng)濾波器，例如卡爾曼濾波器、擴(kuò)展卡爾曼濾波器等大多都采用了歐式空間Rn中的高斯模型假設(shè)，而許多現(xiàn)實(shí)應(yīng)用中，狀態(tài)表示是定義在更復(fù)雜的非歐拓?fù)淇臻g上的，例如在機(jī)器人感知、增強(qiáng)現(xiàn)實(shí)、傳感器輔助導(dǎo)航等領(lǐng)域，經(jīng)常需要同時(shí)估計(jì)運(yùn)動(dòng)物體的位置和姿態(tài)（朝向）。對(duì)于這些應(yīng)用，所選擇的狀態(tài)向量需要同時(shí)包含平移量和旋轉(zhuǎn)量，雖然平移量是定義于Rn空間中的，但旋轉(zhuǎn)量是角度或方向數(shù)據(jù)，具有周期性，它們的值本質(zhì)上是定義在S1、 S3等流形上的，這些流形不屬于向量空間，也沒有等價(jià)的線性模型。因此對(duì)于一般流形，如平面或三維空間中的剛體運(yùn)動(dòng)群SE(2)和SE(3)等，傳統(tǒng)濾波方法[1-2]難以獲得理想的結(jié)果，例如，當(dāng)角度不確定性很大時(shí)，濾波器甚至可能發(fā)散。

于浛等[3]給出了具有隨機(jī)時(shí)滯和異步相關(guān)噪聲的非線性系統(tǒng)的高斯濾波器設(shè)計(jì)，對(duì)于含有觀測時(shí)滯和相關(guān)噪聲系統(tǒng)的狀態(tài)估計(jì)問題給出了更高的精度和數(shù)值穩(wěn)定性。對(duì)于包含方向變量的狀態(tài)估計(jì)，許多學(xué)者利用方向統(tǒng)計(jì)學(xué)中的相關(guān)理論及研究成果，針對(duì)一些特殊流形建立了概率分布模型。Mardia等[4]給出了多變量von Mises分布及其參數(shù)估計(jì)方法，G. Kurz等[5]也提出了定義于剛體運(yùn)動(dòng)群SE(2)上的PWN（Partially Wrapped Normal）概率分布， 但這些研究中都沒有提出相應(yīng)的濾波算法。 除此之外，G. Stienne等[6]提出了多傳感器圓融合濾波器，J. Glover等[7]引入了四元數(shù)Bingham濾波器跟蹤高速3D旋轉(zhuǎn)，但這些研究中，狀態(tài)向量中只包含圓變量或四元數(shù)表示的三維旋轉(zhuǎn)量，不包含任何線性變量。

J. T. Horwood等[8]為研究空間態(tài)勢感知（Space Situational Awareness，SSA）中的不確定性傳播問題，提出了Gauss von Mises（GVM）分布。SSA的主要研究目標(biāo)是近地空間環(huán)境中的軌道空間對(duì)象，需要盡可能準(zhǔn)確地獲得空間對(duì)象的不確定性特征，來表示統(tǒng)計(jì)誤差。而在這樣的空間環(huán)境中，由于測量更新的時(shí)間間隔長、誤檢測率高、非線性、不守恒動(dòng)力學(xué)和非高斯現(xiàn)象等原因，采用高斯分布和局部線性假設(shè)無法得到理想的結(jié)果，因此提出了GVM分布，在此基礎(chǔ)上研究了測量更新間隔時(shí)間段內(nèi)的不確定性傳播問題，并得到了很好的結(jié)果。但文中只考慮了時(shí)間更新問題（即預(yù)測步驟），沒有給出融合測量數(shù)據(jù)的量測更新步驟，且給出的參數(shù)估計(jì)方法具有一定的特殊性，難以通用。

本文針對(duì) GVM分布[8]，擴(kuò)展了狄拉克混合逼近方法，給出了遞推濾波過程中聯(lián)合密度的GVM逼近方法，在此基礎(chǔ)上，推導(dǎo)了后驗(yàn)狀態(tài)分布的GVM參數(shù)計(jì)算公式，設(shè)計(jì)了量測狀態(tài)更新算法，通過將[8]中的時(shí)間更新（預(yù)測）方法與本文的量測更新方法相結(jié)合，可實(shí)現(xiàn)完整的GVM濾波器(GVM filter, GVMF)。

1 Gauss Von Mises分布的定義

定義1 GVM分布[8]：隨機(jī)變量(x,θ)∈ Rn×S的聯(lián)合分布滿足Gauss von Mises（GVM）分布，當(dāng)且僅當(dāng)它們的聯(lián)合概率密度函數(shù)具有形式：

記為：(x,θ)~GVM(μ, P,α,β,Γ,κ)。

式(1)中：

P =LLT；參數(shù)集(μ, P,α,β,Γ,κ)必須滿足下列限制：μ ∈ Rn，P是n× n對(duì)稱正定矩陣，α∈ R，β ∈Rn，Γ是n× n對(duì)稱矩陣，κ ≥ 0；L是參數(shù)矩陣P的下三角 Cholesky因子；參數(shù)β和Γ用于對(duì)x和θ的相關(guān)性進(jìn)行建模。

通過變換：

可將GVM(μ, P,α,β,Γ,κ)簡化為規(guī)范形式：

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

從一個(gè)給定的概率密度函數(shù)生成樣本通常采用隨機(jī)采樣的方式，簡單快速，并有許多已有的成熟方法。但在隨機(jī)采樣中，樣本是獨(dú)立隨機(jī)生成的，因此，矩等統(tǒng)計(jì)性質(zhì)收斂到真實(shí)值的速度很慢，需要大量的樣本。除此之外，樣本之間完全獨(dú)立也會(huì)導(dǎo)致對(duì)給定概率密度的非均勻覆蓋。

為克服隨機(jī)采樣的缺點(diǎn)，研究提出了許多確定性采樣方法，其中，多維高斯分布的狄拉克混合逼近方法[8]無需限制點(diǎn)位于坐標(biāo)軸上，可以靈活控制采樣點(diǎn)個(gè)數(shù)，在滿足某些矩匹配約束的同時(shí)，嘗試對(duì)于某種距離測度，最小化原分布與狄拉克逼近之間的距離，可實(shí)現(xiàn)對(duì)給定密度較為均勻的覆蓋。本節(jié)擴(kuò)展了狄拉克混合逼近方法，使其可用于多維GVM分布，該方法包含4個(gè)步驟：

第一步：首先利用狄拉克混合采樣方法對(duì) Rn空間中的規(guī)范高斯分布進(jìn)行采樣，生成q個(gè)采樣點(diǎn)

第二步：通過d個(gè)n× n維正交旋轉(zhuǎn)矩陣，對(duì)q個(gè)采樣點(diǎn)進(jìn)行旋轉(zhuǎn)變換，共生成dq個(gè)采樣點(diǎn)。通過適當(dāng)選擇正交矩陣，這種方法得到的采樣點(diǎn)能夠更好地滿足對(duì)稱性和均勻覆蓋，且對(duì)于規(guī)范高斯分布,能夠保持矩不變。

第三步：得到 Rn空間中的采樣點(diǎn)后，對(duì)于dq個(gè)采樣點(diǎn)中的每個(gè)點(diǎn)進(jìn)行分裂，隨機(jī)生成l個(gè)符合VM(0,κ)分布的角度樣本點(diǎn)，設(shè)分裂系數(shù)為c， 那么表示對(duì)應(yīng)于點(diǎn) y的c個(gè)角度樣本點(diǎn)，因此，可γi得到cdq個(gè) Rn×S空間中的采樣點(diǎn)

3 GVM分布的量測更新算法

考慮如下離散時(shí)間隨機(jī)動(dòng)力系統(tǒng)：(xk,θk)=，其中表示狀態(tài)向量，表示測量值，表示測量噪聲，f是系統(tǒng)轉(zhuǎn)移函數(shù)，g是測量函數(shù)。

貝葉斯遞推濾波包括預(yù)測和量測更新兩個(gè)步驟。對(duì)于GVM分布模型，預(yù)測步驟根據(jù)上一時(shí)刻估計(jì)得到的GVM狀態(tài)分布及系統(tǒng)方程，計(jì)算出預(yù)測狀態(tài)分布的GVM逼近。文獻(xiàn)[8]已給出預(yù)測步的算法流程，本文將主要考慮量測更新步的算法。

3.1 聯(lián)合分布的GVM逼近

為了計(jì)算出后驗(yàn)狀態(tài)分布，我們首先需要計(jì)算出聯(lián)合分布的GVM逼近，這可看作是求后驗(yàn)概率密度的中間步驟[9]，即：給定預(yù)測密度的逼近，我們需要計(jì)算聯(lián)合分布的 GVM 逼近參數(shù)，使得：

對(duì)于聯(lián)合分布，有：

1）線性部分參數(shù)計(jì)算

并有：

對(duì)于時(shí)間更新（預(yù)測步驟）所得到的GVM分布，將利用狄拉克混合逼近方法所求出的采樣點(diǎn)記為，則經(jīng)測量方程變換后可得預(yù)測的測量值：?？衫眠@些點(diǎn)通過最小二乘擬合得到參數(shù)的最優(yōu)估計(jì)。令=，對(duì)參數(shù)的估計(jì)可轉(zhuǎn)化為優(yōu)化問題：

可采用高斯-牛頓、擬牛頓法，結(jié)合例如線性搜索、置信域等方法求解式(6)的非線性最小二乘問題，求得參數(shù)

3.2 后驗(yàn)概率分布的GVM參數(shù)計(jì)算

由于：

對(duì)比式(10)與(11)，令兩式中xk的各次項(xiàng)系數(shù)相等，可解出為：

3.3 量測狀態(tài)更新算法描述

量測更新算法的輸入為：

● 給定預(yù)測分布的GVM逼近：

● 測量值 zk。

算法輸出為k時(shí)刻的后驗(yàn)狀態(tài)密度的GVM逼近

量測更新算法包含以下4個(gè)步驟：

第一步：根據(jù)給定預(yù)測狀態(tài)密度的GVM逼近，利用式(2)~(5)，計(jì)算出、。

第二步：根據(jù)給定預(yù)測狀態(tài)密度的GVM逼近生成q個(gè)狄拉克采樣點(diǎn)，并計(jì)算經(jīng)測量方程變換后的點(diǎn)

該算法中，利用2n+ 3個(gè)sigma點(diǎn)及積分逼近方法可保證對(duì)線性部分一階二階矩的估計(jì)精度，但利用這些 sigma點(diǎn)難以保證參數(shù)的估計(jì)精度，而狄拉克采樣點(diǎn)個(gè)數(shù)可以控制，且覆蓋較為均勻，利用該方法可對(duì)參數(shù)進(jìn)行更精確的估計(jì)。與基于粒子的濾波方法相比，利用狄拉克混合逼近方法所需的采樣點(diǎn)個(gè)數(shù)遠(yuǎn)小于粒子濾波所需的采樣點(diǎn)數(shù)目，且不存在粒子退化問題。

將本文提出的量測更新算法與文獻(xiàn)[8]中提出的時(shí)間更新（預(yù)測）算法相結(jié)合，可以設(shè)計(jì)出GVM濾波器（GVMF）。

4 仿真驗(yàn)證

我們采用文獻(xiàn)[10]中的仿真模型，該模型是非線性問題的一個(gè)典型例子?？紤]一個(gè)在近圓軌道上運(yùn)行的近地空間目標(biāo)，希望利用一系列角測量結(jié)果來估計(jì)它的位置及速度。為簡便，假定所有的運(yùn)動(dòng)和測量都在同一平面上，從而可用極坐標(biāo)r和θ來表示目標(biāo)的運(yùn)動(dòng)。通過牛頓或拉格朗日力學(xué)，可推導(dǎo)出運(yùn)動(dòng)方程：

測量方程為：

利用MATLAB 2008a，分別對(duì)GVMF與擴(kuò)展卡爾曼濾波器（Extended Kalman filter，EKF）進(jìn)行仿真。仿真過程中采用的參數(shù)為：真實(shí)的初始狀態(tài)為：

仿真結(jié)果如圖1、圖2所示，可以看出，GVMF能夠比較快速地收斂到真實(shí)值，而EKF對(duì)角度的估計(jì)誤差隨時(shí)間增加，GVMF對(duì)角的估計(jì)明顯優(yōu)于EKF。

圖1 角速度估計(jì)誤差Fig.1 Angular velocity estimation error

圖2 角估計(jì)誤差Fig.2 Angular estimation error

5 結(jié) 論

本文針對(duì)GVM分布，擴(kuò)展了狄拉克混合逼近方法，給出了聯(lián)合分布的GVM逼近算法，推導(dǎo)出了后驗(yàn)狀態(tài)分布的GVM參數(shù)計(jì)算公式，并在此基礎(chǔ)上，給出了GVM濾波中的量測狀態(tài)更新方法，并對(duì)非線性狀態(tài)方程和測量方程進(jìn)行了仿真。

仿真結(jié)果表明，當(dāng)狀態(tài)向量符合GVM分布時(shí)，利用本文的所提出的量測更新算法所設(shè)計(jì)出的GVMF很好地實(shí)現(xiàn)了濾波，對(duì)角變量的估計(jì)明顯優(yōu)于EKF。

（References）：

[1] 張敏虎, 任章, 華春紅. UKF在深組合GPS/INS導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào), 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組合導(dǎo)航系統(tǒng)中的高斯粒子濾波混和算法[J]. 中國慣性技術(shù)學(xué)報(bào), 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ī)時(shí)滯和異步相關(guān)噪聲的非線性系統(tǒng)的高斯濾波器設(shè)計(jì)[J]. 中國慣性技術(shù)學(xué)報(bào), 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

遼寧省教育廳科學(xué)研究一般項(xiàng)目（L2013083）；國家自然科學(xué)青年基金（61501308）

陳慕羿（1981—），女，講師，從事傳感器融合、目標(biāo)定位跟蹤研究。E-mail: camchenm@163.com

1005-6734(2016)03-0361-05

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