基于無偏偽線性卡爾曼濾波的3D 到達角目標跟蹤

趙 唯 ,黃子豪 ,郝程鵬

(1.中國科學院 聲學研究所,北京,100190;2.中國科學院大學 電子電氣與通信工程學院,北京,100049)

0 引言

水下到達角目標跟蹤是僅通過觀測站采集到的到達角信息來估計目標位置和速度等狀態的技術,由于其無需觀測站輻射信號,具有抗干擾性強、高隱蔽性的優點[1-2],是水下目標跟蹤重要的方法之一。隨著到達角測量的實現成本不斷降低,到達角目標跟蹤技術的適用范圍不斷擴大,現已廣泛應用至導航定位、偵察監控、水下搜救及資源開發等民用和軍用領域[3-6]。

到達角目標跟蹤是一類典型的非線性濾波問題,其最大的挑戰在于角度觀測值和目標狀態之間的非線性關系[7]。針對2D 空間中的到達角目標跟蹤問題,Aidala[8]提出了偽線性卡爾曼濾波(pseudo linear Kalman filter,PLKF)算法,PLKF 利用觀測站與目標之間的幾何關系將觀測方程轉換為偽線性形式,可直接應用線性卡爾曼濾波,具有結構簡單、對初始誤差不敏感的優點[9],但PLKF 對目標狀態估計存在較大的偏差。隨后,Ngugen 等[10]對PLKF 的偏差進行了分析,認為偏差主要來源于偽線性方程中觀測噪聲和觀測矩陣的相關性,并提出了基于偏差補償的偽線性卡爾曼濾波(bias compensated PLKF,BCKF)算法和基于工具變量的偽線性卡爾曼濾波(instrumental variable PLKF,IVKF)算法。其中BCKF 對PLKF 的估計結果進行了偏差補償,IVKF 則利用BCKF 的估計結果構造與觀測噪聲無關的工具變量矩陣,以此減少噪聲和觀測矩陣的相關性,進一步減小了估計偏差。但隨著觀測噪聲的增加,偏差補償項的準確性降低,使得BCKF 的估計偏差明顯增大,由于工具變量矩陣的構造依賴于BCKF 的估計結果,IVKF 算法也易受到BCKF 偏差增大的影響,工具變量矩陣構造的準確性降低,跟蹤性能也隨之下降。對此,Huang等[11]提出了噪聲分離的方法,通過代數運算直接構造一個不含噪聲的觀測矩陣,提出一種無偏偽線性卡爾曼濾波(unbiased PLKF,UBKF)算法,有效提高了2D 空間中目標跟蹤的精度。由于目標多在3D 空間運動,實際應用意義更高的3D 無源到達角目標跟蹤也得到了發展。Ngugen 等[12]提出了基于工具變量的3D 偽線性卡爾曼濾波(3Dinstrumental variable PLKF,3D-IVKF)算法,將2DIVKF 拓展到3D 空間。與2D-IVKF 類似,3D-IVKF的跟蹤精度會隨觀測噪聲增大迅速下降,且由于維度增加,算法的復雜度較高。

在實際應用中,到達角目標跟蹤面臨的另一個挑戰是觀測站定位誤差導致的估計性能下降,這是因為在目標勻速直線或勻加速運動假設下,為保證目標運動參數可觀測,需要觀測站進行機動運動,而機動觀測站的位置信息依賴于定位系統且往往存在定位誤差[13]。針對這一問題,文獻[14-16]利用位置已知的校準源來降低觀測站定位誤差對到達時間差(time difference-of-arrival,TDOA)目標定位的影響,雖然定位精度有所提高,但校準源的使用增加了定位系統的復雜度。Farina[17]通過對觀測方程線性化,得到與觀測站定位誤差和觀測噪聲相關的近似噪聲協方差矩陣,提出了一種最大似然估計器(maximum likelihood estimator,MLE)。Pang 等[18]利用觀測站定位誤差的統計特性建立了新的2D 到達角偏差補償算法。以上2 種算法均采用批處理方式,不僅計算復雜度高且需要目標的狀態模型為確定系統,難以應對存在過程噪聲或目標機動的跟蹤問題。

