基于迭代雙重?cái)U(kuò)展的Kalman濾波的有源目標(biāo)估計(jì)算法

陸 玉,張 華

(阜陽職業(yè)技術(shù)學(xué)院，安徽 阜陽 236031)

0 引 言

全球定位系統(tǒng)(Global Positioning System, GPS)已廣泛應(yīng)用有于有源目標(biāo)的估計(jì)。然而，GPS系統(tǒng)并不適合室內(nèi)目標(biāo)的定位[1-2]。而基于到達(dá)時(shí)間差(Time Different of Arrival, TDOA)信號(hào)的定位算法廣泛應(yīng)用于室內(nèi)定位[3]。基于TDOA信號(hào)的定位是利用雙曲線軌跡，估計(jì)目標(biāo)位置。即雙曲線的交點(diǎn)就是發(fā)射機(jī)位置。但是，基于TDOA信號(hào)的定位算法存在超定問題。若需準(zhǔn)確估計(jì)發(fā)射機(jī)位置，需測(cè)量更多的TDOA信號(hào)，增加了對(duì)發(fā)射機(jī)數(shù)量的要求。

此外，噪聲也是影響基于TDOA定位精度的重要原因。文獻(xiàn)[4]通過在二維和三維空間內(nèi)的TDOA測(cè)量，消除與距離無關(guān)的噪聲。它先建立與距離無關(guān)噪聲模型，再基于源定位提高TDOA信號(hào)測(cè)量。

而測(cè)量移動(dòng)發(fā)射機(jī)的位置和速度，可預(yù)測(cè)雷達(dá)和導(dǎo)航系統(tǒng)的未來軌跡。此外，測(cè)量到達(dá)頻率差(Frequency Different of Arrival, FDOA)可以降低對(duì)接收機(jī)數(shù)量的限制。

為此，本文考慮只有兩個(gè)接收器的場(chǎng)景，并通過FDOA信號(hào)測(cè)量移動(dòng)發(fā)射機(jī)速度。再通過結(jié)合TDOA和FDOA測(cè)量，進(jìn)行定位。同時(shí)，考慮RF系統(tǒng)噪聲對(duì)測(cè)量值的非線性影響。并通過迭代雙重?cái)U(kuò)展Kalman濾波(Dual-extended Kalman Filter, DEKF)算法對(duì)非線性誤差進(jìn)行補(bǔ)償。依據(jù)迭代DEKF算法，參數(shù)估計(jì)(Parameter Estimation, PE)濾波更新因外部噪聲造成的模型不確認(rèn)性，并通過迭代，加速系統(tǒng)參數(shù)的收斂性。仿真結(jié)果表明，提出的ID-EKF算法有效地降低估計(jì)誤差。

1 系統(tǒng)模型

由一個(gè)發(fā)射機(jī)和兩個(gè)接收機(jī)構(gòu)成定位系統(tǒng)。發(fā)射機(jī)以不同速度移動(dòng)，并發(fā)送RF信號(hào)。令二維坐標(biāo)r0=[x,y]T表示發(fā)射機(jī)位置。兩個(gè)接收機(jī)位置表示ri=[xi,yi]T，且i={1,2}。

‖ri(k)-r0(k)‖

(1)

其中‖·‖表示范數(shù)。

RF信號(hào)達(dá)到接收機(jī)的時(shí)間：

(2)

(3)

其中t21(k)表示兩個(gè)接收機(jī)接收RF信號(hào)的時(shí)間差。

通過兩個(gè)接收機(jī)所接收的信號(hào)參數(shù)，可推導(dǎo)TDOA信號(hào)Td(k)：

‖r1(k)-r0(k)‖+η(k))

(4)

其中η表示零均值的高斯白噪聲。

