基于無(wú)跡卡爾曼濾波的單站混合定位跟蹤算法

劉 翔，宋常建，胡 磊，鐘子發(fā)

（1.解放軍電子工程學(xué)院電子制約技術(shù)重點(diǎn)實(shí)驗(yàn)室，安徽 合肥 230037；2.解放軍汽車管理學(xué)院，安徽 蚌埠 230011）

0 引言

隨著蜂窩網(wǎng)移動(dòng)通信技術(shù)的迅猛發(fā)展，移動(dòng)臺(tái)無(wú)線定位技術(shù)已成為研究熱點(diǎn)。目前，到達(dá)場(chǎng)強(qiáng)（SSOA，Single Strength of Arrival）、到達(dá)入射角度（AOA，Angle of Arrival）、到達(dá)時(shí)間（TOA，Ti me of Arrival）、到達(dá)時(shí)間差（TDOA，Ti me Difference of Arrival）及其融合而成的混合定位技術(shù)，如TDOA／AOA，TOA／AOA等，在商業(yè)化應(yīng)用上已取得成功［1］。但以上定位技術(shù)大多要求用于定位的參數(shù)由基站或移動(dòng)臺(tái)主動(dòng)支持［2］，且大多為建立靜態(tài)模型，并在一定的準(zhǔn)則（如最小二乘、最大似然等）下求得最優(yōu)解進(jìn)而得到估計(jì)位置，不能實(shí)現(xiàn)實(shí)時(shí)跟蹤的需求。針對(duì)移動(dòng)臺(tái)的動(dòng)態(tài)跟蹤定位問(wèn)題，提出了一種基于TSOA［3］／TDOA混合被動(dòng)單站定位模型的無(wú)跡卡爾曼濾波跟蹤算法。

1 TSOA／TDOA與UKF原理

單站式跟蹤通常是一個(gè)非線性估計(jì)問(wèn)題，非線性估計(jì)中應(yīng)用最為廣泛和成熟的是擴(kuò)展卡爾曼濾波器（EKF，Extended Kal man Flter）。但是EKF算法存在復(fù)雜度高、非線性誤差大、應(yīng)用環(huán)境苛刻等問(wèn)題［4］。為此，Juiler等人提出了 UKF（Unscented Kal man Filter）算法［5］，其各方面性能明顯優(yōu)于EKF算法［6－9］，廣泛應(yīng)用于諸多領(lǐng)域。

1.1 TSOA／TDOA定位實(shí)現(xiàn)原理

TSOA／TDOA定位技術(shù)是一種基于到達(dá)時(shí)間和（TSOA）與到達(dá)時(shí)間差（TDOA）的新型混合定位技術(shù)，其定位實(shí)現(xiàn)原理圖如圖1所示。定位系統(tǒng)由定位設(shè)備、基站、目標(biāo)用戶三者構(gòu)成，定位設(shè)備發(fā)起和實(shí)現(xiàn)定位過(guò)程，基站和目標(biāo)用戶都是被動(dòng)參與者。

圖1為TSOA／TDOA定位示意圖。在用戶和基站通信時(shí)要求用戶上報(bào)系統(tǒng)幀號(hào)時(shí)間差SFN-SFN［10］，當(dāng)主基站獲得SFN-SFN且已知與相鄰基站的定時(shí)偏移，即可獲得二者的發(fā)射信號(hào)到達(dá)目標(biāo)用戶的時(shí)間差（TDOA）。設(shè)目標(biāo)用戶信號(hào)到達(dá)主基站（為目標(biāo)用戶服務(wù)的基站）和相鄰基站i的時(shí)間之差為Δti，目標(biāo)用戶信號(hào)到達(dá)主基站的時(shí)間為tb，到達(dá)相鄰基站i的時(shí)間為ti，則TDOA為：Δti＝ti－tb。目標(biāo)用戶接收到該基站下行信號(hào)后，延遲T0向基站發(fā)射信號(hào)［9］，定位臺(tái)分別截獲基站下行信號(hào)和目標(biāo)用戶上行信號(hào)的接收時(shí)刻，即可測(cè)得其接收基站信號(hào)與接收目標(biāo)用戶信號(hào)的時(shí)間差Δt。定義τx為目標(biāo)用戶信號(hào)到達(dá)定位臺(tái)的傳播時(shí)間，τy為基站信號(hào)到達(dá)目標(biāo)用戶的傳播時(shí)間，τz為基站信號(hào)到達(dá)定位臺(tái)的傳播時(shí)間，設(shè)基站發(fā)射信號(hào)的起始時(shí)刻t0，則有Δt＝（t0