為解決上述問題,文中提出了一種3D 修正無偏偽線性卡爾曼濾波(3D-modified UBKF,3DMUBKF)算法。該算法考慮了觀測站定位誤差對方位角及俯仰角觀測方程偽線性化的影響,通過修正偽線性噪聲的協方差矩陣降低觀測站定位誤差的影響,接著對觀測方程進行整體噪聲分離,以此降低觀測矩陣與觀測噪聲之間的相關性引起的偏差。仿真分析表明,3D-MUBKF 算法相較于已有算法在機動和非機動2 種場景下表現更優,在觀測噪聲和觀測站定位誤差較大時能夠顯著降低跟蹤誤差,且計算復雜度低。

1 系統模型

1.1 目標狀態模型

考慮使用單運動觀測站對方位角和俯仰角進行觀測的3D 到達角目標跟蹤問題。在觀測時刻k處,目標位置速度vk=[vx,k,vy,k,vz,k]T,均為待估計未知參數,目標的狀態向量,狀態方程可表示為

式中:F為狀態轉換矩陣;wk為均值為0、協方差為Qk的獨立高斯過程噪聲,代表目標在運動過程中的干擾,會使目標發生輕微的未知機動。假設使用常速度模型來對目標運動進行建模,則F和Qk可表示為

式中: 0m×n表示m×n的零矩陣;In×n表示n×n的單位矩陣;T為采樣間隔;Q0=diag(qx,qy,qz),qx,qy及qz分別代表過程噪聲在x,y及z軸上的功率譜密度分量。

1.2 觀測模型

k時刻測量的含噪方位角和含噪俯仰角可表示為

k時刻不確定的觀測站的位置可表示為

則k時刻的觀測方程為

2 PLKF 分析

針對3D 到達角目標跟蹤問題,利用等價代數運算將觀測方程轉換為偽線性形式是一種簡單高效的濾波方式,但偽線性化會使目標的狀態估計出現偏差。文中推導存在觀測站定位誤差情況下的3D-PLKF,并分析觀測站定位誤差對3D-PLKF的主要影響以及估計偏差的主要來源。

2.1 存在觀測站定位誤差的3D-PLKF

首先對存在觀測站定位誤差的到達角觀測方程進行偽線性化,方位角的觀測方程通過等價代數運算可以轉換為

考慮實際情況中的方位角觀測噪聲和觀測站定位誤差后,式(8)可寫為

將式(6),式(11)代入式(9)得

根據幾何關系cosθk(px,k-rx,k)+sinθk(py,k-ry,k)=dkcos?k和 sinθk(px,k-rx,k)=cosθk(py,k-ry,k),并省略2 階以上的噪聲項,式(12)可以簡化為

式中:dk=∥pk-rk∥為目標到觀測站的距離;uθ,k=[-sinθk,cosθk,0]T。

采用同樣的方法對俯仰角觀測方程進行偽線性化,式(5)可以寫為

考慮俯仰角觀測噪聲以及觀測站定位誤差,式(14)可寫為

將式(6),式(11)及式(17)代入式(16),并根據式(8)、式(14)及幾何關系d=cos?kcosθk(px,k-rx,k)+cos?ksinθk(py,k-ry,k)+sin?k(pz,k-rz,k),可將偽線性噪聲表示為

式中,u?,k=[-sin?kcosθk,-sin?ksinθk,cos?k]T。

此時,k時刻觀測方程的偽線性形式可寫為

且在觀測噪聲較小的情況下,偽線性噪聲 ηk的協方差其中,Rθ,k≈式(19)已為線性形式,因此可直接應用線性卡爾曼濾波,得到存在觀測站定位誤差時的3D-PLKF,具體步驟為

2.2 偏差分析

參考文獻[10]中的推導方法,存在觀測站定位誤差時的3D-PLKF 的估計偏差可表示為

式中:E{Ak}代表前一采樣時刻偏差的傳遞,不是偏差產生的直接原因,若其余2 項偏差等于0,則E{Ak}可忽略;E{Bk}代表由Pk|k和wk-1的相關性引起的偏差,在過程噪聲較小時可忽略;由于觀測矩陣Hk和偽線性噪聲 ηk均與到達角觀測噪聲有關,Hk和 ηk的相關性不可忽略,E{Ck}≠0。因此,觀測矩陣Hk和偽線性噪聲 ηk之間的相關性是PLKF 算法存在偏差的主要原因。

3 3D-MUBKF 算法

