導(dǎo)航信息處理中濾波算法穩(wěn)定性評估方法

王 旺, 趙海軍, 姜 暖

(海軍潛艇學(xué)院, 山東 青島 266042)

濾波算法在導(dǎo)航信息處理中有廣泛應(yīng)用,隨著導(dǎo)航技術(shù)的不斷進步,國內(nèi)外學(xué)者提出了許多適合不同導(dǎo)航信息的濾波算法。穩(wěn)定性是濾波算法得以正常工作的前提,是其性能的重要指標之一。評估濾波算法穩(wěn)定性,有助于判斷其優(yōu)劣,以便為算法的使用和改進提供理論參考。目前,濾波算法穩(wěn)定性評估主要是對其收斂性進行定性判斷。文獻[1]給出了濾波穩(wěn)定性定理,即對于一致完全可控和一致完全可觀測的系統(tǒng),卡爾曼濾波算法是穩(wěn)定的,但實際中往往會產(chǎn)生濾波發(fā)散現(xiàn)象。文獻[1-3]提出了卡爾曼濾波過程的穩(wěn)定性概念,給出了濾波發(fā)散的判定定理。文獻[3]針對組合導(dǎo)航系統(tǒng)濾波算法,給出了性能指標和評估過程。文獻[4-6]從定義出發(fā)研究了算法的穩(wěn)定性,但未給出用于評估濾波算法穩(wěn)定性的指標。

綜合現(xiàn)有文獻,有關(guān)濾波算法穩(wěn)定性評估問題僅局限于穩(wěn)定性概念及收斂判斷條件,并沒有給出評價穩(wěn)定性的技術(shù)指標及精度要求,尚無完整的用于評價濾波算法穩(wěn)定性的指標體系,本文在分析濾波算法穩(wěn)定性影響因素的基礎(chǔ)上,提出濾波算法穩(wěn)定性評估指標,并給出了具體的數(shù)學(xué)表現(xiàn)形式,構(gòu)建其指標體系,結(jié)合卡爾曼濾波在INS/GPS初始對準中的應(yīng)用實例,對卡爾曼濾波的穩(wěn)定性進行了評估,通過計算機仿真驗證了所提指標的有效性。

1 濾波算法穩(wěn)定性評估建模

穩(wěn)定性是系統(tǒng)受到某一擾動后恢復(fù)原有運動狀態(tài)的能力,即如果系統(tǒng)受到有界擾動,不論擾動引起的初始偏差有多大,在擾動撤出后,系統(tǒng)都能以足夠的準確度恢復(fù)到原來的平衡狀態(tài),則稱這種系統(tǒng)是穩(wěn)定的[1]。

濾波算法的穩(wěn)定性是其正常工作的前提,影響導(dǎo)航信息處理中濾波算法穩(wěn)定性的主要方面有:收斂性、發(fā)散性、容錯性及初值敏感性。

1.1 收斂性

收斂性是濾波穩(wěn)定性評估最重要的方面,導(dǎo)航信息處理中濾波算法的目的是導(dǎo)航信息能夠穩(wěn)定收斂。

濾波收斂的判定為:若誤差函數(shù)序列小于等于給定的發(fā)散檢驗閾值Δ,則稱此次濾波收斂。

定義在N次濾波算法中,濾波收斂次數(shù)M與濾波總次數(shù)N的比值M/N為收斂度,記為ηa(0≤ηa≤1),即:

ηa=M/N

(1)

收斂度表征濾波算法收斂能力的強弱。其值越大,收斂能力越強;反之,越弱。收斂度與濾波算法的發(fā)散檢驗閾值Δ、濾波總次數(shù)N有關(guān)。

1.2 發(fā)散性

濾波發(fā)散的判定為:若誤差函數(shù)序列大于給定的發(fā)散檢驗閾值Δ,則稱此次濾波發(fā)散。

定義在N次濾波算法中,濾波發(fā)散次數(shù)L與濾波總次數(shù)N的比值L/N為發(fā)散度,記為ηb(0≤ηb≤1),即

ηb=L/N

(2)

