一種基于多級Kalman濾波的高精度距離估計方法

唐 玲，杜雨洺

(成都信息工程學院電子工程學院，四川成都610225)

0 引言

在對慢速運動目標進行距離估計時，由于目標運動速度過慢，短時間內可以認為其處于靜止狀態(tài)，在強干擾環(huán)境中，目標測量誤差可能遠遠大于其實際運動的距離，在這一情況下，測量值可能很難落在誤差允許范圍之內，從而造成數(shù)據(jù)無法真實反應目標運動軌跡[1]。在真實復雜環(huán)境下，由于障礙物和多徑干擾的原因，測量值中可能出現(xiàn)大量野值點，直接對其進行濾波會導致濾波估計值明顯地偏離系統(tǒng)真實狀態(tài)，導致目標運動軌跡失真[2]。因此研究對慢速目標的高精度距離估計具有重要的實際意義。

通過對慢速運動目標的特點進行分析，提出一種兩級Kalman濾波的距離估計方法，以被測目標的當前時刻距離測量值為輸入，對一分鐘內的測量值取均值進行差值檢測，上一時刻最優(yōu)估計與當前時刻測量值的絕對差作為差值門限檢測的標準距離差以判斷下一時刻數(shù)據(jù)點是否為野值點，計算當前時刻與標準距離的差值，對當前時刻落在門限范圍外的值進行修正，以修正后的數(shù)據(jù)作為第一級Kalman濾波的初始狀態(tài)向量，這樣避免了野值點對濾波效果的不利影響，同時不會對距離變化響應時間有較大影響。

1 卡爾曼濾波

1.1 Kalman濾波理論

Kalman濾波器是線性、無偏、最小方差估計器，是一種通過迭代算法實現(xiàn)的估算器[3]，原理是從觀測到的向量中對隨著時間不斷變化的狀態(tài)向量進行估計，并且希望估計值與狀態(tài)向量的真實值的誤差越小越好[4]，Kalman濾波在處理動態(tài)觀測數(shù)據(jù)時以其計算量少及實時性在很多方面得到了廣泛應用[5]。在跟蹤運動目標的系統(tǒng)中，可以把運動目標的位置信號看成一個離散動態(tài)系統(tǒng)，用k+1時刻的目標狀態(tài)方程X(k+1)和測量方程Z(k)描述該系統(tǒng)。

式中，X(k)為k時刻的目標狀態(tài)向量，W(k)為k時刻目標運動過程噪聲，U(k)為k時刻對系統(tǒng)的控制量，A、B、H為系統(tǒng)參數(shù)，Z(k)為k時刻觀測值，V(k)為k時刻測量噪聲。在測距系統(tǒng)中有

(1){W(k)}、{V(k)}是均值為零的高斯白噪聲隨機序列，即

(2)狀態(tài)向量、觀測噪聲、過程噪聲互不相關，即

式中，k，j均表示時刻，上角標T表示矩陣的轉置。

Kalman濾波是結合當前時刻的測量值，對上一時刻的預測進行校正，得到校正后的最優(yōu)估計，該估計具有最小方差。一般來說，Kalman濾波分為兩個步驟:預測、校正。預測即對時間狀態(tài)向量進行更新，校正即對測量狀態(tài)向量進行更新[6]。當Q、R值確定時，由下列遞推公式得到目標的狀態(tài)估計值:

1.2 多級Kalman濾波方法應用于慢速運動目標距離估計

在對慢速運動目標距離估計數(shù)據(jù)處理時，經典Kalman濾波存在不足。

(1)在應用Kalman對數(shù)據(jù)進行濾波處理時，只需要知道被研究對象的數(shù)學模型和噪聲統(tǒng)計的先驗知識，并不考慮測量值是否有現(xiàn)實意義，即只需要根據(jù)驗前確定的系統(tǒng)參數(shù)和噪聲方差的初始值，按照濾波公式遞推下一時刻的估計值[8]。當目標運動狀態(tài)量中有野值點出現(xiàn)時，Kalman濾波也會跟蹤野值，這對估計結果有很大影響[9]。因此，直接對測量值進行Kalman濾波可以剔除孤立型野值點，但是對連續(xù)型野值點進行剔除時[10]，由于Kalman濾波也會對每一個野值點按權重累積，這對濾波結果影響很大，可能嚴重降低濾波精度。