由2.2 節的偏差分析可知,PLKF 算法的估計偏差主要來源于觀測矩陣與偽線性噪聲的相關性,接下來對偽線性方程(19)進行噪聲分離以消除觀測矩陣與偽線性噪聲的相關性。利用觀測噪聲較小時的近似關系,可將式(19)中的觀測矩陣改寫為

將式(29)和式(30)代入式(19)中,并忽略2 階以上的噪聲項,得到改寫后的偽線性方程為

令tθ,k=[-cosθk,-sinθk,0]Mxk+dkcos?k,t?,k=可得噪聲分離后的偽線性方程

根據式(25),PLKF 算法的偏差可寫為

可以看出,經過噪聲分離后,式(34)等于0。以式(1)和式(33)為狀態空間,得3D-MUBKF,其具體步驟為

由于真實到達角 θk、?k以及觀測站與目標之間的真實距離未知,無法得到tθ,k、t?,k、uθ,k以及u?,k的真實值,可利用狀態預測值計算,對待求真實值進行近似估計,即

根據以上分析可知,3D-MUBKF 通過修正卡爾曼濾波過程中的噪聲協方差矩陣來降低觀測站定位誤差對算法的影響,當cθ,k=c?,k=0時,3DMUBKF 將退化為未修正觀測站定位誤差的3DUBKF 算法。

4 仿真分析

4.1 評價標準

文中通過蒙特卡洛仿真實驗驗證3D-MUBKF算法對勻速目標及機動目標的跟蹤性能,并與3DUBKF、3D-BCKF、3D-IVKF 以及文獻[17]中考慮了觀測站定位誤差的MLE 進行比較。MLE 的數值解通過高斯牛頓(Gauss-Newton,GN)算法[20]迭代計算,初始迭代值設置為偽線性估計器(pseudo linear estimator,PLE)[20]的估計值,GN 算法迭代10 次后得到MLE 的數值解。仿真實驗以均方根誤差(root mean squared error,RMSE)、時均RMSE和計算復雜度為性能指標,且RMSE 定義為

時均RMSE 定義為

4.2 仿真結果

4.2.1 勻速目標跟蹤

仿真實驗首先考慮單運動觀測站跟蹤勻速運動目標的跟蹤問題,目標與觀測站的軌跡如圖1所示。

圖1 勻速目標跟蹤的幾何模型Fig.1 Geometric model of constant-velocity target tracking

目標的初始位置為[ -30,600,-300]Tm,速度為[1,6,1]Tm/s,觀測站采取Leg-by-Leg 機動方式,第1 段從[ -50,-200,-500]Tm 處開始,以[ 4,0.6,0]Tm/s的速度運動70 s,第2 段以 [4,-0.6,0]Tm/s 的速度運動70 s,第3 段以[ 4,0.6,0]Tm/s 的速度運動60 s,連續觀測的總時長為200 s,采樣間隔T=1 s,過程噪聲的功率譜密度qx=qy=qz=0.07 m2/s3。算法的初始狀態服從均值為x0=[-30,600,-300,1,6,1]T、協方差為P0|0=ρ2diag(102,102,102,0.52,0.52,0.252)的高斯分布,其中,變量ρ 控制目標狀態初始協方差的大小,可用于評估各個算法對初始化誤差的敏感程度。假設到達角的觀測噪聲以及觀測站定位誤差均為獨立同分布的高斯白噪聲,其協方差。文中使用σs=σθ=σ?量化到達角觀測噪聲,使用觀測站定位誤差的標準差與兩相鄰采樣時刻間觀測站的運動距離之比kr量化觀測站的定位誤差[19]:kr=(N-1)σx/(|vr,x|T)×100%=(N-1)σy/(|vr,y|T)×100%=(N-1)σz/(|vr,z|T)×1 00%,觀測站的速度向量vr=[vr,x,vr,y,vr,z]T。

首先,比較各算法在 σs=4?,kr=150%,ρ=4時的跟蹤情況,此時觀測站定位誤差的方差Qr,k=diag(36,0.81,0)。圖2 給出了MLE、3D-BCKF、3DIVKF、3D-UBKF、3D-LEUBKF 位置和速度估計的RMSE,其中跟蹤時刻k=N/T?？梢钥闯?MLE在位置估計上偏差較小,但對于速度的估計存在較大偏差。3D-UBKF 和3D-MUBKF 較3D-BCKF和3D-IVKF 估計偏差有大幅提升,且3D-MUBKF由于考慮了觀測站位置誤差對算法的影響,跟蹤精度更高。

