第七講目標(biāo)跟蹤算法【第一版】

1、多源信息融合處理技術(shù)多源信息融合處理技術(shù)主講人：李玉柏主講人：李玉柏第七講：目標(biāo)動態(tài)模型與目標(biāo)跟蹤第七講：目標(biāo)動態(tài)模型與目標(biāo)跟蹤n目標(biāo)跟蹤基本概念目標(biāo)跟蹤基本概念n坐標(biāo)系與跟蹤門模型坐標(biāo)系與跟蹤門模型n目標(biāo)的動態(tài)模型目標(biāo)的動態(tài)模型n基本的目標(biāo)跟蹤算法基本的目標(biāo)跟蹤算法n量測模型的線性化處理量測模型的線性化處理n量測坐標(biāo)轉(zhuǎn)換量測坐標(biāo)轉(zhuǎn)換n基于基于BLUE的的Kalman濾波算法濾波算法1、目標(biāo)跟蹤基本概念、目標(biāo)跟蹤基本概念n目標(biāo)跟蹤是指為了維持對目標(biāo)當(dāng)前狀態(tài)的不斷估目標(biāo)跟蹤是指為了維持對目標(biāo)當(dāng)前狀態(tài)的不斷估計，同時，也是對傳感器接不斷收到的量測進(jìn)行計，同時，也是對傳感器接不斷收到的量測進(jìn)行處理的

2、過程。處理的過程。n目標(biāo)狀態(tài)包括：目標(biāo)狀態(tài)包括： 運動學(xué)分量，如目標(biāo)的位置、速度。運動學(xué)分量，如目標(biāo)的位置、速度。 其他特性分量：有輻射的信號強度，譜特性，其他特性分量：有輻射的信號強度，譜特性，“屬性屬性”信息等。信息等。 常數(shù)或其他緩變參數(shù)：精合系數(shù)，傳播速度常數(shù)或其他緩變參數(shù)：精合系數(shù)，傳播速度等。等。 1）目標(biāo)跟蹤基本概念）目標(biāo)跟蹤基本概念n目標(biāo)跟蹤是一個典型的不確定性問題，并隨著目標(biāo)跟蹤是一個典型的不確定性問題，并隨著監(jiān)視和反監(jiān)視技術(shù)發(fā)展和目標(biāo)機動性提高，使監(jiān)視和反監(jiān)視技術(shù)發(fā)展和目標(biāo)機動性提高，使得目標(biāo)跟蹤問題的不確定性更加嚴(yán)重。跟蹤問得目標(biāo)跟蹤問題的不確定性更加嚴(yán)重。跟蹤問題的不確

3、定性主要來源：題的不確定性主要來源： 目標(biāo)運動狀態(tài)的不確定性目標(biāo)運動狀態(tài)的不確定性-過程噪聲過程噪聲 量測量測(信息信息)源的不確定性源的不確定性-觀測噪聲觀測噪聲 多目標(biāo)和密集雜回波環(huán)境造成量測數(shù)據(jù)模糊多目標(biāo)和密集雜回波環(huán)境造成量測數(shù)據(jù)模糊-虛假噪聲。虛假噪聲。n目標(biāo)跟蹤本質(zhì)是通過濾波，對目標(biāo)運動狀態(tài)進(jìn)目標(biāo)跟蹤本質(zhì)是通過濾波，對目標(biāo)運動狀態(tài)進(jìn)行估計和預(yù)測，來消除目標(biāo)相關(guān)的不確定性。行估計和預(yù)測，來消除目標(biāo)相關(guān)的不確定性。 目標(biāo)跟蹤基本概念目標(biāo)跟蹤基本概念n目標(biāo)跟蹤處理過程所關(guān)注的量測通常不是原始目標(biāo)跟蹤處理過程所關(guān)注的量測通常不是原始的觀測數(shù)據(jù)，而是信號處理子系統(tǒng)或者檢測子的觀測數(shù)據(jù)，而是信

4、號處理子系統(tǒng)或者檢測子系統(tǒng)的輸出信號。包括：系統(tǒng)的輸出信號。包括： 直接的位置估計、斜距、方位角信息直接的位置估計、斜距、方位角信息 多傳感器的抵達(dá)時間差多傳感器的抵達(dá)時間差 由于由于Doppler頻移導(dǎo)致的多傳感器間的觀測頻移導(dǎo)致的多傳感器間的觀測頻差等。頻差等。n 航跡航跡 (Track) 是目標(biāo)跟蹤領(lǐng)域經(jīng)常提到的概念，是目標(biāo)跟蹤領(lǐng)域經(jīng)常提到的概念，它是指基于源于同一目標(biāo)的一組量測信息獲得它是指基于源于同一目標(biāo)的一組量測信息獲得的目標(biāo)狀態(tài)軌跡的估值，本質(zhì)上就是目標(biāo)跟蹤的目標(biāo)狀態(tài)軌跡的估值，本質(zhì)上就是目標(biāo)跟蹤濾波結(jié)果。濾波結(jié)果。2）單目標(biāo)跟蹤基本原理框圖）單目標(biāo)跟蹤基本原理框圖n單機動目標(biāo)跟

5、蹤是目標(biāo)跟蹤的基礎(chǔ)。其基本要素單機動目標(biāo)跟蹤是目標(biāo)跟蹤的基礎(chǔ)。其基本要素包括包括量測數(shù)據(jù)形成量測數(shù)據(jù)形成，目標(biāo)機動模型目標(biāo)機動模型，自，自適應(yīng)濾波適應(yīng)濾波與預(yù)測與預(yù)測以及跟蹤坐標(biāo)系和濾波狀態(tài)的選取。以及跟蹤坐標(biāo)系和濾波狀態(tài)的選取。n單機動目標(biāo)跟蹤本質(zhì)上就是一個遞推濾波過程。單機動目標(biāo)跟蹤本質(zhì)上就是一個遞推濾波過程。首先，由傳感器獲得目標(biāo)的量測數(shù)據(jù)；通過量測首先，由傳感器獲得目標(biāo)的量測數(shù)據(jù)；通過量測模型獲得量測數(shù)據(jù)與目標(biāo)位置的關(guān)系函數(shù)，將此模型獲得量測數(shù)據(jù)與目標(biāo)位置的關(guān)系函數(shù)，將此與目標(biāo)的當(dāng)前狀態(tài)一起作為輸入；由跟蹤濾波器與目標(biāo)的當(dāng)前狀態(tài)一起作為輸入；由跟蹤濾波器結(jié)合機動目標(biāo)模型得到當(dāng)前目標(biāo)狀態(tài)