(2)慢速運動目標的特點是目標與觀測點之間的相對運動速度很慢，若觀測點處于靜止狀態(tài)，短時間內可以忽略目標運動速度，把運動目標看成是靜止狀態(tài)建模，這樣系統(tǒng)具有可觀測性[11]，但是當目標運動到一定距離時，那么測量值偏離真實值的誤差有可能遠遠大于目標實際運動的距離，因此目標的運動給測量帶來了誤差[12]。在這種情況下，經過Kalman濾波后卻不能立即在數(shù)據(jù)上反應出來，即不能在實時對距離變化做出響應，因此不能判斷目標狀態(tài)是否發(fā)生改變。

(3)對于慢速運動目標而言，當改變?yōu)V波參數(shù)提高測距精度時，相鄰估計值之間的差值受濾波精度影響，如果被測目標位置發(fā)生變化，一個數(shù)據(jù)率時間內變化的距離值為厘米級以下，濾波精度過高時被平滑處理，不能在數(shù)據(jù)上體現(xiàn)出距離的變化。因此，濾波精度越高系統(tǒng)反應時間越長，可以看出系統(tǒng)反應時間和濾波精度相互影響并且相互矛盾。

考慮到以上幾個問題，采用多級Kalman濾波來對慢速運動目標距離估計數(shù)據(jù)進行處理，通過改變第一級和第二級濾波參數(shù)來調和系統(tǒng)反應時間和濾波精度不能同時優(yōu)化的矛盾。Kalman濾波前對初始值進行預處理，取一組數(shù)據(jù)的均值進行野值點的判定，當數(shù)據(jù)超過判定門限時，用一個分段函數(shù)加權改變新舊數(shù)據(jù)比重，降低漏判和誤判的概率;否則，進行第一次濾波。取第一級Kalman濾波結果的最小值作為第二級濾波的先驗估計值，從而得到最優(yōu)估計，這樣既減小了濾波誤差，同時在不同場合將反應時間控制在允許范圍內。多級Kalman濾波流程如圖1所示。

圖1 多級Kalman濾波流程圖

2 實驗仿真及結果分析

以德國Nanotron公司的nanoLOC通訊測距模塊[13]為例，利用實驗數(shù)據(jù)仿真驗證多級Kalman濾波方法的有效性。理論上，nanoLOC距離估計精度在3 m左右[14]，這在對精度要求很高的環(huán)境下不能滿足要求。為了驗證多級Kalman濾波在提高慢速目標距離估計精度的效果，以15 m實際觀測值作為一級Kalman濾波的先驗估計值，在MATLAB上進行仿真實驗。該模塊的數(shù)據(jù)率為0.02 s，目標的速度0.001 m/s，在二維平面里，目標的運動距離為2×10－5m，而測量值偏離真實值的距離(測量誤差)為0.6～3 m，測量誤差遠遠大于目標的運動距離。

2.1 濾波模型建立

Kalman濾波最主要的一步就是建立系統(tǒng)的狀態(tài)模型，在實際應用中很難對系統(tǒng)進行精確的描述，因此在實際中常用近似模型代替。為了數(shù)據(jù)精度得到較大改善的同時不影響響應時間，設計了兩級Kalman濾波，第一級濾波對系統(tǒng)測量得到的距離計數(shù)據(jù)進行第一次估計，得到改進后的估計值，這個估計值作為第二級濾波的先驗估計值，進行第二級濾波，最終完成對距離信息的完整估計。

2.2 主要參數(shù)選取