圖1 TSOA／TDOA定位原理示意圖Fig.1 Theory of TSOA／TDOA locating

1.2 UKF算法原理

UKF算法是一種遞歸式貝葉斯估計(jì)方法，它用一組確定的取樣點(diǎn)來(lái)近似后驗(yàn)概率，但是UKF算法不必線性化非線性狀態(tài)方程或測(cè)量方程，能直接利用非線性狀態(tài)方程來(lái)估計(jì)狀態(tài)向量的概率密度函數(shù)，一定程度上避免了EKF線性化過(guò)程中忽略高階項(xiàng)帶來(lái)的誤差問(wèn)題。UKF算法精度可達(dá)到二階甚至更高階，在非線性系統(tǒng)中可以獲得優(yōu)于EKF算法的濾波效果。

UKF算法步驟如下所述：

1）初始化，

式中，Xα＝［XTWTVT］T是由狀態(tài)變量X過(guò)程噪聲W和觀測(cè)噪聲V擴(kuò)展成的新增廣狀態(tài)向量。

2）對(duì)于k＝1，2，3，…，實(shí)現(xiàn)以下步驟：

3）計(jì)算權(quán)值和參數(shù)

式中，λ＝α2（n＋k）－n；n為增廣狀態(tài)向量的維數(shù)；α為決定Sig ma點(diǎn)分布廣度的主要刻度因子，其典型分布范圍為10－3＜α≤1；β是零階Sig ma點(diǎn)權(quán)值的第二刻度因子，用來(lái)強(qiáng)調(diào)驗(yàn)后協(xié)方差計(jì)算；k為第三刻度因子，通常設(shè)置為0。

2 基于TSOA／TDOA模型的UKF算法

本節(jié)在上一節(jié)對(duì)TSOA／TDOA定位和UKF算法原理的分析的基礎(chǔ)上，利用獲得的TSOA與TDOA信息，應(yīng)用UKF算法對(duì)目標(biāo)移動(dòng)臺(tái)實(shí)施跟蹤。對(duì)于上述非線性定位跟蹤問(wèn)題，應(yīng)該首先建立其狀態(tài)方程和觀測(cè)方程。

2.1 基于TSOA／TDOA的UKF觀測(cè)方程

假設(shè)定位臺(tái)獲取TSOA和TDOA測(cè)量值分別為τk和 Δtk。它們分別由式（18）、式（19）得到：

式中，τk為k時(shí)刻系統(tǒng)TSOA測(cè)量時(shí)間，Δtki為k時(shí)刻目標(biāo)用戶到主基站和到相鄰基站的時(shí)間之差，M為參與計(jì)算的相鄰小區(qū)的數(shù)目，（xk，yk）為k時(shí)刻目標(biāo)移動(dòng)臺(tái)坐標(biāo)，（xb，yb）為主基站（目標(biāo)用戶所在小區(qū)基站）的坐標(biāo)，（xi，yi）為相鄰基站的坐標(biāo)，（xr，yr）為偵收臺(tái)的坐標(biāo)，c為光速，ntsoak∈ N（0，TDOA測(cè)量噪聲，并且相互獨(dú)立。將k時(shí)刻的測(cè)量值表示為zk＝ ［τk，tk1，tk2，…，tkM］T，測(cè)量等式可以表示為動(dòng)態(tài)模型形式，即UKF觀測(cè)方程為：

式（20）中，Vk∈ ［是系統(tǒng)的觀測(cè)噪聲，它的協(xié)方差矩陣記為R＝diag（I）。

2.2 基于TSOA／TDOA的UKF狀態(tài)模型