6、的估計值和結(jié)合機動目標(biāo)模型得到當(dāng)前目標(biāo)狀態(tài)的估計值和預(yù)測值，并把得到的狀態(tài)作為下一時刻的初始狀預(yù)測值，并把得到的狀態(tài)作為下一時刻的初始狀態(tài)，從而完成單機動目標(biāo)跟蹤過程。態(tài)，從而完成單機動目標(biāo)跟蹤過程。單目標(biāo)跟蹤基本原理框圖單目標(biāo)跟蹤基本原理框圖111k kk kk kk kxxGz 目標(biāo)跟蹤基本原理框圖目標(biāo)跟蹤基本原理框圖n圖中假定目標(biāo)動態(tài)特性用包含位置、速度和加速圖中假定目標(biāo)動態(tài)特性用包含位置、速度和加速度的狀態(tài)向量度的狀態(tài)向量X 表示，量測量用表示，量測量用Z表示，新息向量表示，新息向量用用 表示。表示。1k kz1k kz1k kz 首先首先先由量測量先由量測量Z 和狀態(tài)預(yù)測量和狀態(tài)預(yù)測

7、量計算殘差（新計算殘差（新息）向量息）向量 ; 然后根據(jù)然后根據(jù) 的變化進(jìn)行機動檢測或機動辨識的變化進(jìn)行機動檢測或機動辨識; 其次按照某一準(zhǔn)則或邏輯調(diào)整濾波增益與協(xié)方其次按照某一準(zhǔn)則或邏輯調(diào)整濾波增益與協(xié)方差矩陣或者實時辨識出目標(biāo)機動特性；差矩陣或者實時辨識出目標(biāo)機動特性； 最后由濾波算法得到目標(biāo)的估計值和預(yù)測值，最后由濾波算法得到目標(biāo)的估計值和預(yù)測值，從而完成目標(biāo)跟蹤功能。從而完成目標(biāo)跟蹤功能。3）多目標(biāo)跟蹤基本原理）多目標(biāo)跟蹤基本原理n多機動目標(biāo)跟蹤是指在多量測數(shù)據(jù)的情況下，利多機動目標(biāo)跟蹤是指在多量測數(shù)據(jù)的情況下，利用跟蹤濾波和數(shù)據(jù)關(guān)聯(lián)算法對多個機動目標(biāo)進(jìn)行用跟蹤濾波和數(shù)據(jù)關(guān)聯(lián)算法對多個

8、機動目標(biāo)進(jìn)行狀態(tài)估計和跟蹤的算法。狀態(tài)估計和跟蹤的算法。n多機動目標(biāo)跟蹤不僅包括單機動目標(biāo)的基本要素，多機動目標(biāo)跟蹤不僅包括單機動目標(biāo)的基本要素，還形成一些新的要素，主要包括跟蹤門規(guī)則，數(shù)還形成一些新的要素，主要包括跟蹤門規(guī)則，數(shù)據(jù)關(guān)聯(lián)和跟蹤維持等。其中數(shù)據(jù)關(guān)聯(lián)是多機動目據(jù)關(guān)聯(lián)和跟蹤維持等。其中數(shù)據(jù)關(guān)聯(lián)是多機動目標(biāo)跟蹤的核心。標(biāo)跟蹤的核心。n多機動目標(biāo)跟蹤的基本框架如下圖所示。多機動目標(biāo)跟蹤的基本框架如下圖所示。多目標(biāo)跟蹤基本原理多目標(biāo)跟蹤基本原理2、坐標(biāo)系與跟蹤門、坐標(biāo)系與跟蹤門n任何目標(biāo)的運動描述和跟蹤問題都是相對于某一任何目標(biāo)的運動描述和跟蹤問題都是相對于某一特定的坐標(biāo)系而言的，是幾種常

9、見的有下面幾種特定的坐標(biāo)系而言的，是幾種常見的有下面幾種坐標(biāo)系：坐標(biāo)系： 慣性坐標(biāo)系：慣性坐標(biāo)系：原點選在地球球心，定義原點選在地球球心，定義X，Y， Z三坐標(biāo)軸互相垂直并且各自指向某相應(yīng)的恒三坐標(biāo)軸互相垂直并且各自指向某相應(yīng)的恒天體，例如令天體，例如令Z指向北極星。指向北極星。 地理坐標(biāo)系：地理坐標(biāo)系：也叫也叫NED坐標(biāo)系，原點設(shè)在載機坐標(biāo)系，原點設(shè)在載機質(zhì)心上，質(zhì)心上，N指向北，指向北，E指向東，指向東，D方向垂直地平方向垂直地平面并指向地心。除了在北極附近外，地理坐標(biāo)面并指向地心。除了在北極附近外，地理坐標(biāo)系可近似看作一個慣性坐標(biāo)系，因為載機的移系可近似看作一個慣性坐標(biāo)系，因為載機的移動

10、造成各軸方向的變化很小，可以忽略不計。動造成各軸方向的變化很小，可以忽略不計。1）各種坐標(biāo)系）各種坐標(biāo)系 載機坐標(biāo)系：載機坐標(biāo)系：也叫機體坐標(biāo)系，或觀測坐標(biāo)系。也叫機體坐標(biāo)系，或觀測坐標(biāo)系。原點設(shè)在載機質(zhì)心上，原點設(shè)在載機質(zhì)心上， X軸為載機縱軸機頭方軸為載機縱軸機頭方向，向，Y 軸為右機翼正向，軸為右機翼正向， Z 軸由右手螺旋定則軸由右手螺旋定則確定，并朝下。確定，并朝下。 方向余弦坐標(biāo)系：方向余弦坐標(biāo)系：是由于相控陣?yán)走_(dá)的應(yīng)用而是由于相控陣?yán)走_(dá)的應(yīng)用而引入的。相控陣?yán)走_(dá)一般采用方向余弦坐標(biāo)系引入的。相控陣?yán)走_(dá)一般采用方向余弦坐標(biāo)系給出觀測值給出觀測值 z =R，cos，cosT，其中，其

11、中R 是原是原點到目標(biāo)的徑向距離，點到目標(biāo)的徑向距離，/ 分別是目標(biāo)徑向與分別是目標(biāo)徑向與X ，Y 軸的夾角。軸的夾角。n上述前三種坐標(biāo)系屬于直角坐標(biāo)系，方向余弦坐上述前三種坐標(biāo)系屬于直角坐標(biāo)系，方向余弦坐標(biāo)系屬于球面坐標(biāo)系。標(biāo)系屬于球面坐標(biāo)系。 各種坐標(biāo)系各種坐標(biāo)系n通常探測器的量測信息是在球面坐標(biāo)系中進(jìn)行的，通常探測器的量測信息是在球面坐標(biāo)系中進(jìn)行的，但是運動目標(biāo)的狀態(tài)方程在直角坐標(biāo)系中可以線但是運動目標(biāo)的狀態(tài)方程在直角坐標(biāo)系中可以線性地表示。性地表示。n如果在一種坐標(biāo)系中建立目標(biāo)的狀態(tài)方程，要么如果在一種坐標(biāo)系中建立目標(biāo)的狀態(tài)方程，要么狀態(tài)方程線性，觀測（量測）方程非線性；要么狀態(tài)方程線