在實際工程中，濾波初始值和估計誤差方差初值都會影響系統(tǒng)的穩(wěn)定性，且精確的初始值可以獲得最優(yōu)的濾波估計值。如果初始值選取不恰當，長時間之后，可能導致濾波發(fā)散，所以選取適合的濾波參數(shù)對濾波精度起著至關重要的作用。

2.2.1 濾波初始值選定

在距離測量系統(tǒng)中，由于噪聲的不確定性，濾波初始值對整體濾波效果的影響很大，Kalman濾波器中初始值為后續(xù)估計的起始參考值，唯一地確定了估計模型的起始估計點，的誤差將直接影響濾波器的收斂速度，誤差越大則濾波器的收斂速度越慢。因此，為了確保數(shù)據(jù)較快的收斂初始值應盡可能的準確，取系統(tǒng)開啟后前5分鐘的數(shù)據(jù)均值，即=，其中N為5分鐘內數(shù)據(jù)樣本量。

2.2.2 過程噪聲方差Q值選取

過程噪聲方差Q反應連續(xù)兩個時刻的距離方差，它的好壞對濾波精度有直接的影響，Q值選取的一個基本原則是使Q值大小與動態(tài)模型的精度相匹配。由于距離測量系統(tǒng)的目標不斷運動，外界環(huán)境也在不斷發(fā)生變化，所以采用自適應的方法估計過程噪聲方差[15]。

通過對實驗數(shù)據(jù)的分析可知，一級Kalman濾波中Q值應該比二級Kalman濾波中Q值小一個數(shù)量級，通過先進行較低精度濾波對距離數(shù)據(jù)中差距較大的值進行粗略的估計，在進行較高精度的濾波獲取較好的估計，這樣達到對濾波精度的較好控制。最終選取Q1=0.85 × 10－3，Q2=10－4。

2.2.3 測量噪聲方差R值選取

測量噪聲方差R同樣是很重要的一個參數(shù)，反應目標距離的測量精度，通過對測量數(shù)據(jù)和距離測量系統(tǒng)動力學方程建模得知，當R=3時，系統(tǒng)的收斂性和測量精度能達到較好的平衡。

2.3 仿真實驗及數(shù)據(jù)分析

2.3.1 濾波精度

圖2為經nanoLOC通訊測距模塊測量得到的原始測量信息，可以看出由測距模塊測量得到的數(shù)據(jù)中斑點型野值點較多，導致測量數(shù)據(jù)誤差高達3.6 m;由前面分析可知，直接進行Kalman濾波濾除斑點型野值點可能導致系統(tǒng)對距離變化的反應時間變慢，因此對圖2中的數(shù)據(jù)進行差值檢測，圖3經差值檢測后的數(shù)據(jù)即第一級Kalman濾波的初始狀態(tài)值，從圖3看出剔除原始數(shù)據(jù)中斑點型野值點后誤差從3.6 m減小到1.78 m，誤差下降1.82 m;圖4為經第一級Kalman濾波后的數(shù)據(jù)，誤差從1.78 m減小到0.87 m，下降0.91 m;圖5為經過第二級Kalman濾波后的距離信息，此時誤差已經減小到0.45 m。

對比分析可以看出，Kalman濾波的方法可以有效地對數(shù)據(jù)進行預估和校正，使輸出變得更平滑。表1為不同時期濾波精度值。

圖2 原始距離信息

圖3 差值檢測后的值

圖4 一級Kalman濾波后的結果

圖5 二級Kalman濾波后的結果

表1 各階段濾波數(shù)據(jù)精度值比較

2.3.2 反應時間

