趙躍新, 齊望東, 劉 鵬, 袁 恩, 徐 兵

(1.陸軍工程大學(xué)指揮控制工程學(xué)院, 江蘇 南京 210007; 2.東南大學(xué)信息科學(xué)與工程學(xué)院,江蘇 南京 210096; 3.網(wǎng)絡(luò)通信與安全紫金山實驗室, 江蘇 南京 211111)

0 引 言

目標跟蹤在無線通信、物聯(lián)網(wǎng)、雷達聲吶等領(lǐng)域得到了廣泛的研究與應(yīng)用,主要包括導(dǎo)航定位[1-2]、偵察監(jiān)控[3-4]、精確制導(dǎo)[5-6]等方面,旨在利用觀測站采集的到達角(angle of arrival, AoA)、傳播時間、到達時間差等測量值來估計運動目標的位置和速度。隨著陣列天線及其信號處理的技術(shù)發(fā)展與大量應(yīng)用,比如5G的大規(guī)模天線陣列和波束成形,AoA測量已經(jīng)在許多低成本硬件平臺上得以實現(xiàn)[7-9],使得基于AoA的目標跟蹤具有更廣的適用范圍。

AoA觀測方程和目標狀態(tài)存在非線性關(guān)系,使得AoA目標跟蹤具有很大的挑戰(zhàn)性。由于經(jīng)典卡爾曼濾波無法適用于非線性狀態(tài)空間模型,所以需要采用非線性濾波算法處理AoA觀測方程。擴展卡爾曼濾波(extended Kalman filter, EKF)通過一階泰勒級數(shù)近似得到線性化的觀測方程,但是截斷誤差使得EKF在初始狀態(tài)誤差和觀測噪聲較大時容易發(fā)散[10]。容積卡爾曼濾波(cubature Kalman filter, CKF)、無跡卡爾曼濾波(unscented Kalman filter, UKF)等Sigma點卡爾曼濾波算法[11-12]并沒有近似非線性函數(shù),而是通過選擇一組Sigma點近似目標狀態(tài)的高斯分布統(tǒng)計量,可獲得二階泰勒近似精度,其穩(wěn)定性更好,但是計算開銷大。粒子濾波[13]從后驗概率中抽取隨機狀態(tài)粒子并以此表達其分布,可處理非線性模型和非高斯噪聲分布,然而計算復(fù)雜度很高,難以適用于實時目標跟蹤。

與上述非線性濾波算法不同,偽線性卡爾曼濾波[14-15](pseudo linear Kalman filter, PLKF)利用等價運算將AoA觀測方程轉(zhuǎn)換為偽線性形式,不需要近似處理非線性函數(shù),可以直接應(yīng)用線性卡爾曼濾波到偽線性觀測方程。PLKF不僅結(jié)構(gòu)簡單、計算高效,可滿足實時跟蹤目標的要求,而且對初始誤差十分魯棒,不容易受到初始狀態(tài)的影響[16]。然而,偽線性化會使得PLKF產(chǎn)生估計偏差,該偏差不能通過提高觀測總數(shù)來降低,在觀測噪聲嚴重的情況下更為明顯,導(dǎo)致跟蹤性能急劇退化[17]。

圍繞PLKF的偏差問題,改進算法主要分為兩類:一類是偏差補償PLKF[18-19](bias-compensated PLKF, BC-PLKF),另一類是基于工具變量的卡爾曼濾波[17,20](instrumental variable Kalman filter, IVKF)。其中,BC-PLKF通過計算PLKF的估計偏差并進行補償,以提高跟蹤精度;IVKF利用工具變量矩陣減小偽線性噪聲和觀測系數(shù)矩陣的相關(guān)性,可以有效降低偏差。隨著觀測噪聲增加,偏差補償?shù)臏蚀_性降低,而工具變量矩陣和觀測系數(shù)矩陣的相關(guān)性減弱,這兩類方法都使性能顯著降低。對此,采用選擇性角度測量IVKF(selective-angle-measurement IVKF，SAM-IVKF)策略[21],當符合SAM條件時使用BC-PLKF的狀態(tài)估計值構(gòu)造工具變量矩陣再執(zhí)行IVKF,否則只需運行BC-PLKF。但是SAM-IVKF本質(zhì)上還是基于偏差補償和工具變量思想,并且SAM閾值并不是自適應(yīng)確定,所以在噪聲嚴重時跟蹤性能仍然會出現(xiàn)明顯的下降。