12、性，觀測（量測）方程非線性；要么狀態(tài)方程非線性，觀測方程線性。狀態(tài)方程非線性，觀測方程線性。 前面我們介紹了量測信息的坐標(biāo)系轉(zhuǎn)換。前面我們介紹了量測信息的坐標(biāo)系轉(zhuǎn)換。 2）跟蹤門）跟蹤門n跟蹤門是整個跟蹤空域中的一塊子區(qū)域，它將傳跟蹤門是整個跟蹤空域中的一塊子區(qū)域，它將傳感器接收到的量測信號劃分為可能源于目標(biāo)和不感器接收到的量測信號劃分為可能源于目標(biāo)和不可能源于目標(biāo)的兩個部分。其可能源于目標(biāo)的兩個部分。其中心位于被跟蹤目中心位于被跟蹤目標(biāo)的預(yù)測位置標(biāo)的預(yù)測位置，大小由接收正確量測信號的概率大小由接收正確量測信號的概率來確定來確定。n跟蹤門的功能是將落入跟蹤門內(nèi)的量測信號稱為跟蹤門的功能是將落入

13、跟蹤門內(nèi)的量測信號稱為候選信號。候選信號。 在數(shù)據(jù)關(guān)聯(lián)過程中，如果只有一個量測信號落在數(shù)據(jù)關(guān)聯(lián)過程中，如果只有一個量測信號落入該目標(biāo)的跟蹤門內(nèi)，則此量測直接用于航跡入該目標(biāo)的跟蹤門內(nèi)，則此量測直接用于航跡更新；更新；跟蹤門基本概念跟蹤門基本概念 如果多于一個以上的量測信號落在被跟蹤目標(biāo)如果多于一個以上的量測信號落在被跟蹤目標(biāo)的跟蹤門內(nèi)，那么通過跟蹤門邏輯可以粗略確的跟蹤門內(nèi)，那么通過跟蹤門邏輯可以粗略確定用于航跡更新的量測信號集合。定用于航跡更新的量測信號集合。 然后通過更高級的數(shù)據(jù)關(guān)聯(lián)技術(shù)，以最終確定然后通過更高級的數(shù)據(jù)關(guān)聯(lián)技術(shù)，以最終確定用于目標(biāo)航跡更新的量測信號。用于目標(biāo)航跡更新的量測信

14、號。1kkx1kkzn定義：定義：濾波殘差，是考慮一個處于跟蹤維持階段濾波殘差，是考慮一個處于跟蹤維持階段的目標(biāo)（已經(jīng)初始化），設(shè)的目標(biāo)（已經(jīng)初始化），設(shè)k-1時刻狀態(tài)變量的時刻狀態(tài)變量的濾濾 波預(yù)報值為波預(yù)報值為 ，通過觀測方程可以求出，通過觀測方程可以求出k時時刻量測的預(yù)報值刻量測的預(yù)報值 ，它與，它與k時刻量測信號之差時刻量測信號之差為濾波殘差向量。為濾波殘差向量。跟蹤門基本概念跟蹤門基本概念11kkkkkzzz濾波殘差：n注意：注意：如果我們把目標(biāo)跟蹤看成動態(tài)參數(shù)估計，如果我們把目標(biāo)跟蹤看成動態(tài)參數(shù)估計，濾波殘差就是前面講的濾波殘差就是前面講的“新息新息(innovation) ”。1

15、1kkkkkxzH H其中：kkkkvxzH H已知：n定義：定義：殘差協(xié)方差陣殘差協(xié)方差陣SkTkkkkkH HP PH HS S1定義：1112kkkTkkkzzdS Sn定義：定義：殘差向量范數(shù)殘差向量范數(shù)2kd典型跟蹤門典型跟蹤門,1,1,11,k ikk kik kik kk kzzzzzzi的第 個分量n 矩形跟蹤門：矩形跟蹤門： 最簡單的跟蹤門形成方法是在跟蹤空間內(nèi)定義最簡單的跟蹤門形成方法是在跟蹤空間內(nèi)定義一個矩形區(qū)域，即矩形跟蹤門。一個矩形區(qū)域，即矩形跟蹤門。 定義各種向量的分量：定義各種向量的分量： 定義定義跟蹤門常數(shù)為跟蹤門常數(shù)為KG。它取決于觀測。它取決于觀測概率概率密

16、度，密度，檢測概率以及狀態(tài)矢量的維數(shù)。檢測概率以及狀態(tài)矢量的維數(shù)。 如果觀測量如果觀測量zk滿足滿足： 則稱則稱zk為候選為候選量測信號量測信號。這里。這里 i為第為第i個殘差的個殘差的標(biāo)準(zhǔn)偏差。標(biāo)準(zhǔn)偏差。,1,1.defk iGik kik kizzzK典型跟蹤門典型跟蹤門n橢球跟蹤門橢球跟蹤門： 設(shè)設(shè) 為橢球跟蹤門的門限大小，為橢球跟蹤門的門限大小，如如量測信號量測信號zk滿足滿足： 則稱則稱zk為候選為候選量測信號量測信號。2kdn其他跟蹤門其他跟蹤門：除了前面提到的兩種常見的跟蹤門除了前面提到的兩種常見的跟蹤門外，還有球面坐標(biāo)系下的扇形跟蹤門，以及基于外，還有球面坐標(biāo)系下的扇形跟蹤門，

17、以及基于數(shù)據(jù)關(guān)聯(lián)性能評價的優(yōu)化跟蹤門算法等。數(shù)據(jù)關(guān)聯(lián)性能評價的優(yōu)化跟蹤門算法等。 在實際的多目標(biāo)跟蹤問題中，跟蹤門的使用非在實際的多目標(biāo)跟蹤問題中，跟蹤門的使用非常廣泛。當(dāng)目標(biāo)無機動時，常廣泛。當(dāng)目標(biāo)無機動時，跟蹤門的大小一般跟蹤門的大小一般為常值；當(dāng)目標(biāo)機動時，調(diào)整門的大小以保證為常值；當(dāng)目標(biāo)機動時，調(diào)整門的大小以保證一定的接收正確回波的概率一定的接收正確回波的概率就成了關(guān)鍵問題。就成了關(guān)鍵問題。3、目標(biāo)動態(tài)模型、目標(biāo)動態(tài)模型n大多數(shù)機動目標(biāo)跟蹤問題都是基于模型的。也就大多數(shù)機動目標(biāo)跟蹤問題都是基于模型的。也就是說，依賴于兩個描述：一是目標(biāo)行為，通常用是說，依賴于兩個描述：一是目標(biāo)行為，通常

