1、第二章跟蹤濾波與自適應(yīng)技術(shù)2.1 卡爾曼濾波與預(yù)測所謂卡爾曼濾波就是從混合在一起的諸多信號中提取出所需要的信號。卡爾曼濾波從被提取信號有關(guān)的量測量中通過算法估計出所需信號。 其中被估計信號是由白噪聲激勵所引起的隨機(jī)響應(yīng)，激勵源與響應(yīng)之間的傳遞結(jié)構(gòu)（系統(tǒng)方程）已知，量測量與被估計量之間的函數(shù)關(guān)系（量測方程）也已知。估計過程中利用了如下信息：系統(tǒng)方程、量測方程、白噪聲激勵的統(tǒng)計特性、量測誤差的統(tǒng)計特性。由于所用信息都是時域內(nèi)的量，所以卡爾曼濾波是在時域內(nèi)設(shè)計的 ，且適用于多維情況，這就完全避免了維納濾波器在頻域內(nèi)設(shè)計遇到的限制和障礙， 適用范圍遠(yuǎn)比維納濾波器廣泛。卡爾曼濾波有如下特點：(1)卡爾曼

2、濾波處理的對象是隨機(jī)信號；(2)被處理信號無有用和干擾之分，濾波的目的是要估計出所有被處理信號；(3)系統(tǒng)的白噪聲激勵和量測噪聲并不是需要濾除的對象， 它們的統(tǒng)計特性正是估計過程中需要利用的信息。所以確切的說， 卡爾曼濾波應(yīng)稱作最優(yōu)估計理論， 此處所謂的濾波與常規(guī)濾波具有完全不同的概念和含義。隨著現(xiàn)代微處理技術(shù)的發(fā)展， 卡爾曼濾波的計算要求與復(fù)雜性已不再成為其應(yīng)用的障礙，并且越來越受到人們的青睞， 尤其在機(jī)動目標(biāo)跟蹤系統(tǒng)中更顯出其獨特的優(yōu)點。對于單機(jī)動目標(biāo)跟蹤情形，其跟蹤的基本原理如圖 2.1 所示。圖中目標(biāo)跟蹤動態(tài)特性由包含位置、速度和加速度的狀態(tài)向量 X 表示，量測（觀測） Y 被假定為含