當(dāng)目標(biāo)在短時(shí)間內(nèi)，加速度不是很大，或者測(cè)量時(shí)間較短，我們可以認(rèn)為目標(biāo)在這段時(shí)間內(nèi)是近似勻速運(yùn)動(dòng)，但在橫向和縱向上均受到隨機(jī)加速影響［12］。在移動(dòng)臺(tái)定位應(yīng)用中，目標(biāo)用戶處于市區(qū)或者郊區(qū)，運(yùn)動(dòng)速度不是很高，認(rèn)為目標(biāo)移動(dòng)臺(tái)的運(yùn)動(dòng)狀態(tài)為受到隨機(jī)加速影響的勻速運(yùn)動(dòng)是合理的。

根據(jù)以上分析建立受到隨機(jī)加速影響的UKF勻速運(yùn)動(dòng)狀態(tài)模型。假設(shè)移動(dòng)臺(tái)在二維平面上運(yùn)動(dòng)，k 時(shí)刻的運(yùn)動(dòng)狀態(tài)為 Xk＝ ［xk，yk，vxk，vyk］T，（xk，yk）為k時(shí)刻移動(dòng)臺(tái)的坐標(biāo)位置，（vxk，vyk）分別為k時(shí)刻移動(dòng)臺(tái)在x軸和y軸方向的速度分量，由此，系統(tǒng)的狀態(tài)模型可以表示為：

式（21）中，k時(shí)刻即t＝t0＋k Ts時(shí)刻，Ts為抽樣間隔。Wk＝ ［］T為k 時(shí)刻系統(tǒng)的過(guò)程噪聲，表示該時(shí)刻的加速度向量，它的協(xié)方差矩陣記為Q 。將式（4）寫成矩陣形式，即系統(tǒng)狀態(tài)方程：

2.3 UKF算法跟蹤實(shí)現(xiàn)

建立狀態(tài)模型、觀測(cè)方程后，利用UKF算法完成對(duì)目標(biāo)移動(dòng)臺(tái)的定位跟蹤，其具體實(shí)現(xiàn)步驟如下：

2）執(zhí)行式（14）—式（17），完成算法的參數(shù)設(shè)置。

3）反復(fù)執(zhí)行式（3）—式（13），完成濾波更新過(guò)程，其中式（4）、式（7）中的函數(shù)分別由式（22）、式（20）確定，直至算法結(jié)束。

3 仿真結(jié)果與討論

為了驗(yàn)證UKF算法在TSOA／TDOA移動(dòng)臺(tái)單站跟蹤系統(tǒng)中的應(yīng)用效果，在典型蜂窩式基站分布的環(huán)境下進(jìn)行仿真，在直角坐標(biāo)系下分別使用EKF和UKF算法對(duì)目標(biāo)移動(dòng)臺(tái)進(jìn)行跟蹤。仿真條件設(shè)置為：目標(biāo)初始位置（200，100），相鄰基站的坐標(biāo)為：BS1（2 250，1 299），BS2（2 250，－1 299），BS3（0，－2 598），主基站的坐標(biāo)為：BSb（0，0），偵察臺(tái)的坐標(biāo)為：BSr（450，－200），運(yùn)動(dòng)狀態(tài)為帶有隨機(jī)加速的勻速運(yùn)動(dòng)，橫向和縱向速度分別為7 m／s、2 m／s，它們均受到零均值方差為3／2的高斯隨機(jī)加速的影響。抽樣間隔為2 s，取100個(gè)采樣點(diǎn)，σtsoa＝100 m，σtdoa＝50 m，α＝0.1，β＝2，k＝0。在上述仿真條件下，經(jīng)過(guò)100次蒙特卡羅仿真，得到EKF和UKF算法的目標(biāo)跟蹤軌跡曲線及誤差曲線，如圖2、圖3所示。

圖2 一次EKF、UKF算法跟蹤對(duì)比圖Fig.2 Cartogram of EKF and UKF tracking

圖3 RMSE比較圖Fig.3 Cartogram of RMSE