發(fā)散度表征濾波算法發(fā)散能力的強弱。其值越大,越容易發(fā)散;反之,越不易發(fā)散。發(fā)散度也與濾波算法的發(fā)散檢驗閾值Δ、濾波總次數(shù)N有關(guān)。

根據(jù)上述定義,收斂度和發(fā)散度有如下關(guān)系:

ηa+ηb=1

(3)

1.3 容錯性

當(dāng)濾波算法的輸入存在較大的波動或偏差(離群值)時,算法仍能滿足一定導(dǎo)航精度要求的能力,稱為容錯能力或抗離群值能力。當(dāng)離群值作為量測量進入濾波時,會對系統(tǒng)的濾波精度產(chǎn)生較大的影響,其影響可看作濾波算法的脈沖響應(yīng)。描述濾波算法容錯能力的指標主要有二次收斂時間和容錯濾波精度。

當(dāng)離群值作為量測量進入濾波算法時,會影響后續(xù)濾波算法的收斂,濾波算法在離群值出現(xiàn)后至再次穩(wěn)定收斂所經(jīng)歷的時間,稱為二次收斂時間t2。二次收斂時間是一時間區(qū)間,區(qū)間的大小反映了濾波算法在離群值存在的條件下再次穩(wěn)定收斂的快慢。t2越大,濾波算法再次穩(wěn)定收斂的速度越慢;反之,則越快。

t2=tw-tl

(4)

其中，tw為濾波值再次穩(wěn)定收斂時刻;tl為離群值出現(xiàn)時刻。

容錯濾波精度定義如下:

(5)

容錯濾波精度為t2時間段內(nèi)誤差函數(shù)序列與相對真值比值的均值,反映了在離群值存在的條件下濾波值相對于相對真值的平均偏離程度,是一種相對精度。R值越大,濾波算法的抗離群值能力就越強;反之,則越弱。

1.4 初值敏感性

濾波算法的初值影響其精度及收斂時間。當(dāng)濾波選取不同的初值,算法仍能穩(wěn)定收斂至規(guī)定精度的能力,稱為抗初值能力。其模型如下:

(6)

其中:ΔXw為穩(wěn)態(tài)誤差;Δ為發(fā)散檢驗閾值。

q值越大,濾波算法對初值越敏感;反之,則越不敏感。

1.5 濾波算法穩(wěn)定性評估指標體系

基于上述分析,得到導(dǎo)航信息處理中濾波算法穩(wěn)定性評估的指標體系，如圖1所示。

圖1 濾波算法穩(wěn)定性評估指標體系

2 仿真及結(jié)果

本文將卡爾曼濾波算法應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)初始對準中,并根據(jù)上述建立的濾波算法穩(wěn)定性評估指標體系對卡爾曼濾波算法的穩(wěn)定性進行評估。

2.1 動基座下初始對準動態(tài)方程的建立

在位置/速度組合模式中,將INS/GPS組合導(dǎo)航系統(tǒng)的誤差量(位置、速度、平臺誤差角)作為濾波器的估計對象,動基座無阻尼慣導(dǎo)的誤差方程[7]即可作為狀態(tài)方程的一部分。為便于對慣性器件的誤差源進行估計,陀螺漂移εx、εy、εz和加速度計誤差ΔAx、ΔAy也應(yīng)表示為系統(tǒng)的狀態(tài)。假定3個陀螺漂移誤差模型相同,均為一階馬爾科夫過程和白噪聲之和,加速度計誤差模型為零位偏置和白噪聲之和,則濾波器狀態(tài)方程如下:

(7)

式中,選取狀態(tài)變量X=[δVx,δVx,δφ,φλ,φx,φy,φz,εx,εy,εz,ΔAx,ΔAy];F(t)為狀態(tài)轉(zhuǎn)移矩陣,由慣導(dǎo)誤差方程[7]列出;w(t)為噪聲向量,w(t)=[wAx,wAy,0,0,wgx,wgy,wgz,wmx,wmy,wmz,0,0]。wAx,wAy為加速度計誤差的白噪聲,wgx,wgy,wgz為陀螺漂移白噪聲,wmx,wmy,wmz為激勵白噪聲。