18、用動態(tài)運動模型表示；另一個是對目標(biāo)的觀測，稱動態(tài)運動模型表示；另一個是對目標(biāo)的觀測，稱為觀測模型。為觀測模型。n目標(biāo)跟蹤的數(shù)學(xué)模型目標(biāo)跟蹤的數(shù)學(xué)模型 目標(biāo)跟蹤的主要目的就是估計移動目標(biāo)的狀態(tài)軌目標(biāo)跟蹤的主要目的就是估計移動目標(biāo)的狀態(tài)軌跡。雖然目標(biāo)在空間上幾乎從來不是一個真正的跡。雖然目標(biāo)在空間上幾乎從來不是一個真正的點，且其方向信息對于跟蹤也是有用的，但通常點，且其方向信息對于跟蹤也是有用的，但通常還是把目標(biāo)看作空間沒有形狀的一個點，特別對還是把目標(biāo)看作空間沒有形狀的一個點，特別對于目標(biāo)建模更是如此。目標(biāo)動態(tài)模型描述了目標(biāo)于目標(biāo)建模更是如此。目標(biāo)動態(tài)模型描述了目標(biāo)狀態(tài)又隨時間的演化過程。狀態(tài)又

19、隨時間的演化過程。1）目標(biāo)跟蹤的數(shù)學(xué)模型）目標(biāo)跟蹤的數(shù)學(xué)模型n幾乎所有的運動目標(biāo)跟蹤方法都是基于模型的。幾乎所有的運動目標(biāo)跟蹤方法都是基于模型的。常用的狀態(tài)空間模型為：常用的狀態(tài)空間模型為：1(,)()()kkkkkkkkkkxfx uwzh xvkNn注意：注意：同前面介紹的動態(tài)系統(tǒng)方程描述多了一個同前面介紹的動態(tài)系統(tǒng)方程描述多了一個控制輸入變量控制輸入變量u(t)，用以表示目標(biāo)機動時外力作，用以表示目標(biāo)機動時外力作用的輸入用的輸入。n離散時間模型離散時間模型( ) ( ), ( )( )( )( )( )kkkkkx tf x t u tw tz th x tv t目標(biāo)跟蹤的數(shù)學(xué)模型目標(biāo)跟

20、蹤的數(shù)學(xué)模型n對于目標(biāo)跟蹤挑戰(zhàn)之一是目標(biāo)運動模式的不確定對于目標(biāo)跟蹤挑戰(zhàn)之一是目標(biāo)運動模式的不確定性，即被跟蹤目標(biāo)的精確動態(tài)模型是不知道的。性，即被跟蹤目標(biāo)的精確動態(tài)模型是不知道的。跟蹤者不知道目標(biāo)實際的控制輸入跟蹤者不知道目標(biāo)實際的控制輸入u，也不知道，也不知道動態(tài)模型的具體形式和相關(guān)參數(shù)。動態(tài)模型的具體形式和相關(guān)參數(shù)。n為此可以針對機動和非機動兩種模型進(jìn)行研究。為此可以針對機動和非機動兩種模型進(jìn)行研究。非機動運動是指在慣性參考坐標(biāo)系中，目標(biāo)按某非機動運動是指在慣性參考坐標(biāo)系中，目標(biāo)按某個定常的速度作直線和水平運動。個定常的速度作直線和水平運動。廣義地講，廣義地講，所所有不是機動運動的模式都

21、叫非機動。有不是機動運動的模式都叫非機動。1kkkkkkkkkkkxF xE uG wzH xvkNn通常我們處理時線性系統(tǒng)，此時離散時間模型通常我們處理時線性系統(tǒng)，此時離散時間模型目標(biāo)跟蹤的數(shù)學(xué)模型目標(biāo)跟蹤的數(shù)學(xué)模型 白噪聲模型，假設(shè)控制輸入白噪聲模型，假設(shè)控制輸入u 為白噪聲，包括為白噪聲，包括常速常速CV模型，常加速模型，常加速CA模型和多項式模型等；模型和多項式模型等； Morkov過程模型，假設(shè)控制輸入過程模型，假設(shè)控制輸入u 為為Morkov過程，包括過程，包括Singer模型及其變形；模型及其變形； 半半Morkov過程模型，假設(shè)控制輸入過程模型，假設(shè)控制輸入u 為半為半Mork

