平方根球形無(wú)味卡爾曼濾波機(jī)載無(wú)源定位算法

裴 畔， 丁永紅， 馬鐵華

（1.中北大學(xué) 儀器科學(xué)與動(dòng)態(tài)測(cè)試教育部重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051；2.中北大學(xué) 電子測(cè)試技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051）

平方根球形無(wú)味卡爾曼濾波機(jī)載無(wú)源定位算法

裴 畔1，2， 丁永紅1，2， 馬鐵華1，2

（1.中北大學(xué) 儀器科學(xué)與動(dòng)態(tài)測(cè)試教育部重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051；2.中北大學(xué) 電子測(cè)試技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051）

針對(duì)機(jī)載無(wú)源定位系統(tǒng)中，初始值誤差和數(shù)值的舍入計(jì)算對(duì)無(wú)味卡爾曼濾波（un-scented Kalman filtering，UKF）算法的定位精度和濾波穩(wěn)定性影響較大的問題，提出一種基于平方根球形無(wú)味的卡爾曼濾波算法（square root spherical unscented Kalman filter，Sqrt-UKFST）。該方法以單位超球體球面無(wú)味變換為基礎(chǔ)，通過減少采樣點(diǎn)數(shù)目和球面半徑，保證所有采樣點(diǎn)在一個(gè)單位超球體上，從而提高算法對(duì)初始值的魯棒性，并采用平方根濾波提高算法的數(shù)值穩(wěn)定性。對(duì)該算法進(jìn)行100次Monte-Carlo實(shí)驗(yàn)，仿真結(jié)果表明，Sqrt-UKFST算法收斂速度快，濾波性能穩(wěn)定；當(dāng)初始狀態(tài)估計(jì)誤差較大時(shí)，Sqrt-UKFST算法的定位精度保持在30%以內(nèi)，提高系統(tǒng)對(duì)初始值的魯棒性。

機(jī)載無(wú)源定位；算法精度；平方根球形無(wú)味卡爾曼濾波；球面無(wú)味變換；魯棒性；穩(wěn)定性

0 引 言

在電子對(duì)抗日趨激烈的今天，機(jī)載單站無(wú)源定位系統(tǒng)自身不發(fā)射信號(hào)，只接收輻射源目標(biāo)發(fā)射的電磁波信號(hào)，具有隱蔽性能好、生存力強(qiáng)、設(shè)備簡(jiǎn)單、跟蹤定位性能好的優(yōu)點(diǎn)，在對(duì)地面靜止目標(biāo)或慢速運(yùn)動(dòng)的目標(biāo)定位上具有重要的應(yīng)用價(jià)值[1]；文獻(xiàn)[2]基于多普勒變化率實(shí)現(xiàn)對(duì)固定目標(biāo)的定位；文獻(xiàn)[3]利用相位差變化率對(duì)地面固定目標(biāo)進(jìn)行定位；文獻(xiàn)[4]基于相位差變化率和多普勒變化率相結(jié)合的定位方法將多普勒頻率變化率法觀測(cè)參數(shù)中的角度變化率由相位差變化率法中的相位差變化率參數(shù)替換，提高算法的實(shí)時(shí)性和實(shí)用性。機(jī)載無(wú)源定位是一個(gè)典型的非線性濾波問題，對(duì)含有噪聲的數(shù)據(jù)進(jìn)行觀測(cè)，從而估計(jì)目標(biāo)的狀態(tài)。為了提高系統(tǒng)的定位精度和穩(wěn)定性能，需要研究跟蹤濾波算法，對(duì)觀測(cè)信息進(jìn)行最大限度的利用。