令φt、λt為載體實際位置,vtX,vtY為載體實際速度,則慣導(dǎo)系統(tǒng)位置和速度可表示為

(8)

式中,φI、λI為慣導(dǎo)輸出經(jīng)緯度,vtIX、vtIY為慣導(dǎo)輸出速度,δφ、δλ、δvX、δvY為慣導(dǎo)系統(tǒng)誤差。

GPS位置和速度可表示為

(9)

式中,φG、λG為GPS輸出經(jīng)緯度,vtGX、vtGY為GPS輸出速度,δvGX、δvGY為GPS速度誤差,NE、NN分別為GPS在經(jīng)度方向和緯度方向的距離誤差,為簡便計,認為NE、NN為白噪聲,R為地球半徑。

在位置/速度組合模式中,定義量測向量:

(10)

位置/速度組合模式下INS/GPS組合導(dǎo)航系統(tǒng)的量測方程為

Y(t)=H(t)X(t)+V(t)

(11)

式中,

對于線性組合Z(t)=Lx(t),取

2.2 仿真條件

設(shè)載體為艦船,航速10kn,航跡如圖2所示(橫軸為經(jīng)度,縱軸為緯度),包括正東向航行(航行時間20min)、圓周航行(航行時間10min)、正北向航行(航行時間15min)、西南向航行(航行時間40min)四個階段。

圖2 仿真時的艦船“航跡”

取采樣時間T=1s,地球自轉(zhuǎn)角速率wie=7.29212e-5 rad/s,重力加速度g=9.78。假定選用一低精度慣導(dǎo),其等效陀螺漂移為0.1°/h,隨機漂移為0.001°/h,陀螺漂移相關(guān)時間為1h;加速度計零位偏置為10-4g,隨機偏差為10-8;慣導(dǎo)平臺的初始誤差為φx0=5′、φy0=5′、φz0=20′,慣導(dǎo)初始定位誤差北向和東向均為200m,初始速度誤差為1m/s;GPS接收機輸出的位置誤差北向和東向均為25m,速度誤差為0.1m/s。

2.3 卡爾曼濾波算法穩(wěn)定性評估

條件1:

卡爾曼濾波算法的初值如下:

X0=[1m/s,1m/s,0,0,5′,5′,20′, 0.001°/h, 0.001°/h, 0.001°/h, 10-4g, 10-4g]

P0=diag{(1m/s)2,(1m/s)2,0,0,(5′)2,(5′)2,(20′)2,(0.001°/h)2,(0.001°/h)2,(0.001°/h)2,(10-4g)2,(10-4g)2}

Q0=diag{(10-8)2,(10-8)2,0,0,(10-8)2,(10-8)2,(10-8)2,(10-8)2, (10-8)2,(10-8)2,0,0}

R0=diag{(0.1m/s)2,(0.1m/s)2, (25m)2,

(25m)2}

狀態(tài)初值X0和估計誤差方差矩陣的初值P0為無偏的,即X0=E[X0]、P0=Var[X0]。發(fā)散檢驗閾值Δ1=Δ2=0.4′,Δ3=1.5′。穩(wěn)定性評估指標數(shù)值如表1所示。誤差估計結(jié)果如圖3-5所示。

圖3 東向水平失準角估計誤差

圖4 北向水平失準角估計誤差

圖5 方位失準角估計誤差

項目東向水平失準角北向水平失準角方位失準角收斂度0.99510.99980.9686發(fā)散度0.00490.00020.0314容錯濾波精度———二次收斂時間———初值敏感值0.43670.29210.3898

條件2:

保持條件1的初值選取不變,引入離群值:慣導(dǎo)輸出的東向速度在1000s:1010s=-8.0467m/s;發(fā)散檢驗閾值Δ1=Δ2=0.4′,Δ3=1.5′。條件2下的穩(wěn)定性評估指標的數(shù)值如表2。

表2 條件2下的穩(wěn)定性評估指標數(shù)值

條件3:

改變?yōu)V波算法的初值X0=[0,0,0,0,0,0,0, 0, 0, 0, 0,0];P0=diag{0,0,0,0,0,0,0, 0, 0, 0, 0,0};Q0和R0保持不變;發(fā)散檢驗閾值Δ1=Δ2=0.4′,Δ3=1.5′,此時不加入野值。評估數(shù)值如表3所示。