22、ov過程。過程。n非機動目標(biāo)跟蹤中，目標(biāo)的控制輸入非機動目標(biāo)跟蹤中，目標(biāo)的控制輸入u等于零。等于零。而在有機動目標(biāo)跟蹤中，對目標(biāo)的控制輸入而在有機動目標(biāo)跟蹤中，對目標(biāo)的控制輸入u 通通?？梢约僭O(shè)為未知加速度，它決定了機動的數(shù)學(xué)常可以假設(shè)為未知加速度，它決定了機動的數(shù)學(xué)模型，具體應(yīng)用中可以分為：模型，具體應(yīng)用中可以分為：2）非機動模型非機動模型 常速常速CV模式模式n在三維物理空間的點目標(biāo)運動，可以用三維的位在三維物理空間的點目標(biāo)運動，可以用三維的位移和速度向量來描述：移和速度向量來描述：n非機動目標(biāo)的動態(tài)模型一般可描述為：非機動目標(biāo)的動態(tài)模型一般可描述為：( , , )( , , )( , ,

23、 , , , )Tx y zx y zx x y y z z x x位位移移向向量量：速速度度向向量量：目目標(biāo)標(biāo)狀狀態(tài)態(tài)：10,0010)(,)(,)(CVCVCVCVCVCVCVCVAtdiagtAAAdiagtw wx xx x非機動模型非機動模型 常速常速CV模式模式n離散化模型（離散化模型（T采樣間隔）采樣間隔）TTGTFGGGdiagFFFdiagCVCVkCVCVCVkCVCVCVk2211,101,w wx xx xykkkykkkkwTyyywTyTyyy,1,2211方向速度：方向位移：具體給出一個狀態(tài)分量（具體給出一個狀態(tài)分量（y方向）的具體表達(dá)式：方向）的具體表達(dá)式：非機

24、動模型非機動模型 常速常速CV模式模式n噪聲協(xié)方差為：噪聲協(xié)方差為：32, ,2cov(),/3/2/2xyzkwwwwwwx y zwwGdiag G Q G QG QTTQGTT各w wk k其其中中代代表表w w分分量量方方差差, ,n特別說明特別說明 在一般飛行器的常速模型分析中，主要研究水平在一般飛行器的常速模型分析中，主要研究水平機動，有時允許機動，有時允許z 方向速度有機動，此時方程：方向速度有機動，此時方程：代表速度擾動zkzkkkwwTzz,1請同學(xué)寫出這種情況下完整的常數(shù)模型請同學(xué)寫出這種情況下完整的常數(shù)模型方程！方程！3）目標(biāo)機動模型）目標(biāo)機動模型 白噪聲加速度模式白噪聲

25、加速度模式n最簡單的目標(biāo)機動模型最簡單的目標(biāo)機動模型 白噪聲加速度模式白噪聲加速度模式具體離散表達(dá)式為：具體離散表達(dá)式為：zyxwzwywx ,加速度：TTETFEEEdiagFFFdiagkkk2211,101,w wx xx x 本質(zhì)上與本質(zhì)上與CV模型類似。只不過體現(xiàn)加速度的隨模型類似。只不過體現(xiàn)加速度的隨機擾動是輸入控制量產(chǎn)生的，因此隨機擾動方機擾動是輸入控制量產(chǎn)生的，因此隨機擾動方差較大，差較大，CV模型的加速度隨機擾動方差很小。模型的加速度隨機擾動方差很小。4）Wiener過程加速度機動模型過程加速度機動模型n假設(shè)加速度是一個假設(shè)加速度是一個Wiener過程，即獨立增量隨機過程，即

26、獨立增量隨機過程，簡稱常加速過程，簡稱常加速CA模型。令狀態(tài)變量：模型。令狀態(tài)變量： , , , , , , , , ( ) , , ( )0100001 ,00001Tx x x y y y z z ztdiag F F Fdiag E E EtFE x xxxwxxwcov( )( )wQttT Tw ww wWiener過程加速度機動模型過程加速度機動模型n具體離散表達(dá)式為：具體離散表達(dá)式為：1,( )kkkkkdiag FFFdiag EEEtkkkkkkkkxxwxxw023()20( )()1/2/601,/2001TTATA TkkkkTdTTTFeTEeEdTTk kwwww其

27、其中中： 54343232cov(,)/20/8/6/8/3/2/6/2wwwwwdiagQTTTTTTTTT 其其中中5）一般多項式模型）一般多項式模型n眾所周知，任何連續(xù)目標(biāo)軌跡都可以用某個階眾所周知，任何連續(xù)目標(biāo)軌跡都可以用某個階次的多項式以任意精度逼近。這樣，可以把目次的多項式以任意精度逼近。這樣，可以把目標(biāo)運動建成笛卡兒坐標(biāo)系中的標(biāo)運動建成笛卡兒坐標(biāo)系中的 n 階多項式模型：階多項式模型：1111( )( )( )( )( )( )( )/ !onxonyonznx taaaw ttty tbbbw tz tcccw ttnx說明：說明：這個模型作為跟蹤模型很少采用，其原這個模型作為

28、跟蹤模型很少采用，其原因是這個模型需要一組數(shù)據(jù)進(jìn)行擬合，即進(jìn)行因是這個模型需要一組數(shù)據(jù)進(jìn)行擬合，即進(jìn)行平滑，而跟蹤的目的是濾波和預(yù)測，并不是擬平滑，而跟蹤的目的是濾波和預(yù)測，并不是擬合與平滑。合與平滑。6）Singer加速度模型加速度模型 零均值一階零均值一階Markov模型模型n在隨機建模中，一個未知的時變量則可用隨機過在隨機建模中，一個未知的時變量則可用隨機過程來描述，白噪聲構(gòu)成最簡單的一類隨機過程，程來描述，白噪聲構(gòu)成最簡單的一類隨機過程，對于連續(xù)變量就是獨立增量過程或?qū)τ谶B續(xù)變量就是獨立增量過程或Wiener過程。過程。( )a tnSinger模型假定目標(biāo)加速度模型假定目標(biāo)加速度 是