接下來，用FDOA信號(hào)估計(jì)發(fā)射機(jī)的速度。由于多普勒效應(yīng)，如果移動(dòng)的接收機(jī)從發(fā)射機(jī)所接收的信號(hào)頻率不同于發(fā)射頻率。因此，引入平均頻率fi(k)，其表示在時(shí)間段Ts內(nèi)的平均頻率，其定義如式(5)所示：

(5)

FDOA測(cè)量階段要求接收機(jī)同步。通過時(shí)間微分，便可獲取FDOA信號(hào)：

(6)

結(jié)合式(1)，通過時(shí)間微分，能夠建立發(fā)射機(jī)的未知位置r0方程：

(7)

2 基于迭代的DEKF的定位算法

2.1 狀態(tài)等式的建立

本節(jié)將發(fā)射機(jī)位置和速度轉(zhuǎn)化狀態(tài)變量，便以DEFK處理，如式(8)所示：

s(k+1)=As(k)+Bu(k)+w(k)=

f(s(k),u(k))+w(k)

(8)

用4維矢量表示接收機(jī)的位置和速度信息，如式(9)所示：

(9)

將TDOA和FDOA的信號(hào)測(cè)量值作為輸出等式。令r1、r2∈R2表示兩個(gè)接收機(jī)位置、令v1、v2∈R2表示兩個(gè)接收機(jī)的速度。相應(yīng)地，令r0、v0表示發(fā)射機(jī)的位置和速度。依據(jù)這些變量符號(hào)表示輸出等式：

z(k)=h(s(k))+n(k)=

(10)

z(k)∈R2的第一項(xiàng)表示TDOA的測(cè)量值；第二項(xiàng)代表FDOA的測(cè)量值。而n(k)表示z(k)的噪聲測(cè)量項(xiàng)。為了能將輸出等式應(yīng)用于迭代的DEFK算法，通過偏微分完成線性操作。先求式(10)的梯度：

(11)

其中vi=[Δxi(k)/Ts,Δyi(k)/Ts]T。

然而，在真實(shí)環(huán)境中，TDOA和FDOA的信號(hào)受到外部噪聲影響[6]。這些外部噪聲導(dǎo)致模型的不確定性，降低了迭代算法的性能。此外，外部噪聲也延緩了收斂速度。

外部噪聲主要由非視距(Nonline-of-sight, NLOS)噪聲和測(cè)量噪聲組成。本文，將TDOA和FDOA測(cè)量看成LOS數(shù)據(jù)[7]。而測(cè)量噪聲是由環(huán)境因素引起的，如傳感節(jié)點(diǎn)故障，環(huán)境溫度和濕度的變化也增加定位誤差。為此，引用迭代DEKF算法降低測(cè)量誤差。

2.2 DEKF算法

為了提高定位算法，引用迭代EKF算法。文獻(xiàn)[8]證實(shí)，迭代EKF算法的性能優(yōu)于EKF算法。DEKF算法可以組織成狀態(tài)濾波和PE濾波。每個(gè)濾波均能估計(jì)狀態(tài)等式的未知參數(shù)。具體而言，狀態(tài)濾波驅(qū)動(dòng)PE濾波去估計(jì)狀態(tài)等式的未知參數(shù)，而狀態(tài)濾波依據(jù)當(dāng)前的參數(shù)值估計(jì)狀態(tài)，其形式化表述如式(12)：

(12)

其中f(·)和h(·)分別表示式(8)、式(12)所示的狀態(tài)等式和輸出等式[9]。而r(k)表示m(k)的過程噪聲。為了將迭代DEKF算法應(yīng)用于定位問題，對(duì)每個(gè)濾波器的狀態(tài)值和誤差協(xié)方值進(jìn)行初始化：

(13)

(14)

為了提高定位精度，應(yīng)在每個(gè)抽樣時(shí)刻更新狀態(tài)等式。狀態(tài)濾波s(k)的更新過程如下：

(15)

(16)

接下來，對(duì)PE濾波器m(k)進(jìn)行更新，如式(17)所示：

(17)

(18)