文獻(xiàn)[5]采用修正EFPF算法進(jìn)行濾波，標(biāo)準(zhǔn)的無(wú)味卡爾曼濾波（unscented Kalman filter，UKF）算法在擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）算法的基礎(chǔ)上，摒棄了線性化近似模型的做法，將隨機(jī)變量的概率分布進(jìn)行近似，利用比例修正的對(duì)稱采樣策略，對(duì)一個(gè)n維狀分布態(tài)空間，選取2n+1個(gè)采樣點(diǎn)，提高了定位精度，不用計(jì)算雅克比矩陣，降低了算法的復(fù)雜度[6-7]。文獻(xiàn)[8]和文獻(xiàn)[9]采用改進(jìn)的UKF算法提高系統(tǒng)的濾波性能。基于施密特正交變換（schmidt orthogonal transform，SOT）采樣策略的UKF（SOUKF）算法，經(jīng)過施密特正交變換后確定n+1個(gè)采樣點(diǎn)，減少了UKF算法所需采樣點(diǎn)的數(shù)目，提高了計(jì)算效率[10]。文獻(xiàn)[11]采用的平方根UKF雙向?yàn)V波算法，提高了濾波性能，但計(jì)算比較復(fù)雜。本文提出了一種平方根球形無(wú)味卡爾曼濾波（square root spherical unscented Kalman filter，Sqrt-UKFST）算法，提高實(shí)時(shí)性的同時(shí)，采用單位超球體球面無(wú)味變換的采樣策略，在濾波算法遞推過程中采用平方根濾波的方法，提高了算法的數(shù)值穩(wěn)定性及對(duì)初始值的魯棒性。

1 機(jī)載無(wú)源定位系統(tǒng)模型

以二維平面直角坐標(biāo)系為例（見圖1），假設(shè)飛機(jī)在k時(shí)刻的位置坐標(biāo)為（xOk，yOk），速度為（目標(biāo)輻射源在k時(shí)刻的位置坐標(biāo)為（xTk，yTk），速度為飛機(jī)O和目標(biāo)輻射源T在k時(shí)刻的狀態(tài)向量分別為

圖1 飛機(jī)對(duì)目標(biāo)輻射源定位的二維示意圖

1.1 系統(tǒng)的狀態(tài)方程

假設(shè)飛機(jī)做勻速直線運(yùn)動(dòng)，忽略加速度項(xiàng)。目標(biāo)輻射源相對(duì)于飛機(jī)是勻速運(yùn)動(dòng)，相對(duì)運(yùn)動(dòng)狀態(tài)向量是

系統(tǒng)的狀態(tài)方程表示為

其中，xk和yk表示k時(shí)刻目標(biāo)的位置變量，x˙k和y˙k表示k時(shí)刻目標(biāo)的速度變量。表示狀態(tài)轉(zhuǎn)移矩陣，T表示測(cè)量周期，I2表示二階單位矩陣；是噪聲轉(zhuǎn)移矩陣[10]，狀態(tài)噪聲是二維零均值高斯白噪聲，其協(xié)方差矩陣是Qk。

1.2 系統(tǒng)的觀測(cè)方程

本文采用多普勒變化率的機(jī)載無(wú)源定位方法，只需要測(cè)量方位角、方位角變化率以及目標(biāo)相對(duì)于飛機(jī)的多普勒變化率。系統(tǒng)的觀測(cè)方程表示為

其中，fT表示目標(biāo)輻射源輻射的頻率，c表示電磁波傳播速度表示飛機(jī)距離目標(biāo)之間的相對(duì)距離，h（Xk，νk）是從狀態(tài)向量向觀測(cè)向量轉(zhuǎn)化的非線性函數(shù)，量測(cè)噪聲是零均值高斯白噪聲，其協(xié)方差矩陣是

2 Sqrt-UKFST算法

2.1 單位超球體球面無(wú)味變換

UKF算法的計(jì)算成本與采樣點(diǎn)σ的數(shù)量成正比，有必要減少σ點(diǎn)。目前σ點(diǎn)的采樣策略有：無(wú)味、單純形和球形變換。假設(shè)系統(tǒng)的維度是n，無(wú)味變換需要2n+1個(gè)σ點(diǎn)；單純形變換需要n+1個(gè)σ點(diǎn)，它的數(shù)值穩(wěn)定性與球體半徑2n/2有關(guān)[12-13]。本文提出的球形變換需要n+2個(gè)σ點(diǎn)，通過減少球面半徑至提高系統(tǒng)的數(shù)值穩(wěn)定性。