針對上述問題,本文將擴展觀測矩陣思想[22-23]融入卡爾曼濾波框架,提出了用于三維AoA目標跟蹤的二次約束卡爾曼濾波(quadratic constraint Kalman filter, QCKF)算法。該算法通過構(gòu)造目標函數(shù)并附加二次約束以實現(xiàn)狀態(tài)更新,再利用矩陣擾動方法完成協(xié)方差矩陣更新。QCKF的核心思想是引入涉及所有觀測噪聲項的增廣矩陣,接著對等價于線性卡爾曼濾波的目標函數(shù)添加二次約束,使其在待估計狀態(tài)為真實值時達到最小值,以此降低由偽線性噪聲和觀測系數(shù)矩陣之間相關(guān)性所引起的估計偏差。仿真分析對比了PLKF、EKF、BC-PLKF、SAM-IVKF、UKF以及QCKF的跟蹤性能,充分表明了本文算法的優(yōu)越性,其均方根誤差(root mean squared error, RMSE)在低噪聲情況下可達到后驗克拉美羅下界(Cramer-Rao lower bound, CRLB),當噪聲嚴重時具有更高的跟蹤精度,并且計算復(fù)雜度不高。

1 三維AoA目標跟蹤

圖1 三維AoA目標跟蹤示意圖

xk=Fk-1xk-1+wk-1

(1)

假設(shè)目標進行勻速運動,則Fk和Qk的表達式如下:

(2)

式中:Ts為相鄰時刻的采樣間隔;Qρ=diag(ρx,ρy,ρz),ρx、ρy和ρz分別為過程噪聲在x、y和z坐標軸上的功率譜密度。目標可以通過調(diào)整Fk將勻速運動模型替換為勻加速運動、勻速率轉(zhuǎn)彎運動等其他動態(tài)模型,同時Qk隨之進行相應(yīng)變化。

在k時刻,目標相對于第i個觀測站的AoA包括方位角θi,k和俯仰角φi,k,并且θi,k∈(-π,π)以及φi,k∈(-π/2,π/2),其觀測方程為

(3)

(4a)

(4b)

(5)

2 PLKF分析

針對AoA目標跟蹤的非線性狀態(tài)估計問題,PLKF是一種簡單有效的非線性濾波算法,利用等價運算將非線性觀測方程轉(zhuǎn)換為偽線性形式,不采取近似的線性化處理。然而,偽線性化會導(dǎo)致PLKF出現(xiàn)有偏估計,特別是在噪聲嚴重時性能快速下降。因此,本節(jié)在推導(dǎo)三維PLKF的基礎(chǔ)上,分析其偏差構(gòu)成及影響,給出引起估計偏差的主要原因。

2.1 PLKF

方位角θi,k的觀測方程(4a)通過等價代數(shù)運算可轉(zhuǎn)換為

(6)

式中:uθ i,k=[sinθi,k,-cosθi,k,0]T;L=[I3×3,03×3]。在實際情況下,受觀測噪聲影響的式(6)變?yōu)?/p>

(7)

同理,俯仰角φi,k的觀測方程(4b)等價為

(8)

(9)

通過組合每個觀測站的式(7)和式(9),從而得到偽線性形式的觀測方程為

(10)

偽線性方程式(10)與非線性觀測方程式(5)相比已轉(zhuǎn)換為線性形式,然后將線性卡爾曼濾波應(yīng)用于式(1)和式(10)可得PLKF,具體步驟如下。

步驟 1時間更新:

(11a)

(11b)

步驟 2測量更新:

(11c)

(11d)

Pk|k=(I-KkHk)Pk|k-1

(11e)

2.2 偏差分析

(12)

式(12)減去真實狀態(tài)xk可得到瞬時估計偏差為

(13)

γk=E{σk}=E{σk1}+E{σk2}+E{σk3}

(14)

式中：

估計偏差γk由包括3部分:① 前一個時刻預(yù)測值所引入的偏差E{σk1};②Pk|k和wk-1相關(guān)性造成的偏差E{σk2};③ 觀測系數(shù)矩陣Hk和偽線性噪聲向量εk相關(guān)性引起的偏差E{σk3}。其中,E{σk1}在其余兩項偏差等于0時可忽略;Pk|k和系統(tǒng)狀態(tài)有關(guān),所以與wk-1相關(guān),但對于速度接近恒定的目標,過程噪聲較小,使得Pk|k和wk-1相關(guān)性很弱,可得E{σk2}≈0。然而,Hk和εk均受到觀測噪聲影響,其相關(guān)性不可忽略,從而E{σk3}≠0。因此,觀測系數(shù)矩陣和偽線性噪聲向量的相關(guān)性是PLKF失去無偏性的主要原因,其偏差不會隨著觀測總數(shù)的增加而降低,并且在觀測噪聲嚴重時更顯著,造成估計性能明顯退化。