對慢速運動目標而言，系統(tǒng)反應時間是很重要的指標，決定了系統(tǒng)是否能及時的對目標位置變化做出反應。以初始距離12.5 m為例，系統(tǒng)處于穩(wěn)定狀態(tài)時移動一段距離(測得移動距離為0.8 m)，若反應時間在10分鐘以內則認為符合要求，對測量誤差和系統(tǒng)反應時間進行分析。圖6為原始距離信息，在距離變化前測量誤差為2.12 m，經過5.42分鐘系統(tǒng)對該變化做出反應，并且距離變化后的誤差為3.23 m。可以看出測量值偏離真實值的誤差遠大于目標實際運動的距離，對于慢速運動目標來說，不能達到測量要求。經過多級Kalman濾波處理后，測量誤差越來越小，系統(tǒng)反應時間也能控制在規(guī)定時間內。最終結果如圖9所示，在距離變化前測量誤差為0.47 m，經過9.89分鐘對距離變化做出反應，距離變化后的誤差為0.37 m，可以看出測量值偏離真實值的誤差小于目標實際運動的距離。對于慢速運動目標來說，測量精度和對距離變化的反應時間都符合系統(tǒng)測量要求。

表2 距離變化中各階段數(shù)據(jù)反應時間

圖6 原始距離信息

圖7 差值處理結果

圖8 第一級Kalman濾波結果

圖9 第二級Kalman濾波結果

3 結束語

通過對nanoLOC測量數(shù)據(jù)的分析，提出對距離測量系統(tǒng)的目標距離信息進行兩級Kalman濾波處理，建立對應的濾波模型，且對主要的濾波器參數(shù)的選取進行討論。實驗結果表明，方法簡單有效，在實際應用中針對具體要求可以增加Kalman濾波級數(shù)，具有較強的實用性。

[1] 易哲，王國宏，張翔宇，陳澤元.一種高數(shù)據(jù)率雷達跟蹤慢速目標的數(shù)據(jù)互聯(lián)算法[J].電學與控制，2014，21(7):31－35.

[2] 祝轉民，秋宏興，李濟生，等.動態(tài)測量數(shù)據(jù)野值的辨識與剔除[J].系統(tǒng)工程與電子技術，2004，26(2):147－149.

[3] 黃鶴，張會生，許家棟，等.一種基于二級變維Kalman濾波的跟蹤算法[J].航空計算技術，2008，38(3):97－100.

[4] 張國權，倪重匡.應用Kalman濾波提高雷達測量精度[J].電子學報，1981，3:19－25.

[5] 樊建新，董緒榮，羅定開，等.動態(tài)測量問題數(shù)據(jù)處理的一個快速濾波算法[J].武漢測繪科技大學學報，1998，23(2):111－114.

[6] 余翔，馮璐，漆晶.一種組合式的Kalman濾波算法[J].電視技術，2013，37(9):168－170.

[7] 趙軍.基于C語言的測量數(shù)據(jù)修正技術[J].計算機與網絡，2013，15:48－50.

[8] 秦慶強，張曉安，李艾華.基于自適應Kalman濾波的加速度計動態(tài)數(shù)據(jù)處理[J].計算機工程與設計，2009，30(17):3943－3945.

[9] 朱學鋒，韓榮閣，楊若紅.基于模糊預測系統(tǒng)的觀測數(shù)據(jù)野值剔除方法[J].系統(tǒng)工程與電子技術，2006，28(3):478－482.

[10] 張帆，盧崢.自適應抗野值Kalman濾波[J].電機與控制學報，2007，11(2):188－190.

[11] 張凱，劉洋.慢速目標僅測角無源定位算法研究[J].雷達科學與技術，2014，12(3):291－296.

[12] 劉愛東，倪永強，王建國.一種實時剔除雷達測量數(shù)據(jù)中野值的方法分析[J].火力與指揮控制，2004，29(S1):17－19.

[13] 余文芳，胡旭科.基于線性調頻的nanoLOC新技術與應用研究[J].信息通信，2011，2:4－6.

[14] 鞏家昌，張軍.Nanotron技術在無線測距系統(tǒng)中的應用[J].現(xiàn)代電子技術，2011，34(3):56－58.

[15] 祝轉民，秋宏興，李濟生，等.Kalman濾波工程應用問題分析及改進方法研究[J].宇航學報，2002，23(3)，44－47.