表3 條件3下的穩(wěn)定性評估指標數(shù)值

由表1、表2,得出以下結(jié)論:在相同的發(fā)散檢驗閾值Δ1=Δ2=0.4′,Δ3=1.5′下,加入離群值后：

1)水平失準角的收斂度變小,方位失準角的收斂度沒有變化;

2)與離群值相關(guān)的北向失準角的容錯濾波精度為0.6215,即濾波值相對于相對真值平均偏離了62.15%。二次收斂時間為68s,再次穩(wěn)定收斂的時間較長,說明了卡爾曼濾波算法的抗離群值能力較弱。

由表1、表3,得出以下結(jié)論:在相同的發(fā)散檢驗閾值Δ1=Δ2=0.4′,Δ3=1.5′下,改變?yōu)V波初值后：

1)三個失準角的收斂度均變小,且變化較大,可看出卡爾曼濾波算法的初值對濾波收斂及發(fā)散有較大影響;

2)由表3,三個失準角的初值敏感值均大于1,即穩(wěn)態(tài)收斂誤差大于發(fā)散檢驗閾值,說明卡爾曼濾波算法的初值影響其穩(wěn)態(tài)收斂誤差,抗初值能力較差。

綜上,卡爾曼濾波算法在滿足其假設(shè)條件下,穩(wěn)定性好;但其抗離群值能力較差;當(dāng)初值不滿足無偏估計時,收斂性較差,抗初值能力較差。評估結(jié)果與理論分析一致。

3 結(jié)束語

穩(wěn)定性是導(dǎo)航信息估計算法正常運行的前提和重要保障。本文以卡爾曼濾波在INS/GPS初始對準中的應(yīng)用實例,得到了與理論分析一致的結(jié)論。但文中仿真條件只涉及了載體的航跡運動,載體艦船的姿態(tài)運動對濾波穩(wěn)定性同樣具有一定的影響,這也是需進一步分析的因素,有關(guān)這方面的內(nèi)容將在后續(xù)工作中進一步深入研究。

[1] 邱愷,黃國榮,陳天如,等.卡爾曼濾波過程的穩(wěn)定性研究[J]. 系統(tǒng)工程與電子技術(shù)，2005,27(1):33-35.

[2] K. You, M. Fu, L. Xie. Mean square stability for Kalman filtering with markovian packet losses[J]. Automatica， 2011,47(12):2647-2657.

[3] 邱愷,黃國榮,陳天如，等.基于濾波過程的卡爾曼濾波發(fā)散判定方法[J].系統(tǒng)工程與電子技術(shù)， 2005,27(2):230-231.

[4] 劉勇志,賈興亮,劉丙杰.一種組合導(dǎo)航濾波性能評估算法[J].彈箭與制導(dǎo)學(xué)報，2008,28(5):45-47，50.

[5] 劉國良,張迎春,孫增圻.聯(lián)邦濾波器的濾波穩(wěn)定性研究[J].中國慣性技術(shù)學(xué)報，2004,12(6):28-31.

[6] 高法欽,談?wù)怪?北斗組合動態(tài)定位算法穩(wěn)定性分析[J].電子與信息學(xué)報，2007,29(10):2388-2391.

[7] 陳永冰,鐘斌.慣性導(dǎo)航原理[M].北京:國防工業(yè)出版社,2007.

[8] 閻紅星,王曉博,王國宏.跟蹤濾波算法性能評估研究[J].現(xiàn)代雷達，2008,30(4):33-36.

[9] Boyi Ni, Qinghua Zhang. Stability of the Kalman Filter for Output Error Systems[J]. IFAC,2015，48(28):1106-1111.

[10] A li T A, John E G, Mc Cabe D H. Performance Evalunation of a Asynchronous Multisensor Track Fusion Filter[C]∥Proceedings of SPIE,2003.

[11] Junfeng Wua, Ling Shi b, Lihua Xie.An improved stability condition for Kalman filtering with bounded Markovian packet losses[J]. Automatica, 2015,62(C):32-38.