圖2 勻速目標跟蹤中不同算法RMSE 曲線Fig.2 RMSE curves of different algorithms in constantvelocity target tracking

其次,比較各算法在kr=150%,ρ=4時隨到達角觀測噪聲變化的跟蹤性能,圖3 給出了在σs∈{1?,2?,···,12?}時的目標位置和速度估計的時均RMSE。可以看出,MLE 在觀測噪聲較小時偏差較小,但隨著觀測噪聲增大,算法的跟蹤性能出現大幅下降,且在 σs≥9?時,算法開始發散。3D-BCKF由于偏差補償項的準確性隨觀測噪聲的增大快速下降,算法性能衰減較大,3D-IVKF 算法由于工具變量矩陣的構造依賴于3D-BCKF 的估計結果,也表現出較大的偏差,且隨著到達角噪聲的增大,3D-BCKF 的估計偏差增大,構造的工具變量矩陣準確性降低,3D-IVKF 的性能也出現下降。在σs≤2?時,3D-UBKF 算法由于并未考慮觀測站定位誤差的影響存在較大的偏差,隨著觀測噪聲增大,觀測站定位誤差的影響減小,3D-UBKF 與3D-MUBKF的性能趨于一致,這是因為隨著觀測噪聲的增大,kr=150%時的觀測站定位誤差對偽線性噪聲協方差的影響變小,觀測噪聲成為影響算法精度的主要因素。而3D-MUBKF 關于位置和速度估計的時均RMSE 明顯小于3D-BCKF 和3D-IVKF,尤其在觀測噪聲較大時,偏差仍維持在較低水平。

圖3 勻速目標跟蹤中不同算法時均RMSE 曲線Fig.3 Time-averaged RMSE curves of different algorithms in constant-velocity target tracking

4.2.2 機動目標跟蹤

文中考慮單運動觀測站跟蹤機動目標的問題,目標與觀測站的軌跡如圖4 所示。目標的初始位置為[-30,1 000,-300]Tm,在開始的80 次掃描中以[2,-3,1]Tm/s 的速度作勻速直線運動,接著在后續的40 次掃描中,以 [0.075,-0.15,0]Tm/s2的加速度作勻加速直線運動,最后以 [5,-9,1]Tm/s 的速度在80 次掃描內作勻速直線運動,由此構成了目標的勻速—勻加速—勻速的目標軌跡。觀測站的初始位置為[-200,-300,-500]Tm,在xy平面上以與非機動場景相同的速度設置做Leg-by-Leg 的機動。連續觀測的總時長為200 s,采樣間隔T=1 s,過程噪聲的功率譜密度qx=qy=qz=0.2 m2/s3。算法的初始狀態服從均值為x0=[-30,1 000,-300,2,-3,1]T、協方差為P0|0=ρ2diag(102,102,102,0.52,0.52,0.252)的高斯分布,關于到達角觀測噪聲與觀測站定位誤差的設定與非機動場景一致。

圖4 機動目標跟蹤幾何模型Fig.4 The geometric model for maneuvering target track

圖5 給出了在 σs∈{1?,2?,···,12?},kr=200%,ρ=4時,各算法關于機動目標位置和速度估計的時均RMSE 曲線。由于MLE 算法采用批處理,在目標機動時算法性能衰減嚴重,超出圖片展示范圍,此處不再展示。從圖5 中可以看出,3D-MUBKF仍然明顯優于3D-BCKF 和3D-IVKF,算法的估計偏差小且對觀測噪聲的變化不敏感。圖6 給出了算法在 ρ=4,σs=2?,kr從60%遞進至200%時各算法關于位置估計值和速度估計值的時均RMSE曲線?？梢钥闯?3D-BCKF 和3D-IVKF 的偏差一直處于較高水平,由于算法偏差受到偏差補償準確性的影響,跟蹤精度較低,對于觀測站定位誤差變化不敏感。3D-UBKF 在kr≤120%時偏差保持在較低水平,但隨著觀測站定位誤差的增大,算法的性能出現明顯下降,因為3D-UBKF 并未考慮定位誤差的影響。而3D-MUBKF 由于考慮了觀測站定位誤差對偽線性噪聲協方差矩陣的影響,算法的跟蹤精確度更高且對于觀測站定位誤差的變化更穩定。值得注意的是,考慮到實際跟蹤過程中,難以對目標的運動模型進行預知,因此在機動目標跟蹤的仿真實驗中,這4 種算法的濾波器預測模型仍然使用勻速運動模型,在模型失配的情況下,所提出的3D-MUBKF 相較于其他算法仍然具備明顯優勢。