在ID-EKF算法的狀態(tài)濾波中，將測(cè)量更新等式進(jìn)行線性迭代，如式(20)所示：

(19)

(20)

而PE濾波器的迭代測(cè)量更新方程，如式(21)所示：

(21)

(22)

圖1 ID-EKF算法框架

3 性能仿真

3.1 仿真環(huán)境

考慮如圖2所示仿真環(huán)境，兩個(gè)接收機(jī)初始位置分別為(5,7)、(5,11)，并可在以初始位置為中心，以1 km為半徑的圓內(nèi)移動(dòng)。假定外部噪聲和干擾信號(hào)服從高斯分布。

此外，測(cè)量的TDOA信號(hào)含有噪聲，且所測(cè)量噪聲協(xié)方差為0.0067。而速度狀態(tài)(?x,?y)的協(xié)方差分別為0.1715和0.2251。在仿真過程中，發(fā)射機(jī)沿著sine波行軌跡移動(dòng)，如圖2所示。

圖2 仿真模型

3.2 數(shù)據(jù)分析

3.2.1實(shí)驗(yàn)一

圖3 ID-EKF算法對(duì)發(fā)射機(jī)軌跡的估計(jì)

圖3顯示ID-EKF算法對(duì)發(fā)射機(jī)移動(dòng)軌跡的估計(jì)。從圖3可知，利用迭代的DEKF算法所估計(jì)的軌跡更接近于發(fā)射機(jī)移動(dòng)的真實(shí)軌跡。同時(shí)，圖3顯示了3次迭代-DEKF和5次迭代DEKF算法對(duì)發(fā)射機(jī)移動(dòng)軌跡的估計(jì)。

從圖3可知，ID-EKF算法通過誤差補(bǔ)償能夠提高對(duì)發(fā)射機(jī)移動(dòng)軌跡估計(jì)的精度。原因在于：PF濾波能夠估計(jì)狀態(tài)等式的未知參數(shù)估計(jì)。同時(shí)，迭代次數(shù)的增加，也提高了估計(jì)精度。

3.2.2實(shí)驗(yàn)二

本次實(shí)驗(yàn)進(jìn)行同類算法的對(duì)比分析。選擇EKF、遞歸最小均方誤差(Recursive Weighted Least Square, RWLS)[7]和Cramer-Rao 下限(Cramer-Rao Lower Bound, CRLB)作為參照。圖4顯示了噪聲參數(shù)κe對(duì)RRSE的影響。

圖4 RMSE隨噪聲參數(shù)κe的變化情況

噪聲參數(shù)κe越大，外部噪聲越嚴(yán)重。從圖4可知，κe的增加，使RRSE增加，降低了估計(jì)性能。相比于EKF、RWLS，提出的ID-EKF算法有效了降低了估計(jì)誤差，提高了發(fā)射機(jī)的估計(jì)精度。但與CRLB還有一定距離。

4 結(jié) 語

針對(duì)有源目標(biāo)的估計(jì)問題，提出基于迭代雙重?cái)U(kuò)展的Kalman的有源目標(biāo)估計(jì)算法ID-EKF。為了準(zhǔn)確估計(jì)發(fā)射機(jī)的速度，ID-EKF算法將FDOA信號(hào)作為測(cè)量等式。此外，為了降低計(jì)算負(fù)擔(dān)，只考慮兩個(gè)接收機(jī)，并利用迭代DEKF算法實(shí)現(xiàn)定位。為了能跟蹤目標(biāo)，建立關(guān)于位置和速度的狀態(tài)等式和關(guān)于TDOA和FDOA信號(hào)的測(cè)量等式。迭代DEKF算法由狀態(tài)濾波和PE濾波構(gòu)成，其中PE濾波提高收斂速度，并降低誤差。仿真數(shù)據(jù)表明，提出的ID-EKF算法降低估計(jì)誤差，提高了對(duì)有源目標(biāo)跟蹤精度。