3、有量測噪聲 V 的狀態(tài)向量的線性組合 (HX+V) ；殘差（新息）向量 d 為量測 (Y)與狀態(tài)預(yù)測?(k1/ k ) ）之差。量 (HX一般情況下，單機(jī)動目標(biāo)跟蹤為一自適應(yīng)濾波過程。首先由量測量Y 和狀態(tài)預(yù)測量?(k1/k) 構(gòu)成殘差（新息）向量，然后根據(jù)d的變化進(jìn)行機(jī)動檢測或者機(jī)(HXd動辨識，其次按照某一準(zhǔn)則或邏輯調(diào)整濾波增益與協(xié)方差矩陣或者實時辨識出目標(biāo)機(jī)動特性，最后由濾波算法得到目標(biāo)的狀態(tài)估計值和預(yù)測值， 從而完成單機(jī)動目標(biāo)跟蹤功能。1動態(tài)特性目標(biāo)增益確定量測Y=HX+VX?輸出d Y HX (k 1/ k)機(jī)動檢測與目濾波標(biāo)辨識-?HX (k1/ k)X (k / k)?X ( k

4、1/ k )變換H預(yù)測圖 2.1 單機(jī)動目標(biāo)跟蹤基本原理框圖單機(jī)動目標(biāo)跟蹤基本要素主要包括量測數(shù)據(jù)形成與處理、機(jī)動目標(biāo)模型、 機(jī)動檢測與機(jī)動辨識、 濾波與預(yù)測以及跟蹤坐標(biāo)系和濾波狀態(tài)變量的選取。下面將介紹卡爾曼濾波及其預(yù)報方程。卡爾曼濾波與預(yù)測基本方程濾波與預(yù)測是跟蹤系統(tǒng)的最基本要素， 也是估計當(dāng)前和未來時刻目標(biāo)運動參數(shù)如位置、速度和加速度的必要技術(shù)手段。當(dāng)目標(biāo)做非機(jī)動運動時，采用基本的濾波與預(yù)測方法即可很好地跟蹤目標(biāo)。這些方法主要有線性回歸法、維納濾波、或濾波以及卡爾曼濾波等。卡爾曼濾波的基本方程濾波的目的是估計當(dāng)前和未來時刻目標(biāo)的運動狀態(tài)，包括位置、速度和加速度等。假定目標(biāo)狀態(tài)方程和量測方

5、程為下列形式X(k+1)= (k+1, k)X(k)+G(k)W(k)（21）Y(k)=H(k)X(k)+V(k)（22）式中 X(k) Rn 1 為目標(biāo)狀態(tài)向量， Y(k) Rm 1 為量測向量， W ( k)R p 1 和 V ( k) Rm 1分別為狀態(tài)噪聲和量測噪聲， 且為互不相關(guān)的高斯白噪聲向量序列，其協(xié)方差矩陣分2（ ）和 R(k)； ( k 1, k)Rn n， G(k)Rn p和 H (k ) Rm n分別為狀態(tài)轉(zhuǎn)移矩別為 Q k陣、輸入矩陣和觀測矩陣。設(shè)目標(biāo)狀態(tài)方程和量測方程分別為（ 21）和（ 22）。其中 X(k)為 n 維目標(biāo)狀態(tài)向量，Y(k)為 m 維量測向量，狀態(tài)噪

6、聲 W(k)和量測噪聲 V(k)為互不相關(guān)的高斯白噪聲序列，其統(tǒng)計特性為EW (k)0, EW (k )W T ( j )Q(k) kjEV (k)0, EV ( k)V T ( j )R(k ) kj并且初始狀態(tài) X 0 與 W(k)，V(k)獨立，即E X 0W T (k )0, E X 0V T (k )0卡爾曼濾波基本方程為 ：？?1)X ( k / k ) X (k / k1) K ( k) Y(k) H (k) X (k / k?1)(k?1 / k1)X ( k / k1) X (kK (k)P( k / k1) H T (k ) H ( k) P(k / k1)H T (k )

7、R( k)1P( k / k1)( k1, k) P(k1/ k1)T (k1,k )G(k1)Q(k 1)G T (k 1)P( k / k) IK (k) H (k ) P(k / k1)其中，殘差（新息）向量被定義為?d (k )Y(k)H (k ) X (k / k1)其協(xié)方差矩陣為S(k )H ( k) P(k / k1)H T ( k)R( k)卡爾曼一步預(yù)測基本方程在機(jī)動目標(biāo)跟蹤， 特別是機(jī)動多目標(biāo)跟蹤中， 濾波預(yù)測量極為重要。 下面給出卡爾曼一步預(yù)測基本方程：?1/ k)(k?1)X (k1/ k) X (k / k 1) K p (k) Y(k )H (k ) X (k /

8、kK p (k )(k1/ k )P(k / k1)H T (k) H T (k ) P( k / k1)H T (k)R(k) 1P(k1/ k )( k1, k)K p (k )H ( k)P(k / k 1)G( k)Q(k )G T (k)式中 K p (k) 為一步預(yù)測增益矩陣。3卡爾曼濾波與預(yù)測在機(jī)動目標(biāo)跟蹤中的作用卡爾曼濾波與預(yù)測的準(zhǔn)則是均方差最小 。除此之外， 它在機(jī)動目標(biāo)跟蹤中還有許多其他優(yōu)點，這些優(yōu)點或作用包括以下幾個方面：（1）基于目標(biāo)機(jī)動和量測噪聲模型的卡爾曼濾波與預(yù)測增益序列可以選擇。這意味著通過改變一些關(guān)鍵性參數(shù)，相同的濾波器可以適應(yīng)于不同的機(jī)動目標(biāo)和量測環(huán)境。（2

9、）卡爾曼濾波與預(yù)測增益序列能自動的適應(yīng)檢測過程的變化，包括采樣周期的變化和漏檢情況。（3）卡爾曼濾波與預(yù)測通過協(xié)方差矩陣可以很方便地對估計精度進(jìn)行度量。同時，在機(jī)動多目標(biāo)跟蹤中，這種度量工具還可以用于跟蹤門的形成，以及門限大小的確定。（4）通過卡爾曼濾波與預(yù)測中殘差向量 d(k)的變化，可以判斷原假定的目標(biāo)與實際目標(biāo)的運動特性是否相符。 因而， d(k)可用來作為機(jī)動檢測與機(jī)動辨識的一種手段。同時，還可用于一致性分析等；（5）在密集多回波環(huán)境下的多機(jī)動目標(biāo)跟蹤方面，通過卡爾曼濾波與預(yù)測方法的使用，可以部分地補(bǔ)償誤相關(guān)的影響，可以通過增大協(xié)方差矩陣來反映不確定性相關(guān)誤差的影響。2.2 目標(biāo)跟蹤中

10、的自適應(yīng)濾波很多控制對象的數(shù)學(xué)模型隨時間和工作環(huán)境的改變而改變，其變化規(guī)律事先往往不知道。當(dāng)控制對象的數(shù)學(xué)模型參數(shù)在小范圍內(nèi)變化時， 可用一般的反饋控制、 最優(yōu)控制或補(bǔ)償控制等方法來消除或減小參數(shù)變化對控制品質(zhì)的有害影響。 如果控制對象的參數(shù)在大范圍內(nèi)變化時， 這些方法就不能圓滿地解決問題。 為了解決控制對象參數(shù)在大范圍內(nèi)變化時， 系統(tǒng)仍能自動地工作于最優(yōu)或接近最優(yōu)工作狀態(tài)， 就提出了自適應(yīng)控制問題。自適應(yīng)控制可簡單地定義如下：在系統(tǒng)工作中，系統(tǒng)本身能不斷地檢測系統(tǒng)參數(shù)或運行指標(biāo)，根據(jù)參數(shù)的變化或運行指標(biāo)，改變控制參數(shù)或改變控制作用， 使系統(tǒng)運行于最優(yōu)或接近于最優(yōu)工作狀態(tài)。自適應(yīng)控制也是一種反

11、饋控制 ，但它不是一般的系統(tǒng)反饋或系統(tǒng)輸出反饋，而是一比較復(fù)雜的反饋控制，自適應(yīng)控制系統(tǒng)很復(fù)雜，即使對于線性定常的控制對象，其自適應(yīng)控制亦是非線性時變反饋控制系統(tǒng)。 所以自適應(yīng)控制系統(tǒng)比一般反饋系統(tǒng)復(fù)4雜。機(jī)動檢測與機(jī)動辨識的基礎(chǔ)是殘差過程的利用，因此所有跟蹤算法都是以卡爾曼濾波為出發(fā)點的。檢測自適應(yīng)濾波的基本思想是：機(jī)動的發(fā)生將使原來的模型變差，從而造成目標(biāo)狀態(tài)估計偏離真實狀態(tài)，濾波殘差（新息）特性發(fā)生變化，于是，人們可以根據(jù)殘差過程的變化， 設(shè)計出機(jī)動檢測準(zhǔn)則， 一旦檢測到機(jī)動發(fā)生或消除，立即進(jìn)行模型轉(zhuǎn)換或方差調(diào)整。 此類算法的關(guān)鍵在于設(shè)計出合理的檢測方式，包括檢測門限的選擇以及恰當(dāng)?shù)哪Ｐ?/p>

12、轉(zhuǎn)換與調(diào)整等。以下將介紹2 自適應(yīng)卡爾曼濾波 。考慮如下的線性系統(tǒng)：(k1)(k1, k )X (k)(k )W (k )Y(k)H (k ) X (k ) V ( k)式 中 X (k )Rn 1，通常包括目標(biāo)位置、速度，有時包括加速度；(k 1, k )Rn n ,( k)Rn p 和H ( k)Rm n分別為狀態(tài)轉(zhuǎn)移矩陣 、輸入矩陣和量測矩陣；W (k )R p 1和V (k)Rm 1 分別為互不相關(guān)的零均值高斯白噪聲，其協(xié)方差矩陣分別為 ) Q(k)和 R(k)。對系統(tǒng)方程應(yīng)用卡爾曼濾波，并且設(shè)初始狀態(tài)X(0)與 W(k)和 V(k)互不相關(guān)，那么，由“新息理論”可知，新息向量?d(k

13、)Y( k)H ( k) X (k / k1)也是零均值高斯白噪聲過程，且其協(xié)方差矩陣為：S(k )H ( k) P(k / k 1)H T ( k) R( k)?1) 為狀態(tài)一步預(yù)測估計， P( k / k1) 為預(yù)測協(xié)方差矩陣。式中 X（ k / k定義距離函數(shù)D(k )d T ( k)S 1 (k )d ( k)由新息序列的統(tǒng)計性質(zhì)可知，D(k) 服從自由度為 m 的2 分布。如果目標(biāo)發(fā)生機(jī)動，新息 d(k)將會不再是零均值高斯白噪聲，D(k) 將會變大，因而可用下述方法檢測機(jī)動的發(fā)生與消除：5取 D(k)大于某一門限M 的概率為 a，即PD(k)>M=a式中 a 為允許的虛警概率