由圖2可看出，在軌跡較平緩的區(qū)域，兩種算法都能實(shí)現(xiàn)跟蹤，但UKF算法跟蹤效果明顯優(yōu)于EKF算法；在目標(biāo)出現(xiàn)機(jī)動(dòng)轉(zhuǎn)彎時(shí)，EKF算法出現(xiàn)跟蹤軌跡與目標(biāo)軌跡發(fā)生較嚴(yán)重偏離的現(xiàn)象，甚至在某些機(jī)動(dòng)轉(zhuǎn)彎較強(qiáng)區(qū)域出現(xiàn)跟蹤丟失，但UKF算法能一直保持較好的跟蹤效果。

圖3為進(jìn)行100次蒙特卡羅仿真統(tǒng)計(jì)得到的RMSE，其中

為蒙特卡羅仿真次數(shù)，（x，y）為目標(biāo)實(shí)際坐標(biāo)，（x′，y′）為算法估計(jì)坐標(biāo)。從圖中易對(duì)EKF，UKF算法的性能有更直觀地了解。當(dāng)?shù)竭_(dá)目標(biāo)機(jī)動(dòng)轉(zhuǎn)彎段時(shí)，EKF算法的跟蹤誤差迅速增大，而UKF算法的跟蹤誤差基本保持平穩(wěn)略有增大，這與圖2所示的情況也是相一致的。出現(xiàn)這種情況是由于EKF算法在對(duì)非線性測(cè)量方程的線性化時(shí)產(chǎn)生了誤差，而這種誤差對(duì)于上一時(shí)刻狀態(tài)初值較為敏感，導(dǎo)致該算法對(duì)于前后狀態(tài)變化較大的情況效果下降甚至濾波發(fā)散。而UKF算法直接利用系統(tǒng)非線性方程，無(wú)需線性化，利用一系列的確定性采樣點(diǎn)描述濾波過(guò)程的真實(shí)均值和方差，在一定程度上避免EKF線性化方程過(guò)程中帶來(lái)的誤差，其穩(wěn)定性和精確性更優(yōu)［13］。

圖4、圖5為兩種算法對(duì)目標(biāo)速度的跟蹤性能。

圖4 X方向速度跟蹤誤差比較圖Fig.4 Cartogram of tracking error in X direction

圖5 Y方向速度跟蹤誤差比較圖Fig.5 Cartogram of tracking error in Y direction

由圖可知：在X和Y方向上EKF算法速度跟蹤偏差在 （－15，15）范圍內(nèi)波動(dòng)且波動(dòng)的幅度較大，而UKF算法速度跟蹤偏差的波動(dòng)范圍和幅度都要明顯小于EKF算法，其對(duì)目標(biāo)速度的跟蹤性能更佳。

為進(jìn)一步驗(yàn)證算法在不同速度下的跟蹤性能，在上述仿真的條件下，分別以3 k m／h、30 k m／h、60 k m／h、120 k m／h的速度沿x軸方向作勻速直線運(yùn)動(dòng)，統(tǒng)計(jì)得到不同速度下位移和速度的估計(jì)誤差如表1、表2所示。

表1 不同速度下的位移均方根誤差Tab.1 The locating error with different speed

表2 不同速度下的速度估計(jì)誤差Tab.2 The speed error with different speed

從表1、表2易看出，在對(duì)目標(biāo)位移和速度的跟蹤估計(jì)中，UKF算法較EKF算法更加精確。在速度小于等于60 k m／h時(shí)，兩種算法的估計(jì)誤差都隨速度的增加而增大，但增加的幅度較小，相對(duì)穩(wěn)定；在目標(biāo)速度達(dá)到120 k m／h時(shí)，誤差快速增大，但UKF算法較EKF算法的誤差增加幅度更小，說(shuō)明其穩(wěn)定性更佳。

4 結(jié)論

