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

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

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

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

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

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

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

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

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

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

10、造成各軸方向的變化很小，可以忽略不計(jì)。動(dòng)造成各軸方向的變化很小，可以忽略不計(jì)。1）各種坐標(biāo)系）各種坐標(biāo)系 載機(jī)坐標(biāo)系：載機(jī)坐標(biāo)系：也叫機(jī)體坐標(biāo)系，或觀測(cè)坐標(biāo)系。也叫機(jī)體坐標(biāo)系，或觀測(cè)坐標(biāo)系。原點(diǎn)設(shè)在載機(jī)質(zhì)心上，原點(diǎn)設(shè)在載機(jī)質(zhì)心上， X軸為載機(jī)縱軸機(jī)頭方軸為載機(jī)縱軸機(jī)頭方向，向，Y 軸為右機(jī)翼正向，軸為右機(jī)翼正向， 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)系給出觀測(cè)值給出觀測(cè)值 z =R，cos，cosT，其中，其

11、中R 是原是原點(diǎn)到目標(biāo)的徑向距離，點(diǎn)到目標(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通常探測(cè)器的量測(cè)信息是在球面坐標(biāo)系中進(jìn)行的，通常探測(cè)器的量測(cè)信息是在球面坐標(biāo)系中進(jìn)行的，但是運(yùn)動(dòng)目標(biāo)的狀態(tài)方程在直角坐標(biāo)系中可以線但是運(yùn)動(dòng)目標(biāo)的狀態(tài)方程在直角坐標(biāo)系中可以線性地表示。性地表示。n如果在一種坐標(biāo)系中建立目標(biāo)的狀態(tài)方程，要么如果在一種坐標(biāo)系中建立目標(biāo)的狀態(tài)方程，要么狀態(tài)方程線性，觀測(cè)（量測(cè)）方程非線性；要么狀態(tài)方程線

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

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

14、號(hào)。1kkx1kkzn定義：定義：濾波殘差，是考慮一個(gè)處于跟蹤維持階段濾波殘差，是考慮一個(gè)處于跟蹤維持階段的目標(biāo)（已經(jīng)初始化），設(shè)的目標(biāo)（已經(jīng)初始化），設(shè)k-1時(shí)刻狀態(tài)變量的時(shí)刻狀態(tài)變量的濾濾 波預(yù)報(bào)值為波預(yù)報(bào)值為 ，通過(guò)觀測(cè)方程可以求出，通過(guò)觀測(cè)方程可以求出k時(shí)時(shí)刻量測(cè)的預(yù)報(bào)值刻量測(cè)的預(yù)報(bào)值 ，它與，它與k時(shí)刻量測(cè)信號(hào)之差時(shí)刻量測(cè)信號(hào)之差為濾波殘差向量。為濾波殘差向量。跟蹤門基本概念跟蹤門基本概念11kkkkkzzz濾波殘差：n注意：注意：如果我們把目標(biāo)跟蹤看成動(dòng)態(tài)參數(shù)估計(jì)，如果我們把目標(biāo)跟蹤看成動(dòng)態(tài)參數(shù)估計(jì)，濾波殘差就是前面講的濾波殘差就是前面講的“新息新息(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的第 個(gè)分量n 矩形跟蹤門：矩形跟蹤門： 最簡(jiǎn)單的跟蹤門形成方法是在跟蹤空間內(nèi)定義最簡(jiǎn)單的跟蹤門形成方法是在跟蹤空間內(nèi)定義一個(gè)矩形區(qū)域，即矩形跟蹤門。一個(gè)矩形區(qū)域，即矩形跟蹤門。 定義各種向量的分量：定義各種向量的分量： 定義定義跟蹤門常數(shù)為跟蹤門常數(shù)為KG。它取決于觀測(cè)。它取決于觀測(cè)概率概率密

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

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

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

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

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

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

22、ov過(guò)程。過(guò)程。n非機(jī)動(dòng)目標(biāo)跟蹤中，目標(biāo)的控制輸入非機(jī)動(dòng)目標(biāo)跟蹤中，目標(biāo)的控制輸入u等于零。等于零。而在有機(jī)動(dòng)目標(biāo)跟蹤中，對(duì)目標(biāo)的控制輸入而在有機(jī)動(dòng)目標(biāo)跟蹤中，對(duì)目標(biāo)的控制輸入u 通通?？梢约僭O(shè)為未知加速度，它決定了機(jī)動(dòng)的數(shù)學(xué)?？梢约僭O(shè)為未知加速度，它決定了機(jī)動(dòng)的數(shù)學(xué)模型，具體應(yīng)用中可以分為：模型，具體應(yīng)用中可以分為：2）非機(jī)動(dòng)模型非機(jī)動(dòng)模型 常速常速CV模式模式n在三維物理空間的點(diǎn)目標(biāo)運(yùn)動(dòng)，可以用三維的位在三維物理空間的點(diǎn)目標(biāo)運(yùn)動(dòng)，可以用三維的位移和速度向量來(lái)描述：移和速度向量來(lái)描述：n非機(jī)動(dòng)目標(biāo)的動(dòng)態(tài)模型一般可描述為：非機(jī)動(dòng)目標(biāo)的動(dòng)態(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非機(jī)動(dòng)模型非機(jī)動(dòng)模型 常速常速CV模式模式n離散化模型（離散化模型（T采樣間隔）采樣間隔）TTGTFGGGdiagFFFdiagCVCVkCVCVCVkCVCVCVk2211,101,w wx xx xykkkykkkkwTyyywTyTyyy,1,2211方向速度：方向位移：具體給出一個(gè)狀態(tài)分量（具體給出一個(gè)狀態(tài)分量（y方向）的具體表達(dá)式：方向）的具體表達(dá)式：非機(jī)

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

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

26、獨(dú)立增量隨機(jī)過(guò)程，簡(jiǎn)稱常加速過(guò)程，簡(jiǎn)稱常加速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過(guò)程加速度機(jī)動(dòng)模型過(guò)程加速度機(jī)動(dòng)模型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）一般多項(xiàng)式模型）一般多項(xiàng)式模型n眾所周知，任何連續(xù)目標(biāo)軌跡都可以用某個(gè)階眾所周知，任何連續(xù)目標(biāo)軌跡都可以用某個(gè)階次的多項(xiàng)式以任意精度逼近。這樣，可以把目次的多項(xiàng)式以任意精度逼近。這樣，可以把目標(biāo)運(yùn)動(dòng)建成笛卡兒坐標(biāo)系中的標(biāo)運(yùn)動(dòng)建成笛卡兒坐標(biāo)系中的 n 階多項(xiàng)式模型：階多項(xiàng)式模型：1111( )( )( )( )( )( )( )/ !onxonyonznx taaaw ttty tbbbw tz tcccw ttnx說(shuō)明：說(shuō)明：這個(gè)模型作為跟蹤模型很少采用，其原這個(gè)模型作為

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

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

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

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

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

33、r加速度模型的離散方程加速度模型的離散方程3）Singer運(yùn)動(dòng)模型第一次將位置目標(biāo)的加速度描述運(yùn)動(dòng)模型第一次將位置目標(biāo)的加速度描述為一個(gè)時(shí)間相關(guān)的隨機(jī)過(guò)程，成為進(jìn)一步建立其為一個(gè)時(shí)間相關(guān)的隨機(jī)過(guò)程，成為進(jìn)一步建立其他模型的基礎(chǔ)，可以說(shuō)他模型的基礎(chǔ)，可以說(shuō)Singer模型是一個(gè)目標(biāo)機(jī)模型是一個(gè)目標(biāo)機(jī)動(dòng)的標(biāo)準(zhǔn)模型。動(dòng)的標(biāo)準(zhǔn)模型。7）“當(dāng)前當(dāng)前”模型模型 均值自適應(yīng)加速度模型均值自適應(yīng)加速度模型n“當(dāng)前當(dāng)前”模型：模型： 也是一種加速度模型，其本質(zhì)上也是一種加速度模型，其本質(zhì)上就是一個(gè)帶自適應(yīng)的就是一個(gè)帶自適應(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模型為零均值模型，這種機(jī)動(dòng)加速度的零模型為零均值模型，這種機(jī)動(dòng)加速度的零均

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

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

37、目標(biāo)跟蹤的狀態(tài)方程和觀測(cè)方程：解：雷達(dá)目標(biāo)跟蹤的狀態(tài)方程和觀測(cè)方程：1kkkkkkkkFwHvxxxxx xz ( ), ( ), ( ), ( )Tr k r kkkk kx x目標(biāo)跟蹤的典型算法應(yīng)用目標(biāo)跟蹤的典型算法應(yīng)用n狀態(tài)方程和觀測(cè)方程的系數(shù)表達(dá)式：狀態(tài)方程和觀測(cè)方程的系數(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)。計(jì)算

38、卡爾曼濾波所。計(jì)算卡爾曼濾波所需的狀態(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)行卡爾曼迭代濾波！計(jì)算本次輸出：計(jì)算本次輸出：為下次計(jì)算準(zhǔn)備：為下次計(jì)算準(zhǔn)備：11111111111111)()(kkkkkkkkkkkkkkkkkkkkkvkTkkkkTkkkkkkkkkzGxFzGxFxxHkzzQHPHHPFGFG11111)(1)Tkkkkkkk kk kk kwTvkkkkTwTkkkkkkk kk kPFGHPFGHQQFG HPFQ（5、不同坐標(biāo)的量測(cè)模型與、不同坐標(biāo)的量測(cè)模型與EKL濾波跟蹤濾波跟蹤1）一般傳感

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

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

42、以以用用跟跟蹤蹤濾濾波波zxww ww2）基于導(dǎo)數(shù)的線性化：）基于導(dǎo)數(shù)的線性化：n觀測(cè)方程線性化觀測(cè)方程線性化-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、 為下次計(jì)算更新為下次計(jì)算更新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矩陣計(jì)算（偏微分矩陣計(jì)算（偏微分矩陣計(jì)算）矩陣計(jì)算）n 如何確定差分運(yùn)算的如何確定差分運(yùn)算的x*，最簡(jiǎn)單的方法是：，最簡(jiǎn)單的方法是：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濾波處理濾波處理 為下次計(jì)算更新為下次計(jì)算更新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、量測(cè)轉(zhuǎn)換與基于、量測(cè)轉(zhuǎn)換與基于BLUE的的Kalman濾

46、波濾波n由于大部分傳感器的觀測(cè)坐標(biāo)與目標(biāo)坐標(biāo)不同，由于大部分傳感器的觀測(cè)坐標(biāo)與目標(biāo)坐標(biāo)不同，可以對(duì)觀察信號(hào)進(jìn)行處理，得到目標(biāo)坐標(biāo)系的量可以對(duì)觀察信號(hào)進(jìn)行處理，得到目標(biāo)坐標(biāo)系的量測(cè)信號(hào)測(cè)信號(hào) 稱為量測(cè)轉(zhuǎn)換處理。稱為量測(cè)轉(zhuǎn)換處理。1）二維觀測(cè)信號(hào)的量測(cè)轉(zhuǎn)換處理：）二維觀測(cè)信號(hào)的量測(cè)轉(zhuǎn)換處理：,cos()sin()kk realkkkkkk realkkkkrrrxryr,()cos()()sin()kk realkk realkk realkkk realkk realkk realkxxxrryyyrr量測(cè)轉(zhuǎn)換與基于量測(cè)轉(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新的觀測(cè)方程：新的觀測(cè)方程： 其中：其中：kkkkHzxv量測(cè)轉(zhuǎn)換與基于量測(cè)轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波cov()covkkkkkkkkx xx ykkkky xy ykkkxRRyRR vn新的觀測(cè)方程中觀測(cè)噪聲不在是高斯白噪聲，但新的觀測(cè)方程中觀測(cè)噪聲

48、不在是高斯白噪聲，但可以求一階可以求一階/二階矩特性二階矩特性: cos()(cos) 1() sin()(cos) 1kkkkkkkkkrExEyrEE v2/2(cos),(sin)0kkkEeE當(dāng)當(dāng)為為高高斯斯白白噪噪聲聲分分布布時(shí)時(shí)：量測(cè)轉(zhuǎn)換與基于量測(cè)轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波n其中各項(xiàng)方差具體表達(dá)式其中各項(xiàng)方差具體表達(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 量測(cè)轉(zhuǎn)換與基于量測(cè)轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波n二維量測(cè)轉(zhuǎn)換處理后的二維量測(cè)轉(zhuǎn)換處理后的Kalman濾波問(wèn)題：濾波問(wèn)題：假設(shè)狀態(tài)方程：假設(shè)狀態(tài)方程： 觀測(cè)方程：觀測(cè)方程：,kkkkkHHIzxv1kkkkFxxw000cov()cov()cov()wkkvkkwQvQxxP，k kk k同同時(shí)時(shí)已已知知：w w 是是0 0均均值值獨(dú)獨(dú)立立過(guò)過(guò)程程，且且v v 不不是是0 0均均值值獨(dú)獨(dú)立立過(guò)過(guò)程程，但但已已知知系系統(tǒng)統(tǒng)初初始始狀狀態(tài)態(tài)的的統(tǒng)

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

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

52、kkukkkkkkkkkkkkkrrrrxyrrrrv()0ukkukxEyE v量測(cè)轉(zhuǎn)換與基于量測(cè)轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波cov()covkkkkkkkkx xx yukkkky xy yukkkxRRyRR vn此時(shí)觀測(cè)噪聲的二階統(tǒng)計(jì)特性：此時(shí)觀測(cè)噪聲的二階統(tǒng)計(jì)特性：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 量測(cè)轉(zhuǎn)換與基于量測(cè)轉(zhuǎn)換與基于BLUE的的Kalman濾波濾波n顯然此時(shí)可以建立下列方程：顯然此時(shí)可以建立下列方程： 狀態(tài)方程：狀態(tài)方程： 觀測(cè)方程：觀測(cè)方程：,kkkkkHHIzxv1kkkkFxxwn同時(shí)下列一階同時(shí)下列一階/二階特性可求：二階特性可求：()0,cov()()0,cov()wkkkvkkkE wwQE vvQ 此時(shí)可以利用基于此時(shí)可以利用基于BLUE的的Kalman濾波進(jìn)行跟濾波進(jìn)行跟蹤濾波處理。蹤濾波處理?；诨贐L

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

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