Kalman濾波在勻速直線運動目標跟蹤中的應用研究

柴華溢, 張清泉*, 李俊吉

1. 山西師范大學物理與信息工程學院,山西 臨汾 041000；2. 太原科技大學計算機科學與技術學院，山西 太原 030024

針對高斯白噪聲的濾除方法有很多，對于不同的信號傳輸過程中的污染應采用濾除效果更好的濾波辦法來縮小誤差.已知的幾種濾波理論均需要大量的采樣數據，各自根據一定的濾波準則，計算量非常多而且要求巨大的數據存儲量.Kalman濾波估計過程中利用的信息都是時域內的量，不用在頻域內對信號功率譜作分解，所以可以在時域內設計最優濾波器，應用領域更加廣泛.濾波器設計并不復雜，利用系統過程噪聲和觀測噪聲的統計特性形成遞推算法，可方便地通過計算機實現實時應用[1].

1 一維直線運動目標跟蹤問題分析和數學模型的建立

1.1 Kalman濾波原理

設tk時刻的被估計狀態Xk受系統噪聲序列Wk-1驅動，其狀態方程表述為

Xk=Φk,k-1Xk-1+Γk-1Wk-1

(1)

對Xk的量測符合線性關系，其量測方程表述為

Zk=HkXk+Vk

(2)

式中Φk,k-1為狀態轉移矩陣，Γk-1為噪聲驅動矩陣，Hk為觀測矩陣，Vk為量測噪聲序列，Wk為系統激勵噪聲序列，Wk和Vk是均值為零、方差陣各為Q和R的不相關白噪聲.

(3)

(4)

(5)

(6)

(7)

1.2 直線運動目標跟蹤問題分析

給出上一時刻狀態Xt-1，那么運動目標當前時刻Xt的狀態的位置及速度表示如下

(8)

vt=vt-1+Ut×Δt

(9)

目標狀態Xt的位置與速度的輸出變量是其輸入變量的線性組合，線性關系存在于前后狀態之間，可用矩陣的形式表示成

(10)

狀態變換矩陣A和控制矩陣B如下

通過狀態轉移矩陣A，由上一時刻的狀態可推出當前時刻的狀態.B為控制矩陣，它表示控制量Ut如何作用當前狀態，Δt代表時間采樣間隔.H為觀測矩陣[2].

由此可得出該問題下Kalman濾波數學模型

(11)

Z=HXt+Vt

(12)

1.3 卡爾曼濾波流程分析

Kalman濾波經過以上數學模型的推導，建立的流程圖如圖1所示.由圖1可以看出當給定初始條件后，第一步在控制項的作用下，進行狀態更新：第k時刻的狀態通過k-1時刻的狀態預測；第二步進行先驗協方差估計：第k時刻的系統預測誤差根據第k-1時刻系統預測誤差來估計；第三步是計算Kalman增益；第四步用測量值更新狀態估計：計算系統最優估算值；第五步是更新系統的誤差協方差估計：計算當前時刻的預測誤差.以上五步描述了Kalman濾波的觀測更新過程[3].

2 基于Kalman濾波的勻速直線運動目標跟蹤分析

設某一物體做勻速直線運動，觀測裝置對其位移進行檢測，在傳感器受到未知的高斯信號的干擾下，基于Kalman濾波來估計運動物體的位置和速度.

過程噪聲的方差矩陣取Q=[0.01,0;0,0.05],觀測噪聲方差取R=15;狀態估計初始值取位置為0，速度為1；誤差協方差矩陣初始值P0=[3,0;0,1].狀態轉移矩陣A=[1,1；0,1]，控制矩陣B=[0.5,1],觀測矩陣H=[1,0][4].

根據已有的運動學方程，得出該運動物體的狀態方程

(13)

給定位置觀測裝置，在測量值受到某種獨立的隨機干擾下的觀測方程為

(14)

對一維物體勻速運動下的實時位置，速度進行Kalman濾波估計，得到的仿真結果如圖2.

圖1 Kalman濾波流程圖Fig.1 Kalman filter flow chart

圖2 Kalman估計、觀測值與實際值比較圖
Fig.2 Comparison chart of Kalman estimation, observation value and actual value

Kalman濾波后得到的位置、速度以及觀測法得到的位置與實際位置比較曲線如圖2所示.圖2(a)是實際位置與Kalman濾波估計位置比較圖，可以看出Kalman濾波估計值圍繞在實際位置附近，波動較小.圖2(b)是實際速度與Kalman濾波估計速度的比較圖，可以看出Kalman濾波的估計速度也接近于實際速度，偏離程度也較小.圖2(c)是實際位置和普通觀測法得到的位置比較圖，可以看出觀測值受到測量噪聲的污染，擺動幅度大，路程偏差大.由圖2(a)與圖2(c)比較可以看出，使用Kalman濾波算法有效地降低了白噪聲的污染，Kalman濾波估計位置比觀測位置更加接近于實際位置[4].

圖3 kalman估計、觀測值與實際值誤差圖
Fig.3 Kalman estimation, observation and actual value error map

Kalman 濾波后得到的位置、速度以及觀測法得到的位置與實際值誤差曲線如圖3所示.圖3(a)是Kalman濾波估計位置與實際位置誤差圖，可以看出估計誤差在5米之內，誤差較小.圖3(b)是Kalman濾波估計速度與實際速度誤差圖，可以看出誤差基本在1米/min內，誤差較小.圖3(c)是實際位置與觀測位置誤差圖，可以看出誤差接近于10米，誤差較大.由此可知，Kalman濾波估計位置與實際位置比較的誤差要比觀測位置與實際位置比較的誤差要小.Kalman濾波估計速度誤差擺動不大.

3 結束語

綜上所述，Kalman濾波在一維直線運動物體中的估計效果是比較好的.在高斯白噪聲的干擾下，Kalman濾波估計位置比觀測位置更加接近于實際位置，Kalman濾波估計速度與實際速度的誤差也較小.通過對系統的數學模擬和仿真實驗，證實了Kalman濾波算法在平穩的一維隨機過程狀態應用中得出的估計精度是比較高的.所以在工程上，可以用Kalman濾波去估計實際位置和實際速度，然后進行實時跟蹤[5].