其中，初始權(quán)重W0只影響選擇的四階及高階的σ點(diǎn)。球面半徑上的σ點(diǎn)分布取決于估計(jì)狀態(tài)向量的大小，保證所有σ點(diǎn)在一個(gè)單位超球體上。單位超球體球面無(wú)味變換步驟如下：

1）選擇初始權(quán)重0≤W0≤1。

2）計(jì)算權(quán)重的其余部分

其中，Wi表示均值和協(xié)方差加權(quán)的權(quán)值，i=1，2，…，n+1。

3）元素向量初始化

4）遞推擴(kuò)展后（j=2，…，n），迭代公式如下：

2.2 平方根無(wú)味卡爾曼濾波器

對(duì)于標(biāo)準(zhǔn)的UKF算法，協(xié)方差矩陣Pk是通過公式Pk=SkSTk更新和分解到矩陣廣根Sk，σ點(diǎn)映射在每個(gè)時(shí)間步。然后，Pk矩陣重構(gòu)來(lái)傳播更新σ點(diǎn)。Sqrt-UKFST算法直接傳播和更新Sk，沒有分解和重構(gòu)矩陣Pk的需要，節(jié)省了重構(gòu)Pk在每步的時(shí)間。平方根UKF算法利用3種方法更新和傳播：QR分解、喬里斯基因素更新（cholupdate）和最小二乘法[14]。Sqrt-UKFST算法步驟如下：

1）設(shè)置初始狀態(tài)均值和協(xié)方差

其中，chol表示cholesky分解。

2）計(jì)算σ點(diǎn)的預(yù)測(cè)值，i=0，1，…，n+1

3）狀態(tài)估計(jì)預(yù)測(cè)

4）估計(jì)狀態(tài)均值

5）協(xié)方差平方根遞推和更新

6）估計(jì)測(cè)量rk和均值

7）計(jì)算協(xié)方差測(cè)量Syˉk1和它的更新值

8）計(jì)算交叉協(xié)方差矩陣

9）卡爾曼增益Kk和狀態(tài)估計(jì)更新

10）更新協(xié)方差矩陣

3 仿真條件與結(jié)果分析

假設(shè)飛機(jī)起始點(diǎn)在坐標(biāo)原點(diǎn)，作水平勻速直線運(yùn)動(dòng)且忽略姿態(tài)變化，速度為vx=vy=300m/s。飛機(jī)對(duì)平面上運(yùn)動(dòng)的目標(biāo)輻射源進(jìn)行觀測(cè)，目標(biāo)的起始位置為 x0=2 km，y0=10 km，速度為 vx=vy=－15 m/s，加速度擾動(dòng)噪聲的協(xié)方差矩陣為Q=0.001I2×2。對(duì)x方向和y方向分別進(jìn)行觀測(cè)，觀測(cè)周期 1 s，持續(xù)時(shí)間100s，測(cè)量精度為σβ=3mrad，σfd=1Hz，σβ′=0.1mrad/s，σfd′=0.5Hz/s。

濾波誤差的均值用來(lái)描述算法的穩(wěn)定性能，計(jì)算公式[14]為

式中：M——蒙特卡洛模擬次數(shù)；

N——采樣次數(shù)，k=1，2，…，N。

圖2 X方向和Y方向的估計(jì)誤差均值

圖3 RRE的統(tǒng)計(jì)平均值

表1 不同算法的單次運(yùn)行時(shí)間

相對(duì)距離誤差（relative range error，RRE）用來(lái)評(píng)估算法的收斂性能。RRE的計(jì)算公式[15-16]為

其中（xk，yk）和分別表示目標(biāo)在k時(shí)刻的真實(shí)位置和估計(jì)位置。

為了驗(yàn)證算法的穩(wěn)定性，收斂性和定位精度，對(duì)UKF、SOUKF和Sqrt-UKFST 3種算法分別進(jìn)行100次Monte-Carlo實(shí)驗(yàn)，得到濾波誤差在兩個(gè)方向上的均值曲線和RRE的統(tǒng)計(jì)平均值。分別如圖2、圖3和表1所示。