14、。此時自適應(yīng)濾波過程為：當(dāng) D(k)>M 時，機(jī)動發(fā)生，增大 Q(k) 矩陣；當(dāng) D(k) M 時，機(jī)動消除，減小 Q(k) 矩陣。上述自適應(yīng)濾波過程的目的是，保持并恢復(fù)新息過程的白高斯性質(zhì)，并降低狀態(tài)估計誤差。檢測自適應(yīng)濾波類方法側(cè)重于非機(jī)動模式，機(jī)動時性能有所下降， 同時由于需要機(jī)動檢測，不可避免地存在著虛警率和估計時間延遲，影響濾波器的跟蹤性能。2.3 跟蹤精度影響跟蹤精度的因素機(jī)動目標(biāo)跟蹤的主要問題之一是建立未知的目標(biāo)加速度模型。顯然，影響跟蹤精度最直接和最重要的因素是模型和算法。然而，盡管模型和算法已事先選定，但在系統(tǒng)設(shè)計中仍有一些參數(shù)需要分配與確定。第一個因素是目標(biāo)機(jī)動時間常

15、數(shù)的倒數(shù)，機(jī)動頻率a。由于目標(biāo)怎樣機(jī)動和機(jī)動時間多長是目標(biāo) （如飛機(jī)）駕駛員的主觀決策， 當(dāng)邊掃描邊跟蹤系統(tǒng)用作多目標(biāo)跟蹤目的時，要預(yù)測 a 值有多大無疑是一件十分困難的事。因此，在系統(tǒng)設(shè)計中最好是將a 設(shè)計成可調(diào)參數(shù)而不是從實際情況所得到的參數(shù)。 a 包含在狀態(tài)轉(zhuǎn)移矩陣 中并通過卡爾曼濾波增益矩陣來影響系統(tǒng)跟蹤誤差的。影響估計結(jié)果的另一個參數(shù)是采樣周期 。一般的說，采樣周期越小，跟蹤濾波器進(jìn)入穩(wěn)態(tài)越快。 然而，由于采樣周期 T 是狀態(tài)轉(zhuǎn)移矩陣中的一個參數(shù)， 因此它也影響著跟蹤精度。狀態(tài)噪聲 的協(xié)方差是與目標(biāo)加速度方差a2 相聯(lián)系的，因此，a2 將直接影響估計誤差的協(xié)方差矩陣。如果采用加速度方