29、一個零均值的平是一個零均值的平穩(wěn)一階穩(wěn)一階Markov過程，可以用線性時不變系統(tǒng)的狀過程，可以用線性時不變系統(tǒng)的狀態(tài)來描述：態(tài)來描述：2( )( )( )( ) (0,2)a ta tw tw tN 2( ) () ( )aRE a ta te 222( )2/()aS 具有功率譜密度：具有功率譜密度： 具有自相關(guān)量：具有自相關(guān)量：Singer機動模型的表達(dá)式機動模型的表達(dá)式nSinger加速度模型的加速度離散方程：加速度模型的加速度離散方程：221,(0,(1)TkkkkaawewNnSinger模型的機動模型表達(dá)式，令狀態(tài)變量：模型的機動模型表達(dá)式，令狀態(tài)變量： , , ( )( )( )

30、0100001,0001Tx x xtFtEtFE x xxxwxxwSinger加速度模型的離散方程加速度模型的離散方程nSinger模型的成功依賴于精確獲得參數(shù)模型的成功依賴于精確獲得參數(shù) 和和 。參參數(shù)數(shù) 是機動時間常數(shù)的倒數(shù)，依賴于機動時間持是機動時間常數(shù)的倒數(shù)，依賴于機動時間持續(xù)長短。比如對于飛機，當(dāng)懶散回轉(zhuǎn)時，機動時續(xù)長短。比如對于飛機，當(dāng)懶散回轉(zhuǎn)時，機動時間常數(shù)約為間常數(shù)約為60s；而當(dāng)逃逸機動時，機動時間常數(shù)；而當(dāng)逃逸機動時，機動時間常數(shù)為為10-20s，空氣擾動機動時間常數(shù)為，空氣擾動機動時間常數(shù)為1-2s。2211(1)/01(1)/00TTkkkkkTTTeFee wwx

31、xxxxxn等價離散時間模型：等價離散時間模型：加速度積分得到加速度積分得到Singer加速度模型的離散方程加速度模型的離散方程nSinger加速度模型討論：加速度模型討論：1）當(dāng)機動時間常數(shù)增大時，）當(dāng)機動時間常數(shù)增大時，Singer模型就還原成近模型就還原成近似勻加速似勻加速(CA)模型，更精確地說是白噪聲加加速模型，更精確地說是白噪聲加加速度度+Jerk模型。如果基于模型。如果基于Singer加速度模型直接建加速度模型直接建立離散時間狀態(tài)空間模型，在極限情況下就是立離散時間狀態(tài)空間模型，在極限情況下就是Wiener序列加速度模型。序列加速度模型。211/201001kkkTTTGwxxx

32、xSinger加速度模型的離散方程加速度模型的離散方程nSinger加速度模型討論：加速度模型討論：2）另外，當(dāng)機動時間常數(shù)減小時，）另外，當(dāng)機動時間常數(shù)減小時，Singer模型就還模型就還原成近似勻速原成近似勻速(CV)模型。在此情況下，加速度變模型。在此情況下，加速度變成噪聲。成噪聲。21(1)/1001(1)/01000001TTTTTeTFee n選擇機動時間常數(shù)，選擇機動時間常數(shù)，Singer模型相應(yīng)于在常速模型相應(yīng)于在常速模型和常加速模型之間折中。所以模型和常加速模型之間折中。所以Singer模型模型較之較之CV模型和模型和CA模型具有更寬的覆蓋面。模型具有更寬的覆蓋面。Singe

33、r加速度模型的離散方程加速度模型的離散方程3）Singer運動模型第一次將位置目標(biāo)的加速度描述運動模型第一次將位置目標(biāo)的加速度描述為一個時間相關(guān)的隨機過程，成為進(jìn)一步建立其為一個時間相關(guān)的隨機過程，成為進(jìn)一步建立其他模型的基礎(chǔ)，可以說他模型的基礎(chǔ)，可以說Singer模型是一個目標(biāo)機模型是一個目標(biāo)機動的標(biāo)準(zhǔn)模型。動的標(biāo)準(zhǔn)模型。7）“當(dāng)前當(dāng)前”模型模型 均值自適應(yīng)加速度模型均值自適應(yīng)加速度模型n“當(dāng)前當(dāng)前”模型：模型： 也是一種加速度模型，其本質(zhì)上也是一種加速度模型，其本質(zhì)上就是一個帶自適應(yīng)的就是一個帶自適應(yīng)的Singer模型，即模型，即Singer模型被模型被修正而具有非零均值：修正而具有非零均

34、值：2( )( )( )( )( ) (0,2)a ta ta tw tw tN 1,(1),(0,)kkkkTkkwaaawewN( )( )( )( ), , , 01000001,0 ,0001TtFtU x tEtx x xFUE xxwxxxwx“當(dāng)前當(dāng)前”模型模型 均值自適應(yīng)加速度模型均值自適應(yīng)加速度模型n“當(dāng)前當(dāng)前”模型的離散化。模型的離散化。21(1)/01(1)/00TTTTTeFee 221/2(1)/(1)/1TTkkkkTTTeFTexe wxxxx8）半馬爾可夫模型）半馬爾可夫模型nSinger模型為零均值模型，這種機動加速度的零模型為零均值模型，這種機動加速度的零均

35、值特性對于模擬機動目標(biāo)來說似乎不太合理。均值特性對于模擬機動目標(biāo)來說似乎不太合理。為此，為此，Moose等提出了具有隨機開關(guān)均值的相關(guān)等提出了具有隨機開關(guān)均值的相關(guān)高斯噪聲模型。該模型把機動看作是相應(yīng)于半馬高斯噪聲模型。該模型把機動看作是相應(yīng)于半馬爾科夫過程描述的一系列有限指令，該指令由馬爾科夫過程描述的一系列有限指令，該指令由馬爾可夫過程的轉(zhuǎn)移概率來確定，轉(zhuǎn)移時間為隨機爾可夫過程的轉(zhuǎn)移概率來確定，轉(zhuǎn)移時間為隨機變量。變量。n半馬爾科夫模型為半馬爾科夫模型為:4、目標(biāo)跟蹤的典型算法應(yīng)用、目標(biāo)跟蹤的典型算法應(yīng)用舉例舉例1、雷達(dá)目標(biāo)跟蹤基本算法以及、雷達(dá)目標(biāo)跟蹤基本算法以及kalman濾波應(yīng)濾波應(yīng)

36、用的具體實現(xiàn)。假設(shè)系統(tǒng)狀態(tài)為四維矢量（距離、用的具體實現(xiàn)。假設(shè)系統(tǒng)狀態(tài)為四維矢量（距離、速度、方位角及變化率），假設(shè)機動模型為速度、方位角及變化率），假設(shè)機動模型為CV模模型，有：型，有： ( ), ( ), ( ), ( )( )( )( )010000000010( ),000100000001Trr t r ttttFtEtwtFEwx xxxwxxww w目標(biāo)跟蹤的典型算法應(yīng)用目標(biāo)跟蹤的典型算法應(yīng)用 雷達(dá)目標(biāo)跟蹤的觀測變量和觀測方程：雷達(dá)目標(biāo)跟蹤的觀測變量和觀測方程：( )( )( ), ( ), ( )1000,( )0010TrtHttr ttvHtvzxvzzxvzv v解：雷達(dá)

37、目標(biāo)跟蹤的狀態(tài)方程和觀測方程：解：雷達(dá)目標(biāo)跟蹤的狀態(tài)方程和觀測方程：1kkkkkkkkFwHvxxxxx xz ( ), ( ), ( ), ( )Tr k r kkkk kx x目標(biāo)跟蹤的典型算法應(yīng)用目標(biāo)跟蹤的典型算法應(yīng)用n狀態(tài)方程和觀測方程的系數(shù)表達(dá)式：狀態(tài)方程和觀測方程的系數(shù)表達(dá)式：()()03222222cov()/3/2(,),/2(,)TTTF TF TkkkwrkrQw weE QeEdTTdiagTTRdiag10001001000,00100100001FTkkTFeHHT目標(biāo)跟蹤的典型算法應(yīng)用目標(biāo)跟蹤的典型算法應(yīng)用n假設(shè)已經(jīng)有觀察量假設(shè)已經(jīng)有觀察量z(1), z(2)。計算

38、卡爾曼濾波所。計算卡爾曼濾波所需的狀態(tài)初變量始值需的狀態(tài)初變量始值:111222(2)1 (2)(1) (2), (2), (2), (2)(2)1(2)(1)TzzzTrrzzzTx x2 2n狀態(tài)變量初始均方誤差矩陣狀態(tài)變量初始均方誤差矩陣:2222(2)(-)(-) TPExxxxxxxx目標(biāo)跟蹤的典型算法應(yīng)用目標(biāo)跟蹤的典型算法應(yīng)用n狀態(tài)變量初始均方誤差矩陣狀態(tài)變量初始均方誤差矩陣:(2) (2)(2)(2)(2) (1)(1)(2)(1)(2) (2)(2)(2)(2) (1)(1)(2)(1)rrrrrrvrvrvrrwTTvvvwTTxxxx22222,12,222222,12,2

39、22222222(2)(,)/,/2/2/rrrrrPdiag PPTTPPTTTT目標(biāo)跟蹤的典型算法應(yīng)用目標(biāo)跟蹤的典型算法應(yīng)用n進(jìn)行卡爾曼迭代濾波！進(jìn)行卡爾曼迭代濾波！計算本次輸出：計算本次輸出：為下次計算準(zhǔn)備：為下次計算準(zhǔn)備：11111111111111)()(kkkkkkkkkkkkkkkkkkkkkvkTkkkkTkkkkkkkkkzGxFzGxFxxHkzzQHPHHPFGFG11111)(1)Tkkkkkkk kk kk kwTvkkkkTwTkkkkkkk kk kPFGHPFGHQQFG HPFQ（5、不同坐標(biāo)的量測模型與、不同坐標(biāo)的量測模型與EKL濾波跟蹤濾波跟蹤1）一般傳感