本文提出一種基于TSOA／TDOA混合被動(dòng)單站定位模型的無(wú)跡卡爾曼濾波跟蹤算法。該算法引入TSOA／TDOA 觀 測(cè) 模 式，以 觀 測(cè) 的 TSOA／TDOA的有噪信息為基礎(chǔ)，使用受隨機(jī)加速影響的勻速運(yùn)動(dòng)狀態(tài)作為跟蹤算法的狀態(tài)模型，將UKF算法應(yīng)用在移動(dòng)臺(tái)的定位跟蹤上，實(shí)現(xiàn)了對(duì)移動(dòng)臺(tái)的位移和速度的同步跟蹤。仿真實(shí)驗(yàn)結(jié)果表明：使用UKF算法對(duì)目標(biāo)位置和速度的跟蹤效果優(yōu)于EKF算法，基于TSOA／TDOA混合被動(dòng)定位模型的無(wú)跡卡爾曼濾波跟蹤具有良好的準(zhǔn)確性和魯棒性。

［1］范志平，鄧平，劉林.蜂窩網(wǎng)無(wú)線定位［M］.北京：電子工業(yè)出版社，2002：52-100.

［2］3GPP Office.3GPP TS 25.215 v6.0.0.Physical layer-Measurements（FDD）［S］.France：3GPP，2003.

［3］鄧平，朱中梁.一種天線陣列定位法及其仿真研究［J］.電子與信息學(xué)報(bào)，2005，27（6）：841-844.DENG Ping，ZHU Zhongliang.An antenna array location method and its si mulation study［J］.Jour nal of Electronics＆Infor mation Technology，2005，27（6）：841-844.

［4］Aidala V，Hammel S E.Utilization of modified polar coor dinates f or bearings-only tracking ［J］.IEEE Transactions on Auto matic Control，1983，28（3）：283 -294.

［5］Si mon Julier，Jeffrey Uhl mann，Hugh F Durrant-Whyte.A new method for the nonlinear transfor mation of means and covariances in filters and esti mators［C］／／IEEE Transactions on Auto matic Control，USA：IEEE Press，2000：477-482.

［6］Banani S A，MasnadiShirazi M A.A new version of unscented Kal man filter［C］／／Proceedings of the World A-cademy of Science，Engineering and Technology.Barcelona，Spain，2007：192 -197.

［7］趙鶴，王喆垚.基于UKF的 MEMS傳感器姿態(tài)測(cè)量系統(tǒng)［J］.傳感技術(shù)學(xué)報(bào)，2011，28（5）：642-646.ZHAO He，WANG Zheyao.MEMS sensors based attitude measurement system using UKF［J］.Chinese Jour nal of Sensors and Actuators，2011，28（5）：642-646.

［8］Xiong K，Chan C，Zhang H S.Detection of satellite attitude sensor faults using the UKF［J］.IEEE Transactions on Aerospace and Electronic Systems，2007，43（2）：480-491.

［9］Ar ulampalam S，Maskell S，Gordon N，et al.A tutorial onparticle filters for online non-linear／non-Gaussian Bayesiantracking［J］.IEEE Transactions on Signal Processing，2002，50 （2）：174 -188

［10］胡浩，李婧，陳樂(lè)然，WCDMA系統(tǒng)中基于UE測(cè)量報(bào)告的無(wú)線定位及參數(shù)提取方法［J］.信息工程大學(xué)學(xué)報(bào)，2008，9（2）：200-202.HU Hao，LI Jing，CHEN Leran.Radio positioning and parameters getting method based on measurement report of UE in WCDMA system［J］.Journal of Inf or mation Engineering University，2008，9（2）：200-202.

［11］3 GPP Office.3 GPP TS 25.211 v6.0.0.Physical channels and mapping of transport channels onto physical channels（FDD）［S］.France：3GPP，2003.

［12］Eli Brooker.Tracking and Kal man filtering made easy［M］.New Yor k：Wiley，1998.

［13］王向磊，丁碩 ，蘇牡丹.EKF／UKF在基于地磁場(chǎng)的衛(wèi)星自主定軌中的應(yīng)用比較［J］.測(cè)繪科學(xué)技術(shù)學(xué)報(bào)，2011，28（1）：50-53.WANG Xianglei，DING Shuo，SU Mudan.Co mpare on the application of EKF／UKF in satellite autono mous or bit deter mination using geo magnetic field［J］.Jour nal of Geomatics Science and Technology，2011，28（1）：50-53.