3 QCKF

為減小PLKF的狀態(tài)估計偏差,需要降低觀測噪聲相關(guān)性的影響。本文提出的QCKF算法仍使用與觀測噪聲無關(guān)的狀態(tài)預(yù)測方程(11a)及其協(xié)方差矩陣方程(11b),然后附加二次約束實現(xiàn)狀態(tài)更新,再利用矩陣擾動方法完成協(xié)方差矩陣更新。

3.1 狀態(tài)更新

結(jié)合線性卡爾曼濾波的狀態(tài)估計協(xié)方差最小準則,可構(gòu)造等價的目標函數(shù)

(15)

(16)

(17)

(18)

(19)

其中，

ηi,k=[0,nθ i,k]T

Λ1,k和Λ2,k的第2i-1行和第2i行(i=1,2,…,N)分別為

Λ2,k(2i-1,:)=0T

其中，

將式(17)和式(18)代入式(16)可得

(20)

為分析噪聲擾動矩陣ΔAk和ΔBk對狀態(tài)估計偏差的影響,對式(20)取期望可得

(21)

由于ΔAk和ΔBk的均值為零矩陣,所以式(20)的右側(cè)第3項在式(21)中也等于0。根據(jù)式(17)中ΔAk定義,可得

(22)

(23)

(24)

式中的約束值μk可取任意正數(shù),約束矩陣Ωk的表達式為

(25)

式中：

由于具有等式的二次約束優(yōu)化問題式(24)可通過拉格朗日乘子法進行求解,所以需要最小化的輔助目標函數(shù)為

(26)

式中：λ為拉格朗日乘子。通過式(26)對mk求偏導(dǎo),并令其等于0,可得

(27)

(28)

(29)

3.2 協(xié)方差矩陣更新

由于狀態(tài)更新采用廣義特征值分解進行優(yōu)化問題的求解,所以無法直接利用解析表達式推導(dǎo)協(xié)方差矩陣。對此,本節(jié)結(jié)合約束條件,利用矩陣擾動方法完成協(xié)方差矩陣更新。

(30)

(31)

式中：Φ1,k和Φ2,k的第2i-1行和第2i行(i=1,2,…,N)分別為

Φ2,k(2i-1,:)=0T

(32)

將式(32)代入式(24),然后約束優(yōu)化問題等價于

(33)

(34)

(35)

(36)

(37)

(38)

(39)

(40)

(41)

(42)

結(jié)合式(41)和式(42),可得

(43)

Tk((Φ2,kTkDk)⊙Θk)12N

(44)

式中：

Θk=blkdiag{Θ1,k,Θ2,k,…,ΘN,k}

(45)

(46)

結(jié)合觀測方程偽線性化、二次約束狀態(tài)更新及其協(xié)方差矩陣更新,QCKF算法的流程總結(jié)如下。

算法1 QCKF輸入: x^k-1|k-1,Pk-1|k-1,Fk-1,Qk-1,R-k,y^k,S=[s1,s2,…,sN]時間更新: x^k|k-1=Fk-1x^k-1|k-1 Pk|k-1=Fk-1Pk-1|k-1FTk-1+Qk-1觀測方程偽線性化: [z^k,Hk]=pesudo-linear(y^k,S) ∥式(7)和式(9)Dk=f(x^k|k-1,S) ∥Dk定義于式(10)之下Rk=DkR-kDTk測量更新: 構(gòu)造增廣矩陣:Ak=[-I,x^k|k-1],Bk=[-Hk,z^k] 計算約束矩陣:Ωk ∥式(25) ^mk=min{eig(ATkP-1k|k-1Ak+BTkR-1kBk,Ωk)} x^k|k=^mk(1∶6)^mk(7) 更新觀測矩陣Hk|k∥x^k|k反推觀測值代入Hk Pk|k=(P-1k|k-1+HTk|kR-1kHk|k)-1輸出:x^k|k,Pk|k

4 性能指標

4.1 偏差和RMSE

(47a)

(47b)

式中:M為蒙特卡羅仿真次數(shù)。進一步可得目標跟蹤的時間平均偏差和RMSE:

(48a)

(48b)

式中：U=Nt-Lt+1,Nt為整個跟蹤階段的觀測次數(shù)；Lt表示時刻偏移量,是為了降低跟蹤初始階段的誤差對客觀比較性能影響。

4.2 CRLB

參數(shù)估計的RMSE下界通過CRLB表示,該指標可作為評估性能的參考值。由于運動目標的狀態(tài)轉(zhuǎn)移還受到過程噪聲的影響,所以使用后驗CRLB(posterior CRLB, PCRLB)[26]作為狀態(tài)估計性能的參考值,從而可以給出不等式：

(49)

式中：Jk為Fisher信息矩陣,可通過遞推公式計算得到

(50)

(51)

式中:

所以,目標跟蹤的時間平均PCRLB為

(52)

5 仿真分析

為評估本文所提QCKF算法的跟蹤性能,通過蒙特卡羅仿真對比該算法與PLKF、EKF、BC-PLKF、SAM-IVKF以及UKF的三維AoA目標跟蹤精度,以狀態(tài)估計的偏差和RMSE作為性能指標,并用PCRLB的平方根(PCRLB1/2)作為RMSE的參考下界。仿真不僅分析各算法在不同觀測噪聲水平下的性能,而且對比跟蹤誤差在指定噪聲水平下隨時刻的變化情況,同時給出各算法的相對運行時間。

5.1 仿真參數(shù)

圖2 目標跟蹤仿真場景

為降低初始跟蹤階段的誤差影響,時間平均偏差和RMSE的偏移參數(shù)Lt=20。SAM-IVKF在時刻k≤30時執(zhí)行BC-PLKF算法,并且SAM策略的閾值為4σ。UKF算法需要計算尺度因子,相關(guān)參數(shù)取值為α=0.5、β=2以及κ=0。

本文QCKF以及PLKF、BC-PLKF和SAM-IVKF算法均建立在觀測噪聲較小的條件下,但觀測噪聲標準差σ的取值區(qū)間并沒有參考指標,這主要源自兩方面原因:一方面噪聲近似誤差與狀態(tài)估計誤差的關(guān)系難以構(gòu)建,另一方面噪聲近似誤差對狀態(tài)估計誤差的影響能否忽略是與具體的應(yīng)用需求相關(guān)。對此,結(jié)合基站進行三維跟蹤無人機的仿真場景,以sinnθ,k≈nθ,k和sinnφ,k≈nφ,k的1σ近似誤差小于1%作為觀測噪聲較小的適用條件,所以其標準差σ取值為{1°,2°,…,12°}。此外,所有算法的蒙特卡羅仿真次數(shù)M=10 000。

5.2 仿真結(jié)果

圖3和圖4分別為狀態(tài)估計的位置誤差和速度誤差隨著觀測噪聲標準差的變化情況。

圖3 位置誤差隨觀測噪聲變化

圖4 速度誤差隨觀測噪聲變化

從圖3(a)可看出,PLKF的位置偏差最大,并且隨著噪聲增大而快速增加;EKF在低噪聲條件情況下位置偏差較小,但是當σ≥7°時其偏差也開始較快增加;BC-PLKF的偏差補償方法能有效降低PLKF的偏差影響,但位置偏差也依然逐步變大,其原因在于偏差補償?shù)臏蚀_性隨著噪聲增大而降低;SAM-IVKF、UKF和QCKF的位置偏差性能比較接近,均維持在較低水平,尤其當噪聲大時QCKF的位置偏差比其他算法更低。

由圖3(b)可知,PLKF的位置RMSE仍然最大,這主要由位置偏差影響所造成;EKF在σ≤4°時可以達到后驗克拉美羅界,但隨著噪聲增加,其跟蹤誤差迅速增大,因為一階近似引起的初值敏感和發(fā)散問題顯現(xiàn);BC-PLKF相較于PLKF的位置RMSE顯著降低,但跟蹤性能差于SAM-IVKF、UKF和QCKF;當σ≥9°時,SAM-IVKF的位置RMSE明顯增加,此時SAM策略逐步以偏差補償濾波為主導(dǎo),所以其誤差與BC-PLKF不斷接近;QCKF在噪聲大(σ≥7°)的情況下,位置RMSE始終低于其他算法,尤其當σ=12°時具有更明顯的優(yōu)勢,表明其目標跟蹤的抗噪聲能力更強。