40、器量測坐標(biāo)系和參數(shù)坐標(biāo)系不同，因此）一般傳感器量測坐標(biāo)系和參數(shù)坐標(biāo)系不同，因此觀測方程不是線性方程，以多邊定位為例：觀測方程不是線性方程，以多邊定位為例：222222( )()()()()()(),1,2,ijijiiijjjir tC txxyyzzxxyyzzwi jnijn比如有比如有4個個 同步的傳感同步的傳感 器系統(tǒng)進(jìn)行器系統(tǒng)進(jìn)行 多變定位：多變定位：222112111222222122222322222233322223343332224443( )( )()()()()()()( )( )()()()()()()( )( )()()()()()()r trtxxyyzzxxyyz

41、zwr trtxxyyzzxxyyzzwr trtxxyyzzxxyyzzwn不同坐標(biāo)的量測模型一般是非線性的，表示為：不同坐標(biāo)的量測模型一般是非線性的，表示為：不同坐標(biāo)的量測模型與不同坐標(biāo)的量測模型與EKL濾波跟蹤濾波跟蹤n雷達(dá)的觀測方程也是非線性的：雷達(dá)的觀測方程也是非線性的：22222222arctan( / )arctan( /) = ( , , )()iiiiTiiiiiirriirrwy xwwzxywr rrrwxyzwrrwxxyyzzxyzw z2( )( ,)( ),( )hNEKLEVarBLUE ，。其其中中如如果果可可以以用用跟跟蹤蹤濾濾波波；如如果果只只已已知知可可

42、以以用用跟跟蹤蹤濾濾波波zxww ww2）基于導(dǎo)數(shù)的線性化：）基于導(dǎo)數(shù)的線性化：n觀測方程線性化觀測方程線性化-1-1*-1-1-1*-1-1-1( )( )()()()(),()()k kk kkk kk kk kkk kk kk khhhhhdhhdhxxxxx - xxzxx - xv = H x+xvxHxxxxxn使用使用EKL濾波處理：濾波處理：*11111TTvkkkkkkk kk kk kk kGF GF PHH PHQ11( )()k kk kzz kh x基于導(dǎo)數(shù)的線性化：基于導(dǎo)數(shù)的線性化：n使用使用EKL濾波處理濾波處理 本次濾波本次濾波n 使用使用EKL濾波處理濾波處理

43、 為下次計算更新為下次計算更新1111111()kkkkk kk kk kk kk kk kxF xGzF xGz*1111*1)(1)Tkkkkkkk kk kk kwTvkkkkTwTkkkkkkk kk kPFGHPFGHQQFG HPFQ（3）基于差分的線性化：）基于差分的線性化：n本質(zhì)上是利用差分代替本質(zhì)上是利用差分代替Jacobi矩陣計算（偏微分矩陣計算（偏微分矩陣計算）矩陣計算）n 如何確定差分運算的如何確定差分運算的x*，最簡單的方法是：，最簡單的方法是：1-1-1-1*-1()()()kkk kk kk kkk kGhhh*k*k*kxxzxxxHxx*()( )( )( )

44、()()( )( )()( )()( )()( ),( )( )kkhhhhhhhdhhhhdh*xxxxx - xxxxxzxx - xv = H x+xvxxxxxxHxxxxxxx基于差分線性化的基于差分線性化的EKL濾波濾波n本次濾波預(yù)處理：本次濾波預(yù)處理：*11111TTvkkkkkkk kk kk kk kGF GF PHH PHQ11( )()k kk kzz kh xn使用使用EKL濾波處理濾波處理 本次濾波本次濾波n 使用使用EKL濾波處理濾波處理 為下次計算更新為下次計算更新1111111()kkkkk kk kk kk kk kk kxF xGzF xGz*1111*1)