16、差自適應(yīng)算法，那么a2 將取決于最大機(jī)動加速度 amax 。機(jī)動頻率對跟蹤精度的影響6圖 2.2、圖 2.3 和圖 2.4 分別給出了位置、速度和加速度估計誤差的均方根值隨機(jī)動頻率 a 變化的計算結(jié)果。圖 2.2圖 2.3圖中曲線表明，跟蹤精度隨 a 的變化還與采樣周期 T 有關(guān)，當(dāng) T=0.1s 時，位置、速度和加速度的估計誤差隨 a 的增大而減小；當(dāng) T=0.5s 和 T=1s 時，估計誤差隨 a 的增大而增大；當(dāng) T=5s 時，估計誤差在某一a 值處取最大值。通常， T=0.1s 表示很高的數(shù)據(jù)采樣速率，只有相控陣?yán)走_(dá)才能達(dá)到這一要求；T=5s 表示較低的數(shù)據(jù)采樣速率，這種情況不適合于跟

17、蹤高度機(jī)動圖 2.4目標(biāo)；對一般情況， 即 T=0.52s，較低的 a 將給出較好的跟蹤精度。采樣周期對跟蹤精度的影響圖 2.2、圖 2.3 和圖 2.4 表明，在大多數(shù)情況下，位置、 速度和加速度的估計誤差隨采樣周期 T 的增大而 增大 。因此，根據(jù)跟蹤系統(tǒng)的任務(wù)， 最后是盡可能小地選擇采樣周期 T。加速度方差對跟蹤精度的影響狀態(tài)噪聲 w 的協(xié)方差是與目標(biāo)加速度方差a2 相聯(lián)系的，因此，a2 將直接影響估計誤差的協(xié)方差矩陣。 如果采用加速度方差自適應(yīng)算法， 或者考慮非零加速度均值，即應(yīng)用如下方程：7x(t )010x(t)00x(t )001x(t)0a0w(t )x (t )00ax(t)

18、a1那么 2 將取決于最大機(jī)動加速度amax。圖、圖2.6是在不同的加速度情況下，a2.5速度和加速度估計的均值誤差隨加速度方差a2 的變化曲線；圖 2.7、圖 2.8 是速度和加速度估計的均方誤差隨a2 的變化曲線。圖 2.5圖 2.6圖 2.7圖 2.8圖 2.7 和圖 2.8 表明，當(dāng)目標(biāo)機(jī)動加速度減小， 速度和加速度的均方誤差隨a2 的增大而單調(diào)增加； 對于高速機(jī)動加速度來說， 結(jié)果與較小機(jī)動加速度情況相反。 就速度和加速度估計的均值誤差而言， 除機(jī)動加速度接近于零外， 均值誤差隨加速度方差a2 的增高而減小。8參考書周宏仁etc. ?機(jī)動目標(biāo)跟蹤 ?，國防工業(yè)出版社，1994作業(yè)對于單機(jī)動目標(biāo)跟蹤情形，請采用下述目標(biāo)運動模型之一進(jìn)行2 自適應(yīng)卡爾曼濾波跟蹤。假定目