三維AoA目標跟蹤的二次約束卡爾曼濾波算法

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

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

0 引 言

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

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

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

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

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

1 三維AoA目標跟蹤

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

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

(1)

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

(2)

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

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

(3)

(4a)

(4b)

(5)

2 PLKF分析

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

2.1 PLKF

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

(6)

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

(7)

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

(8)

(9)

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

(10)

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

步驟 1時間更新:

(11a)

(11b)

步驟 2測量更新:

(11c)

(11d)

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

(11e)

2.2 偏差分析

(12)

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

(13)

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

(14)

式中：

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

3 QCKF

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

3.1 狀態更新

結合線性卡爾曼濾波的狀態估計協方差最小準則,可構造等價的目標函數

(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對狀態估計偏差的影響,對式(20)取期望可得

(21)

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

(22)

(23)

(24)

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

(25)

式中：

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

(26)

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

(27)

(28)

(29)

3.2 協方差矩陣更新

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

(30)

(31)

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

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

(32)

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

(33)

(34)

(35)

(36)

(37)

(38)

(39)

(40)

(41)

(42)

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

(43)

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

(44)

式中：

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

(45)

(46)

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

算法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測量更新: 構造增廣矩陣: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為蒙特卡羅仿真次數。進一步可得目標跟蹤的時間平均偏差和RMSE:

(48a)

(48b)

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

4.2 CRLB

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

(49)

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

(50)

(51)

式中:

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

(52)

5 仿真分析

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

5.1 仿真參數

圖2 目標跟蹤仿真場景

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

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

5.2 仿真結果

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

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

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

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

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

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

表1 σ=12°時95%置信區間寬度

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

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

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

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

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

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

6 結 論

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

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