45、(1)Tkkkkkkk kk kk kwTvkkkkTwTkkkkkkk kk kPFGHPFGHQQFG HPFQ（4）基于最優(yōu)線性化模型線性化：）基于最優(yōu)線性化模型線性化：n本質(zhì)上是利用最小本質(zhì)上是利用最小MSE準(zhǔn)則擬合線性方程：準(zhǔn)則擬合線性方程： 待優(yōu)化的目標(biāo)函數(shù)：待優(yōu)化的目標(biāo)函數(shù)：( )defhHxx)()(TxHxhxHxhEJ10)(10)(11)()()()()()()()()()()()(TTTTTxxTxExxxxTTTxExxTxxTPxxhEPxEPxEIxExhExxhEHxhExEPxEIxEPxxhExhE6、量測轉(zhuǎn)換與基于、量測轉(zhuǎn)換與基于BLUE的的Kalman濾

46、波濾波n由于大部分傳感器的觀測坐標(biāo)與目標(biāo)坐標(biāo)不同，由于大部分傳感器的觀測坐標(biāo)與目標(biāo)坐標(biāo)不同，可以對觀察信號進(jìn)行處理，得到目標(biāo)坐標(biāo)系的量可以對觀察信號進(jìn)行處理，得到目標(biāo)坐標(biāo)系的量測信號測信號 稱為量測轉(zhuǎn)換處理。稱為量測轉(zhuǎn)換處理。1）二維觀測信號的量測轉(zhuǎn)換處理：）二維觀測信號的量測轉(zhuǎn)換處理：,cos()sin()kk realkkkkkk realkkkkrrrxryr,()cos()()sin()kk realkk realkk realkkk realkk realkk realkxxxrryyyrr量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波 cos()(cos1)sinsi

47、n sin()sincoscos sin()(cos1)cossin cos()sinsincoskkkkkkkkkkkkkkkkkkkkkkkkkkkrrrrxyrrrrv,coscossin sink realk realk realkkkkk realk realk realkkkxrxryryr xkkkkxyHI,zn新的觀測方程：新的觀測方程： 其中：其中：kkkkHzxv量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波cov()covkkkkkkkkx xx ykkkky xy ykkkxRRyRR vn新的觀測方程中觀測噪聲不在是高斯白噪聲，但新的觀測方程中觀測噪聲

48、不在是高斯白噪聲，但可以求一階可以求一階/二階矩特性二階矩特性: cos()(cos) 1() sin()(cos) 1kkkkkkkkkrExEyrEE v2/2(cos),(sin)0kkkEeE當(dāng)當(dāng)為為高高斯斯白白噪噪聲聲分分布布時時：量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波n其中各項方差具體表達(dá)式其中各項方差具體表達(dá)式:2222222222222222222222cos ()(cosh() 1)sin ()sinh()cos ()cosh()sin ()sinh()sin ()(cosh() 1)cos ()sinh()sin ()cosh()ckkkkx xkkk

49、krkky ykkkkrkRr eeRr ee 2222222os ()sinh()sin()cos()(1)kkky xkkkrkRere 量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波n二維量測轉(zhuǎn)換處理后的二維量測轉(zhuǎn)換處理后的Kalman濾波問題：濾波問題：假設(shè)狀態(tài)方程：假設(shè)狀態(tài)方程： 觀測方程：觀測方程：,kkkkkHHIzxv1kkkkFxxw000cov()cov()cov()wkkvkkwQvQxxP，k kk k同同時時已已知知：w w 是是0 0均均值值獨獨立立過過程程，且且v v 不不是是0 0均均值值獨獨立立過過程程，但但已已知知系系統(tǒng)統(tǒng)初初始始狀狀態(tài)態(tài)的的統(tǒng)

50、統(tǒng)計計量量為為： 由于由于w, v不再是高斯白噪聲，不能使用基本不再是高斯白噪聲，不能使用基本Kalman濾波，同時量測噪聲濾波，同時量測噪聲v均值不為零，也均值不為零，也不能直接使用基于不能直接使用基于BLUE的的Kalman濾波。濾波。量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波2）二維觀測信號的量測轉(zhuǎn)換處理的去偏處理）二維觀測信號的量測轉(zhuǎn)換處理的去偏處理,(cos)(cos)kk realkkk realkkk realkkk realkxxxxxEEyyyyyE11,11,cos()()cos()sin()()sin()ukkkk realkk realkukkkk r

51、ealkk realkxrrryrrr 顯然如果顯然如果 ，則量測均值不是真實，則量測均值不是真實值的無偏估計，需要進(jìn)行去偏處理。值的無偏估計，需要進(jìn)行去偏處理。(cos)1kE定義量測信號定義量測信號( )：2/2(cos)kdefEe量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波無偏二維量測轉(zhuǎn)換處理的觀測噪聲：無偏二維量測轉(zhuǎn)換處理的觀測噪聲：顯然此時觀測噪聲的一階統(tǒng)計特性：顯然此時觀測噪聲的一階統(tǒng)計特性：11111111 cos()(cos1)sinsin sin()sincoscos sin()(cos1)cossin cos()sinsincoskkkkkkukkkkkk

52、kkukkkkkkkkkkkkkrrrrxyrrrrv()0ukkukxEyE v量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波cov()covkkkkkkkkx xx yukkkky xy yukkkxRRyRR vn此時觀測噪聲的二階統(tǒng)計特性：此時觀測噪聲的二階統(tǒng)計特性：12222221222222cos()cos(),1(2)cos()1(cos2 )cos(2)2sin()sin(),1(2)sin()1(cos2 )cos(22kkkkx xkkkkkkkkkkkkrky ykkkkkkkkkkkkrRErrrrrrERErrrrrrE 112222)cos()cos(

53、)sin()sin(),1(2)cossin()1(cos2 )sin(2)2kkky xkkkkkkkkkkkkkkkkkkkrkRErrrrrrrrrE 量測轉(zhuǎn)換與基于量測轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波n顯然此時可以建立下列方程：顯然此時可以建立下列方程： 狀態(tài)方程：狀態(tài)方程： 觀測方程：觀測方程：,kkkkkHHIzxv1kkkkFxxwn同時下列一階同時下列一階/二階特性可求：二階特性可求：()0,cov()()0,cov()wkkkvkkkE wwQE vvQ 此時可以利用基于此時可以利用基于BLUE的的Kalman濾波進(jìn)行跟濾波進(jìn)行跟蹤濾波處理。蹤濾波處理?；诨贐L

54、UE的的Kalman跟蹤濾波器跟蹤濾波器00000000000)cov(,Pxxxxxx第一步：第一步：初始條件：初始條件：第二步：第二步：一步提前預(yù)測值和預(yù)測誤差的協(xié)方差陣分一步提前預(yù)測值和預(yù)測誤差的協(xié)方差陣分 別是：別是：TkkwkTkkkkkkkkkkkkkkkkQFPFxxxPxFx11111111111111)cov()cov(基于基于BLUE的的Kalman跟蹤濾波器跟蹤濾波器kz第三步：第三步：獲取新的量測獲取新的量測 后，濾波更新值和相應(yīng)后，濾波更新值和相應(yīng) 的濾波誤差的協(xié)方差陣分別是：的濾波誤差的協(xié)方差陣分別是：1111111()cov()()kkk kk kk kk kk

55、kvkk kk kk kk kxxKzxPxPPPQP其中其中 是濾波誤差；是濾波誤差；而而k 時刻的時刻的Kalman增益陣為：增益陣為：kkkkkxxx111()vkkk kk kKPPQ思考題思考題( , , )Xx x x (0, 12/ )TVxm sn設(shè)一平面目標(biāo)狀態(tài)向量為設(shè)一平面目標(biāo)狀態(tài)向量為 ，初始位置，初始位置為（為（1000米，米，8000米），初始加速度為零，采樣米），初始加速度為零，采樣時間時間T為為2S，測量噪聲為，測量噪聲為100米。前米。前400S為均速運為均速運動，動， ；401S到到600S目標(biāo)向目標(biāo)向x軸方軸方向向90度慢轉(zhuǎn)彎，加速度度慢轉(zhuǎn)彎，加速度0.075m/S2； 601S到到610S目標(biāo)恢復(fù)勻速運動；目標(biāo)恢復(fù)勻速運動； 611S到到660S目標(biāo)進(jìn)行目標(biāo)進(jìn)行90度快度快轉(zhuǎn)彎，加速度轉(zhuǎn)彎，加速度-0.3m/S2； 然后目標(biāo)勻速運動至觀然后目標(biāo)勻速運動至觀測結(jié)束。測結(jié)束。n針對以上場景進(jìn)行目標(biāo)跟蹤濾波設(shè)計和仿真！針對以上場景進(jìn)行目標(biāo)跟蹤濾波設(shè)計和仿真！附件附件1：離散模型公式推導(dǎo)：離散模