圖4展示的速度偏差和RMSE情況與圖3的分析類似,但是各算法的速度估計相較于位置估計而言性能差異更小。QCKF的速度估計性能遠優(yōu)于PLKF和EKF,略優(yōu)于BC-PLKF、SAM-IVKF以及UKF。因此,圖3和圖4表明即使在速度估計誤差接近的情況下,QCKF可以實現(xiàn)更準確的目標位置跟蹤,特別是在噪聲很大時優(yōu)勢更加突出。表1給出了σ=12°時的95%置信區(qū)間寬度,其中EKF的寬度最大,其他算法的差異較小。由于置信區(qū)間寬度遠低于位置和速度誤差,所以圖3和圖4的結(jié)果及分析是可靠有效的。

表1 σ=12°時95%置信區(qū)間寬度

圖5～圖7給出了不同觀測噪聲標準差時跟蹤誤差隨時刻k的變化情況。由于目標跟蹤旨在獲得目標位置,并且位置和速度的跟蹤誤差變化情況類似,所以圖5～圖7僅展示位置偏差和RMSE的變化趨勢。為便于直觀區(qū)分不同算法的數(shù)據(jù)曲線,圖中每隔5個時刻進行符號標記,但實際數(shù)據(jù)時刻間隔仍為1。從圖5(σ=3°)可看出,PLKF受到不斷增大的偏差影響,位置誤差隨時刻快速增加,而其他算法偏差較低,并且可以一直逼近PCRLB。由圖6(σ=8°)可知,PLKF和EKF在初始階段誤差明顯增加,進而出現(xiàn)發(fā)散趨勢;BC-PLKF從時刻k=35開始逐步偏離真實軌跡,濾波跟蹤誤差不斷增加;SAM-IVKF、UKF和QCKF的偏差保持在較小范圍,RMSE維持在PCRLB附近,其中QCKF的偏離誤差更小。從圖7(σ=12°)可看出,PLKF和EKF很快出現(xiàn)發(fā)散,位置誤差迅速增加;SAM-IVKF和BC-PLKF的跟蹤性能較為接近,原因在于噪聲嚴重時SAM策略以偏差補償作為主要濾波方法;QCKF在位置偏差和RMSE性能指標上均比其他算法更優(yōu)。值得注意的是,運動目標的位置不斷遠離觀測站,使得AoA目標跟蹤的距離放大誤差問題顯現(xiàn),所以PCRLB在圖5～圖7中均呈現(xiàn)上升趨勢。

圖5 σ=3°時位置誤差隨時刻變化

圖6 σ=8°時位置誤差隨時刻變化

圖7 σ=12°時位置誤差隨時刻變化

各算法的相對運行時間如表2所示,其中以EKF運行時間作為基準進行歸一化。BC-PLKF的偏差補償過程使得計算開銷高于PLKF;SAM-IVKF結(jié)合偏差補償和工具變量方法,所以比BC-PLKF的運算時間更長;QCKF需要構(gòu)造約束矩陣以及特征值分解,其計算開銷比SAM-IVKF略高,但是相較于UKF可以大幅降低運行時間。因此,表2、圖3和圖4表明QCKF以相對較低的計算開銷實現(xiàn)了更優(yōu)的估計性能。

表2 不同算法相對運行時間比較

6 結(jié) 論

本文針對PLKF在三維AoA目標跟蹤中存在嚴重偏差的問題,提出一種有效降低估計偏差的QCKF算法。通過附加二次約束條件到所建立的目標函數(shù),使其在狀態(tài)為真值時取得最小值,以此克服由偽線性噪聲和觀測系數(shù)矩陣的相關(guān)性所引起的偏差影響。仿真對比分析了PLKF及其改進算法BC-PLKF、SAM-IVKF,非線性濾波算法EKF、UKF以及本文QCKF算法,結(jié)果充分證實了QCKF的濾波跟蹤性能更優(yōu),尤其是當噪聲嚴重時具有更顯著的優(yōu)勢,并且計算開銷相對較低。

本文在分析偏差時假設(shè)過程噪聲較小,這適用于勻速運動、勻加速運動、勻速率轉(zhuǎn)彎運動等平穩(wěn)變化的動態(tài)模型。當目標狀態(tài)劇烈變化時,系統(tǒng)過程噪聲較大,也會引起偏差,可通過多模型方法降低其影響。針對觀測噪聲服從高斯分布的假設(shè),可以結(jié)合高斯混合模型處理非高斯觀測噪聲。