圖5 機動目標跟蹤中不同算法時均RMSE 曲線(kr=200%,ρ=4)Fig.5 Time-averaged RMSE curves of different algorithms in maneuvering target tracking (kr=200%,ρ=4)

圖6 機動目標跟蹤中不同算法時均RMSE曲線(ρ=4,σs=2?)Fig.6 Time-averaged RMSE curves of different algorithms in maneuvering target tracking (ρ=4,σs=2?)

圖7 給出了在 ρ ∈{1,2,···,15}、σs=2?、kr=200%時不同算法關于機動目標位置和速度估計的時均RMSE 曲線?？梢钥闯?3D-IVKF 和3D-UBKF的估計偏差隨初始化誤差水平的增大逐漸增大,而3D-MUBKF 及3D-UBKF 關于機動目標位置的估計誤差對于初始化誤差水平的變化不敏感,且始終保持在較低水平。

圖7 機動目標跟蹤不同算法時均RMSE 曲線(σs=2?、kr=200%)Fig.7 Time-averaged RMSE curves of different algorithms in maneuvering target tracking (σs=2?, kr=200%)

4.3 計算復雜度分析

由于到達角目標跟蹤問題的非線性,MLE 算法不存在閉式解,需要通過GN 算法迭代求解數值解,且MLE 采用批處理,矩陣計算的維度高,這使得MLE 的計算復雜度明顯高于基于PLKF 的4 種算法。文中通過計算浮點操作數的方式統計3DPLKF、3D-BCKF、3D-IVKF 和3D-MUBKF 等算法的計算復雜度。2 個浮點數的簡單數學運算可用一次fflops表示,復雜的矩陣運算需要用相同計算復雜度的flops 進行等效,假設A為n×m維矩陣B為n×m維矩陣,C為n×n維矩陣,不同矩陣運算對應的浮點操作次數如表1 所示。

表1 不同矩陣運算對應的浮點操作次數Table 1 Number of flops corresponding to different matrix operations

根據以上條件可得4 種算法的等效計算復雜度如表2 所示,表中nx為目標狀態向量的維數,mz為觀測向量的維數。從表2 可以看出,復雜度由低至高可排列為3D-PLKF<3D-MUBKF<3DBCKF<3D-IVKF,總體來說,3D-PLKF 的計算復雜度最低,3D-MUBKF 由于偽線性噪聲修正和噪聲分離,計算復雜度有少量增加,3D-BCKF 由于偏差補償計算復雜度增加,3D-IVKF 計算復雜度最高。在3D 到達角目標跟蹤中,狀態向量維數n取6,觀測向量維數取2,將其代入計算復雜度的分析結果可得3D-PLKF、3D-BCKF、3D-IVKF、3DMUBKF 的復雜度比值為1∶1.09∶1.61∶1.07。可以看出,3D-MUBKF 以僅次于3D-PLKF 的計算復雜度實現了更高的跟蹤精度。

表2 4 種算法計算復雜度Table 2 Computational complexity of four algorithms

5 結束語

由于PLKF 算法在3D 到達角目標跟蹤中存在估計偏差,且未考慮觀測站定位誤差的影響,提出一種改進的UBKF 算法,即3D-MUBKF。該算法首先采用修正噪聲協方差矩陣的措施來降低觀測站定位誤差的影響,進一步通過分離觀測矩陣中的噪聲,克服由觀測矩陣和偽線性噪聲相關性引起的偏差。仿真實驗結果表明,相較于已有算法,3D-MUBKF 在非機動和機動場景下均具有性能優勢,可以較少的計算復雜度實現更高的跟蹤精度,且在觀測噪聲、觀測站定位誤差以及初始化誤差較大時有效降低跟蹤誤差。

下一階段為了擴大3D 到達角目標跟蹤的應用范圍,需要研究觀測噪聲和觀測站定位誤差等先驗信息難以獲得以及機動目標運動模型難以預知時的目標跟蹤算法。