由圖2可以看出，Sqrt-UKFST算法比UKF算法和SOUKF算法的濾波性能都更穩(wěn)定，曲線波動(dòng)更小。

由圖3可以看出，在相同的觀測(cè)精度下，Sqrt-UKFST算法的定位精度最好且收斂速度最快，SOUKF算法次之，UKF算法最差。忽略曲線中個(gè)別發(fā)散點(diǎn)，若RRE<15%，則實(shí)驗(yàn)結(jié)果收斂，否則發(fā)散。

由表1可以看出，Sqrt-UKFST算法和SOUKF算法的運(yùn)算時(shí)間比UKF算法短，Sqrt-UKFST算法運(yùn)算時(shí)間比SOUKF算法略多，仍然可以滿足實(shí)時(shí)性要求。

圖4 初始狀態(tài)估計(jì)相對(duì)誤差變化時(shí)3種算法定位精度曲線

表2 初始狀態(tài)估計(jì)相對(duì)誤差變化時(shí)3種算法定位精度 %

為了檢驗(yàn)Sqrt-UKFST算法對(duì)初始值的魯棒性，初始狀態(tài)估計(jì)相對(duì)誤差取從0到100%的11組值。在不同的初始狀態(tài)估計(jì)相對(duì)誤差下，作100次Monte-Carlo實(shí)驗(yàn)，將RRE的統(tǒng)計(jì)平均值作為算法的定位精度，仿真結(jié)果如表2和圖4所示。

由表2可以看出，在初始狀態(tài)估計(jì)誤差較低時(shí)，3種算法的定位精度都保持在15%以內(nèi)，收斂性良好。當(dāng)初始狀態(tài)估計(jì)誤差較大時(shí)，Sqrt-UKFST算法的定位精度保持在30%以內(nèi)，UKF算法的定位精度達(dá)58.1%。

由圖4可以看出，UKF算法的變化程度最大，說(shuō)明該算法對(duì)初始值估計(jì)相對(duì)誤差最敏感，而Sqrt-UKFST算法的變化程度最小，說(shuō)明該算法對(duì)初始值的魯棒性最好。

4 結(jié)束語(yǔ)

本文在采用角度、角度變化率和多普勒變化率的機(jī)載無(wú)源定位方法的基礎(chǔ)上，提出了一種基于平方根球形無(wú)味的卡爾曼濾波算法（Sqrt-UKFST）。該算法以單位超球體球面無(wú)味變換為采樣策略，在滿足實(shí)時(shí)性要求的條件下，保證所有采樣點(diǎn)在一個(gè)單位超球體上，提高算法對(duì)初始值的魯棒性。同時(shí)，在濾波算法遞推過程中采用平方根濾波的方法，提高了濾波的穩(wěn)定性和收斂性。仿真結(jié)果也表明，Sqrt-UKFST算法比UKF算法和SOUKF算法具有更好的跟蹤性能，對(duì)機(jī)載無(wú)源定位系統(tǒng)具有重要的應(yīng)用價(jià)值。

[1]劉學(xué).機(jī)載無(wú)源定位技術(shù)與跟蹤算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2012.

[2]平殿發(fā)，司文健，蘇峰.基于多普勒變化率的機(jī)載無(wú)源定位方法研究[J].中國(guó)雷達(dá)，2010，29（4）：4-6.

[3]侯建民.基于相位差變化率的空中運(yùn)動(dòng)單站對(duì)地面固定目標(biāo)的無(wú)源定位[D].合肥：合肥工業(yè)大學(xué)，2015.

[4]張志.機(jī)載單站無(wú)源定位關(guān)鍵技術(shù)研究[D].南京：南京理工大學(xué)，2013.

[5]申正義，閆抒升，王曉軍，等.修正的EKPF算法在固定單站被動(dòng)目標(biāo)跟蹤中的應(yīng)用[J].現(xiàn)代防御技術(shù)，2015（2）：116-121，153.

[6]黃耀光，李建新，高博.基于UKF的單站無(wú)源定位改進(jìn)算法[J].現(xiàn)代雷達(dá)，2012，34（11）：45-48.

[7]QIU L B，SU H Y，HAO H，et al.Application of UKF algorithm in airborne single observer passive location[J]. Advanced Materials Research，2011（267）：356-362.

[8]AI-HUSSEIN A，HALDAR A.Unscented Kalman filter with unknown input and weighted global iteration for health assessment of large structural systems[J].Structural Control and Health Monitoring，2016，23（1）：156-175.

[9]ZHAO H S，WEN Z Y，YU H P.A new method of multi-sensor data fusion based on multiscale analysis and UKF[J].Internatonal Journal of Control and Automation，2016，9（2）：329-342.

[10]黃耀光，高博，李建新，等.基于施密特正交變換UKF的單站無(wú)源定位算法[J].電光與控制，2013，20（02）：37-40.

[11]黃耀光，高博，李建新，等.基于平方根UKF雙向?yàn)V波的單站無(wú)源定位算法[J].數(shù)據(jù)采集與處理，2013，28（02）：207-212.

[12]ZHAO K C，YOU Z.Square root spherical simplex unscented kalman filter for micro satellite attitude measurement[A].2nd International Conference on Information Engineering and Computer Science-Proceedings(ICIECS2010)[C]∥2nd International Conference on Information Engineering and Computer Science-Proceedings. ICIECS，2010.

[13]HTET A，KAY S L，SHU T G.State-of-charge estimation of lithiumion battery using square root spherical unscented kalman filter（Sqrt-UKFST）in nanosatellite[J]. IEEE Journals&Magazines，2015，30（9）：4774-4783.

[14]LI W X，HUANG C Q，WANG Y，et al.An improved airborne passive location&tracking algorithm using the rate-of-change ofphase difference[J].Journal of Electronics and Information Technology，2013，35（1）：68-73.

[15]萬(wàn)某峰，趙長(zhǎng)勝.UKF濾波中蒙特卡洛采樣策略比較分析[J].測(cè)繪通報(bào)，2012（12）：37-39.

[16]黃小平，王巖.卡爾曼濾波原理及應(yīng)用：Matlab仿真[M].北京：電子工業(yè)出版社，2015：71-79.

（編輯：劉楊）

Airborne passive location algorithm based on spherical square root unscented Kalman filter

PEI Pan1，2，DING Yonghong1，2，MA Tiehua1，2
（1.Key Lab of Instrumentation Science&Dynamic Measurement，North University of China，Taiyuan 030051，China；2.Key Laboratory of Electronic Measurement Technology，North University of China，Taiyuan 030051，China）

As the unscented Kalman filtering（UKF）in airborne passive location has greater impact on positioning accuracy and stability because of the initial value and numerical calculation error，an improved smoothing algorithm based on square root spherical unscented Kalman filter（Sqrt-UKFST）is presented.To guarantee that all sampling points on a unit hypersphere algorithm to improve the robustness of the initial value，the algorithm uses the unit hypersphere sphere tasteless converted by reducing the number of sampling points and the spherical radius.And the algorithm utilizes the square root matrix in the process of estimation to improve the stability of the filter. After 100 times Monte-Carlo experiments，simulation results show that the Sqrt-UKFST algorithm has better performance in the filter’s stability，convergence velocity and the robustness of the initial value.When the initial state estimation reaches big error，the positioning precision maintained less than 30%.

airborne passive location； algorithm accuracy； Sqrt-UKFST； sphericaltasteless transformation；robustness；stability

A

：1674-5124（2017）02-0093-05

10.11857/j.issn.1674-5124.2017.02.019

2016-06-06；

：2016-08-07

國(guó)家自然科學(xué)基金項(xiàng)目（61471385）

裴 畔（1991-），女，山西臨汾市人，碩士研究生，專業(yè)方向?yàn)闄C(jī)載無(wú)源定位。