幾種非線性濾波算法的研究-內(nèi)附程序

1、2017年秋季學(xué)期研究生課程考核（讀書報(bào)告、研究報(bào)告）考核科目:雷達(dá)系統(tǒng)導(dǎo)論學(xué)生所在（系）:電子與信息工程學(xué)院學(xué)生所在學(xué)科:電子與同學(xué)工程學(xué)生姓名學(xué)號(hào)學(xué)生類別考核結(jié)果閱卷人第1頁(yè)（共頁(yè)）幾種非線性濾波算法的介紹與性能分析作者姓名：學(xué)號(hào)：專業(yè)院系：電信學(xué)院電子工程系電子郵件：療4 非線性濾波算法在雷達(dá)目標(biāo)跟蹤中有著重要 的應(yīng)用，對(duì)雷達(dá)的跟蹤性能有著至關(guān)重要的影響。好 的濾波算法有利于目標(biāo)航跡的建立及保持，能夠得到 較精確的目標(biāo)位置，為發(fā)現(xiàn)目標(biāo)后的后續(xù)工作提供可 靠的數(shù)據(jù)依據(jù)。本文重點(diǎn)介紹了雷達(dá)數(shù)據(jù)處理中的幾 種非線性濾波算法：擴(kuò)展卡爾曼濾波(ekf)、不敏卡 爾曼濾波(ukf)、粒子濾波(pf)

2、,并且給出了一個(gè) 利用這三種算法進(jìn)行數(shù)據(jù)處理的一個(gè)實(shí)例，通過這個(gè) 實(shí)例對(duì)比分析了這三種算法的性能以及優(yōu)劣。關(guān)鍵字非線性濾波算法;擴(kuò)展卡爾曼濾波;不敏 卡爾曼濾波;粒子濾波;i. 概述(一級(jí)表題格式)在雷達(dá)對(duì)目標(biāo)進(jìn)行跟蹤前要先對(duì)目標(biāo)進(jìn)行檢測(cè)。對(duì) 于滿足檢測(cè)條件的目標(biāo)就需要進(jìn)行跟蹤，在跟蹤的過程 中對(duì)以利用新獲得的數(shù)據(jù)完成對(duì)冃標(biāo)的進(jìn)一步檢測(cè)比如 去除虛假目標(biāo)等，同時(shí)利用跟蹤獲得數(shù)據(jù)可以進(jìn)一步完 成対目標(biāo)動(dòng)態(tài)特性的檢測(cè)和識(shí)別。因此對(duì)目標(biāo)進(jìn)行準(zhǔn)確 的跟蹤是雷達(dá)性能的一個(gè)重要指標(biāo)。在檢測(cè)到滿足條件 的目標(biāo)后，根據(jù)目標(biāo)運(yùn)動(dòng)狀態(tài)建立目標(biāo)運(yùn)動(dòng)模型，然后 對(duì)目標(biāo)跟蹤算法進(jìn)行設(shè)計(jì)，這是雷達(dá)目標(biāo)跟蹤中的核心 部分。

3、目前主要的跟蹤算法包括線性自回歸濾波，兩點(diǎn)外 推濾波、維納濾波、濾波、加權(quán)最小二乘濾波、 維納濾波和卡爾曼濾波山。對(duì)于線性系統(tǒng)而言最優(yōu)濾波 的方法就是卡爾曼濾波，卡爾曼濾波是線性高斯模型下 的最優(yōu)狀態(tài)估計(jì)算法。但是實(shí)際問題中目標(biāo)的運(yùn)動(dòng)模型 往往不是線性的，因此卡爾曼濾波具有很大的局限性。 目前主要用的非線性濾波算法可以分為高斯濾波和粒子 濾波。不敏卡爾曼濾波和擴(kuò)展卡爾曼濾波就是高斯濾 波中的典型代表，也是應(yīng)用相對(duì)較為廣泛的。粒子濾波 的應(yīng)用范圍比高斯濾波的適用范圍要廣，對(duì)于系統(tǒng)狀態(tài) 非線性，觀測(cè)模型非高斯等問題都有很好的適用性。本 文具體分析闡述了擴(kuò)展卡爾曼濾波算法，不敏卡爾曼濾 波算法，粒子

4、濾波算法，并且通過一個(gè)實(shí)例利用仿真的 方法分析了這三種算法在濾波性能上的優(yōu)劣，最后對(duì)這 三種算法做了一定的總結(jié)。我本科畢業(yè)設(shè)計(jì)題目為基于歷史數(shù)據(jù)的路徑生成 算法研究，由于我是跨專業(yè)保研到電信學(xué)院，該課題 所研究?jī)?nèi)容不屬于雷達(dá)系統(tǒng)研究范圍，是一種城市路網(wǎng) 最快路徑生成算法。ii. 幾種非線性濾波算法a. 擴(kuò)展卡爾曼濾波擴(kuò)展卡爾曼濾波是將非線性系統(tǒng)轉(zhuǎn)換為近似的線性 系統(tǒng)的一種方法，其核心思想是圉繞濾波值將非線性函 數(shù)展開成泰勒級(jí)數(shù)并略去二階及以上的項(xiàng)，得到一個(gè)近 似的線性化模型，然后應(yīng)用卡爾曼濾波完成狀態(tài)估計(jì)。擴(kuò)展卡爾曼濾波狀態(tài)空間模型：x&+1 = /(忑)+叫 狀態(tài)方程zk = h(x

5、k) + vk觀測(cè)方程其中/()和h(.)為非線性函數(shù)在擴(kuò)展卡爾曼濾波中，狀態(tài)的預(yù)測(cè)以及觀測(cè)值的預(yù) 測(cè)由非線性函數(shù)計(jì)算得岀，線性卡爾曼濾波屮的狀態(tài)轉(zhuǎn) 移矩陣a陣和觀測(cè)矩陣h陣由f和h函數(shù)的雅克比矩 陣代替。對(duì)/()和taylor展開，只保留一次項(xiàng)有：/(")= /(念)+ 4(血一九)"(無(wú))"(&)+弘(忑-&)其中：為/對(duì)無(wú)“求導(dǎo)的雅克比矩陣為對(duì)無(wú)7求導(dǎo)的雅克比矩陣& 竝-1),于是可以得出：耳+1 = a內(nèi)+(/(九)一佔(zhàn))+叫%嚴(yán)4" + (加九)- /紜)+叫通過以上變換，將非線性問題線性化。接下來(lái)ekf 濾波過程同線性

6、卡爾曼濾波相同，公式如下：xa+ir)=/(x (k | k)p(k + hk) =(k)p(k |k)'(k) + 0伙)s(k + 1) = h(k + 1)p伙 +11 k)hk +1) + r(k +1)k(jt +1) = p (k + k)h'(k +1)5_, (k +1)& = 乂, i = 0v $ = x + k)px | , i = 1,nx i+nx = x _+ k)px).,' = 1,., ©x(k + 伙 +1)=龍伙 +11 k) + k(k + l)z伙 + 1)-h(k + 1)x伙+ 1| 燈p伙+ 1 伙+ 1

7、) = (/ x伙+ 1 伙+ 1)h 伙+ 1)p 伙+ 1 伙)通過ekf算法線性化狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣 后，剩下的濾波過程與普通的卡爾曼濾波無(wú)異，濾波過 程簡(jiǎn)單且容易進(jìn)行。正因ekf簡(jiǎn)單易于實(shí)現(xiàn)的特性， 使得該算法一直以來(lái)都應(yīng)用廣泛，但是它的局限性也是 非常明顯的。在這種濾波方法中非線性因子的存在對(duì)濾 波穩(wěn)定性和狀態(tài)估計(jì)精度都用很大的影響，其濾波結(jié)果 的好壞與量測(cè)噪聲和狀態(tài)噪聲的統(tǒng)計(jì)特性也有很大的關(guān) 系，対于高斯噪聲，該算法有很好的適用性，但是對(duì)于 非高斯噪聲，該算法的濾波精度會(huì)受到很大的影響。在 濾波過程屮由于需要預(yù)先估計(jì)過程噪聲協(xié)方差矩陣 0伙）和量測(cè)噪聲協(xié)方差矩陣r伙），如果這兩

8、個(gè)矩陣的 值的估計(jì)出現(xiàn)較大的誤差，將使得濾波結(jié)果出現(xiàn)很大的 偏差，容易產(chǎn)生誤差積累從而導(dǎo)致濾波結(jié)果發(fā)散。在用 該算法濾波進(jìn)的初始時(shí)刻要預(yù)先假設(shè)狀態(tài)的初始值和初 始協(xié)方差，如果這兩個(gè)值的估計(jì)出現(xiàn)較大偏差，濾波結(jié) 果也會(huì)岀現(xiàn)發(fā)散的現(xiàn)彖?？偟膩?lái)說(shuō)只有當(dāng)系統(tǒng)的動(dòng)態(tài)模 型和觀測(cè)模型都接近線性吋，利用擴(kuò)展卡爾曼濾波算法 跟蹤目標(biāo)會(huì)収得較好的效果，濾波結(jié)果較接近真實(shí)值。b. 不敏卡爾曼濾波算法不敏濾波也叫無(wú)損卡爾曼濾波，它的核心是不敏變 換，摒棄了對(duì)非線性函數(shù)進(jìn)行線性化的傳統(tǒng)做法，采用 卡爾曼線性濾波框架，依據(jù)不敏變換汁算非線性變換的 隨機(jī)變量的統(tǒng)計(jì)特性，對(duì)于一步預(yù)測(cè)方程，使用不敏變 換來(lái)處理均值和協(xié)方差的

9、非線性傳遞。不敏變化不需要 像ekf那樣對(duì)非線性狀態(tài)方程和量測(cè)方程線性化，它 是對(duì)狀態(tài)向量的概率密度兩數(shù)進(jìn)行近似化，用一系列確 定樣本來(lái)逼近狀態(tài)的后驗(yàn)概率密度，表現(xiàn)為一些列選収 好的采樣點(diǎn)，不盅要求導(dǎo)計(jì)算雅克比矩陣。這些采樣點(diǎn) 完全體現(xiàn)了高斯密度的真實(shí)均值和協(xié)方差。近似化后的 概率密度函數(shù)仍然是高斯的。當(dāng)這些點(diǎn)經(jīng)過任何非線性 系統(tǒng)的傳遞后，得到的后驗(yàn)均值和協(xié)方差都能夠精確到 二階。rh于不需要對(duì)非線性系統(tǒng)進(jìn)行線性化，可以很 容易的應(yīng)用于非線性系統(tǒng)的狀態(tài)估。對(duì)于不敏變換對(duì)以做如下闡述：假設(shè)x為一個(gè)©維隨機(jī)向量，g: rg t r”、為一 生線性函數(shù)并且y二g（x）。x的均值和協(xié)方差分別

10、為 。計(jì)算ut變化的步驟如下：式中，久是一個(gè)尺度參數(shù)，可以為任何數(shù)值，只要 仏+兄）上0。（j（代+ 是仏+ 2比均方根矩陣 的第i行或第i列，©為狀態(tài)向量的維數(shù)，其中 a = n （a2 -1）。 每個(gè)采樣點(diǎn)通過非線性函數(shù)傳播，得到x = g（6），, = 0,1,2,.,2乞 計(jì)算y的估計(jì)均值和協(xié)方差2恨獨(dú)尹工wj,=工比o-刃（x-刃'匸0/=0濾波模型如下：由狀態(tài)方程可以計(jì)算得到6點(diǎn)的一步預(yù)測(cè)：6（21 伙） = /£,§ 伙伙）狀態(tài)預(yù)測(cè)估計(jì)和狀態(tài)預(yù)測(cè)協(xié)方差：xa+i伙）=£比點(diǎn)伙+i伙）1=02叭p（k +11幻二工叱ax,伙+1伙）a

11、x；伙+1伙）+ 0伙）/=0式中伙+1伙）=纟伙+1|切一去伙+1伙）由量測(cè)方程可得/點(diǎn)量測(cè)的預(yù)測(cè)值：g （k +1伙）=力伙+1, &伙+1伙）量測(cè)的預(yù)測(cè)值和協(xié)方差為：首先計(jì)算s+1 ）個(gè)踩樣點(diǎn)纟和相對(duì)應(yīng)的權(quán)值叱2伙+1伙）=£叱©伙+1伙）1=0p: = £w.az;( + 1| qaz：伙 +11 幻 + r伙 +1)t=0式中az, (k +1伙)二©伙+1伙)一 2伙+11幻量測(cè)和狀態(tài)向量的互協(xié)方差為：化二工叱ax" + 1伙)az；/=0狀態(tài)更新和協(xié)方差更新表示為：x(k + 1伙+ 1)=文仇+ 1伙)+ k伙+ 1)z

12、伙+ 1) 2伙+ 1| 燈p(k + 1|£ + 1) = p 伙+ 1| 燈一 k 伙+ 1)s伙+ 1)k伙+ 1)k(k + v) = pj；不敏卡爾曼濾波不必計(jì)算雅克比矩陣，不必對(duì)非線 性系統(tǒng)函數(shù)/(x)進(jìn)行任何形式的逼近；在預(yù)測(cè)階段只 是標(biāo)準(zhǔn)的線性代數(shù)運(yùn)算；對(duì)于系統(tǒng)函數(shù)來(lái)說(shuō)可以不連 續(xù)。不敏卡爾曼濾波算法的計(jì)算量一般擴(kuò)展卡爾曼濾波 算法，這是由于擴(kuò)展卡爾曼濾波算法通過線性化處理來(lái) 實(shí)現(xiàn)非線性濾波佔(zhàn)計(jì)，而不敏卡爾曼濾波是利用樣本來(lái) 逼近狀態(tài)的概率密度函數(shù)，計(jì)算量主要發(fā)生在選取 點(diǎn)時(shí)的方根分解運(yùn)算j石。在計(jì)算速度上擴(kuò)展卡爾曼 濾波算法擁冇明顯優(yōu)勢(shì)，但他的性能隨著非線性強(qiáng)度變

13、大而明顯下降。不敏卡爾曼濾波算法因不用線性化處理 而很好的解決了這一問題。但是不敏卡爾曼濾波算法是 用高斯分布來(lái)逼近系統(tǒng)狀態(tài)的后驗(yàn)概率密度，如果系統(tǒng) 狀態(tài)的后驗(yàn)概率密度函數(shù)時(shí)非高斯的，那么濾波結(jié)果將 產(chǎn)生極大的誤差。c. 粒子濾波算法粒子濾波是一種非線性濾波算法，是一種基于 monte carlo仿真的最優(yōu)回歸貝葉斯濾波算法。這種 濾波方法將所關(guān)心的狀態(tài)矢量表示為一組帶有相關(guān)權(quán)值 的隨機(jī)樣本，并基于這些樣本和權(quán)值可以計(jì)算出狀態(tài)估 值。這種方法不受線性化誤差或高斯噪聲假定的限制， 適用于任何環(huán)境下的任何狀態(tài)轉(zhuǎn)化或量測(cè)模型。粒子濾 波算法的核心思想便是利用一系列隨機(jī)樣本的加權(quán)和表 示后驗(yàn)概率密度，

14、通過求和來(lái)近似積分操作。濾波模型 表述如下：假定k時(shí)刻，一組隨機(jī)樣本xjg；是根據(jù)后驗(yàn) 概率密度p(x0:jz1:j所獲得的釆樣。其中xj表示為0 到k時(shí)刻的第i個(gè)樣本集合，即粒子集合；/為相關(guān) 權(quán)值，并且權(quán)值滿足1=1, ns為樣本采樣數(shù)，即粒 el子數(shù)；z無(wú)代表傳感器k時(shí)刻的量測(cè)集合； x/x“ = o,.，h表示為0到k時(shí)刻的所有狀態(tài)向量 集合。則在k時(shí)刻，后驗(yàn)概率密度可近似表示為：p(x0;jz1:a.) = 2；(x0,-xi,)1=1由于很難直接從p(x0:,|z,j抽取樣木，通?？衫?用一個(gè)重要性概率密度龍(xiz)來(lái)獲得樣本值。從而， 權(quán)值/可以按序貫重點(diǎn)抽樣的方法獲得。如杲x

15、(l是從 /x|z)獲得的樣本，未歸一化的權(quán)值7可以定義為：如果所選擇的重要性概率密度滿足：|zq“(x； |&歸,比)龍(&：1 izg)由以上兩式可得：k ”(x；wj 弘t為了能夠方便的采用回歸貝葉斯濾波算法，我們希望重 要性概率密度只與前一時(shí)刻的測(cè)量和狀態(tài)有關(guān)，即：機(jī) x；|x；z，zg 二龍(x；|x：t，zj根據(jù)以上兩式有：p（zjx；）p（x；|x；j弘龍（x；|x；t，zj't在粒子濾波算法中，經(jīng)過幾個(gè)迭代周期后，大多數(shù) 的粒子權(quán)值會(huì)趨近于零，即粒子衰減現(xiàn)象。由于粒子權(quán) 值的協(xié)方差隨著時(shí)間的增長(zhǎng)而不斷變大，這種現(xiàn)象是無(wú) 法避免的。為了減弱這種影響，一種最

16、直接的方法就是 使用大量的粒子數(shù)目。當(dāng)然，這經(jīng)常是不實(shí)際的，因 此，目前釆用的兩種方法：(1)選擇最優(yōu)的重要性概 率密度；(2)進(jìn)行重抽樣。最優(yōu)的重耍性概率密度為：龍(x；|x：-z)= p(x：|x；7，zj最優(yōu)重要性概率密度可以使采樣點(diǎn)權(quán)值的協(xié)方差最 小。冃前有兩種情況經(jīng)常采用最優(yōu)重要性概率密度。第 一種情況是xk為有限集合，例如用于跟蹤機(jī)動(dòng)目標(biāo)的 跳變馬爾科夫線性系統(tǒng)；笫二種情況是狀態(tài)方程為非線 性的，量測(cè)方程為線性的系統(tǒng)。重抽樣的基木思想是削減小權(quán)值的粒子，并集屮較 大權(quán)值的粒子。冃前經(jīng)常采用的集中重抽樣方法有：分 層抽樣和殘差抽樣、系統(tǒng)重抽樣。為了得到正確的狀態(tài) 估計(jì)，通常希望粒子權(quán)

17、值的方差盡可能趨近于零。然而，蒙特卡洛模擬方法一般都存在權(quán)值退化問題。在實(shí) 際計(jì)算中，經(jīng)過數(shù)次迭代，只有少數(shù)粒子的權(quán)值較大， 其余粒子的權(quán)值可忽略不計(jì)。粒子權(quán)值的方差隨著時(shí)間 增大，狀態(tài)空間中的有效粒子數(shù)較少。對(duì)于大多數(shù)系統(tǒng) 來(lái)說(shuō)最優(yōu)重要性概率密度往往是無(wú)法實(shí)現(xiàn)的因此經(jīng)常利 用線性化的技術(shù)對(duì)最優(yōu)重要性概率密度進(jìn)行次優(yōu)近似。粒子濾波算本質(zhì)上是一種狀態(tài)搜索算法。我們估 計(jì)得到一系列狀態(tài)，然后將這些狀態(tài)與我們的觀測(cè)進(jìn)行 比較，依據(jù)似然函數(shù)，我們判斷哪一些狀態(tài)是最有可能 生成當(dāng)前觀測(cè)的狀態(tài)，然后保留他們，并且給了他們較 大的權(quán)重，其他的狀態(tài)責(zé)備削弱或者拋棄。但是如果似 然函數(shù)冇若干個(gè)窄的峰，那么粒子將

18、冇可能聚集到其中 的某些峰處，對(duì)其他的峰視而不見，也就是說(shuō)粒子濾波 不能保證對(duì)任意復(fù)雜形狀的概率分布都實(shí)現(xiàn)準(zhǔn)確的描述 和跟蹤，尤其當(dāng)空間維數(shù)較高時(shí)，這一缺陷會(huì)更加突 岀，這歸根結(jié)底是重采樣破壞了粒子的多樣性。一個(gè)好 的重采樣算法應(yīng)該在增加粒子多樣性和減少權(quán)值較小的 粒子數(shù)目之間進(jìn)行有效折衷。iii. 仿真分析在本文中我們分別利用擴(kuò)展卡爾曼濾波算法，不敏 卡爾曼濾波算法，粒子濾波算法對(duì)一個(gè)非線性例子進(jìn)行 仿真分析。系統(tǒng)的狀態(tài)方程和量測(cè)方程都是非線性的。 模型如下：x嚴(yán)0.5笛+25笛（1 +火二）+ 8心（1.2伙-1） +匕<z嚴(yán) x；/20 + %實(shí)驗(yàn)參數(shù)設(shè)定：系統(tǒng)噪聲匕un（1o,1

19、）,量測(cè)噪 聲吧ejn（1,1）,初始狀態(tài)x0=l ,初始方差=5 ,時(shí) 間序列長(zhǎng)度t=50,o不敏卡爾曼濾波中的參數(shù)為： g = 1, 0二0, k=2 o粒子濾波中的粒子數(shù)n=300.本文 對(duì)各種濾波方法進(jìn)行/ m=50次的monte carlo仿真， 三種算法的濾波結(jié)果如圖1所示。時(shí)間步長(zhǎng)圖1.三種算法的狀態(tài)估計(jì)曲線圖1給岀了三種非線性濾波算法的經(jīng)過5（）次 monte carlo仿真后的狀態(tài)估計(jì)。從圖中可以看出擴(kuò)展 卡爾曼濾波算法的濾波效果很差，濾波結(jié)果大部分時(shí)間 都偏離真實(shí)狀態(tài)較大。不敏卡爾曼濾波效果相對(duì)來(lái)說(shuō)較 好，大部分時(shí)間濾波結(jié)果和真實(shí)狀態(tài)之間的偏差不大。 粒子濾波的效果最好，濾

20、波結(jié)果和真實(shí)狀態(tài)之間的差距 很小，二者具有很好的一致性。三種濾波算法的均方根誤差圖像如圖2所示。時(shí)何步長(zhǎng)圖2.三種算法的均方根誤差曲線由圖二可以看出擴(kuò)展卡爾曼濾波算法的均方根誤差 最大，而且謀差數(shù)值相對(duì)真實(shí)值偏離較大，這主要是由 于線性化的過程中忽略了高階項(xiàng)，引入了線性化誤差。 不敏卡爾曼濾波算法的均方根誤差相對(duì)較小，但是粒子 濾波的均方根誤差最小，跟隨真實(shí)值的效果最好。因此 可以看岀在非線性條件下，粒子濾波的效果好于不敏卡 爾曼濾波好于擴(kuò)展卡爾曼濾波。表1各種濾波算法的實(shí)驗(yàn)結(jié)果比較濾波運(yùn)算復(fù)運(yùn)行時(shí)間（s）存儲(chǔ)非線性算法雜度（歸一化）量強(qiáng)度ekf0(2)0.142低弱非線性u(píng)kf0(n4)0.

21、220中無(wú)限制pfo(nn3)0.855高無(wú)限制表1給出了各種濾波算法的運(yùn)算復(fù)雜度，運(yùn)算時(shí) 間，運(yùn)算所需存儲(chǔ)塑和濾波對(duì)非線性條件的要求。從運(yùn) 算速度上看，擴(kuò)展卡爾曼濾波算法具有絕對(duì)優(yōu)勢(shì)，這是 市于他將系統(tǒng)方程和量測(cè)方程線性化以后的結(jié)果。但是 根據(jù)圖1和圖2對(duì)以看岀在非線性較強(qiáng)的條件下這種濾 波算法的效果很差，濾波精度較低，甚至出現(xiàn)發(fā)散。從 濾波精度上看不敏卡爾曼濾波算法和粒子濾波算法差距 不大，但是粒子濾波算法的汁算量要比不敏卡爾曼濾波 算法的大的多，而且算法復(fù)雜度是隨z粒子數(shù)目的增長(zhǎng) 而線性增加的。對(duì)于這兩種算法，在一般的高斯白噪聲環(huán)境下宜使用不敏卡爾曼濾波算法，但是在更復(fù)雜的非 高斯情況下

22、粒子濾波具有更好的適用性。iv.結(jié)論本文詳細(xì)的介紹了三種非線性濾波算法：擴(kuò)展卡爾 曼濾波算法，不敏卡爾曼濾波算法，粒子濾波算法。在 給出三種濾波算法的詳細(xì)的理論基礎(chǔ)上對(duì)每一種算法的 優(yōu)缺點(diǎn)都進(jìn)行了一定的分析和總結(jié)。在文章的最后通過 一個(gè)仿真實(shí)驗(yàn)分析比較了這三種算法在具體濾波過程屮 的性能。擴(kuò)展卡爾曼濾波算法在線性化狀態(tài)方程和量測(cè) 方程的過程中引入了線性化誤淬，濾波的精度出現(xiàn)嚴(yán)重 下降，濾波結(jié)杲甚至岀現(xiàn)發(fā)散；但是線性化后的模型讓 它的計(jì)算量大大地減小了。不敏卡爾曼濾波算法和粒子 濾波算法在非線性問題的目標(biāo)跟蹤中表現(xiàn)的都很好，不 敏卡爾曼濾波算法的計(jì)算量相對(duì)來(lái)說(shuō)是比較小的，不過 這種算法只適用于高

23、斯白噪聲的環(huán)境中，如果系統(tǒng)狀態(tài) 的后驗(yàn)概率密度函數(shù)吋非高斯的，那么濾波結(jié)果將產(chǎn)生 極大的誤差。粒子濾波最復(fù)雜的環(huán)境有極高的適應(yīng)性， 而且也不要求噪聲服從高斯分布，但是該算法的計(jì)-算量 較大，計(jì)算量是隨著粒子數(shù)目線性增加的，而粒子數(shù)目 往往較大，這就對(duì)計(jì)算機(jī)數(shù)據(jù)處理性能提出了較高的要 求。致謝在本論文的撰寫過程中袁子寅師兄給予了很多的建 設(shè)性意見，宋河晏同學(xué)在編程方面提供了一定的幫助， 特此表示感謝。參考文獻(xiàn)11秦勤.雷達(dá)目標(biāo)跟蹤的卡爾曼濾波方法的研究d. 大連海事大學(xué),2006.2殷俊麗.天波超視距雷達(dá)機(jī)動(dòng)目標(biāo)檢測(cè)與跟蹤d. 西安電子科技大學(xué),2009.3夏建濤，任震，陳立，景占榮.極坐標(biāo)下卡

24、爾曼濾波算 法的研究j.西北工業(yè)大學(xué)學(xué)報(bào),2000 (03) : 396- 399.【4龔亞信.基于粒子濾波的弱目標(biāo)檢測(cè)前跟蹤算法研 究d.國(guó)防科學(xué)技術(shù)大學(xué),2009. 吳兆平.雷達(dá)微弱目標(biāo)檢測(cè)和跟蹤方法研究d.西 安電子科技大學(xué),2012.16劉麗麗.兒種非線性濾波算法的性能分析j.價(jià)值 工程，2010, 29(34) : 190-191% ekf ukf pf三種算法對(duì)比clcclose allclear;% tic;x = 0.1;%初始狀態(tài)x_estimate = 1;%狀態(tài)的估計(jì)e_x_estimate = x_estimate; %ekf 的初始估計(jì) u_x_estimate =

25、x_estimate; %ukf 的初始估計(jì) p_x_estimate = x_estimate; %pf 的初始估計(jì)q= 10;%s程狠態(tài)協(xié)方差r= 1；%測(cè)量噪聲協(xié)方差p =5;%初始估計(jì)方差e_p = p; %ukf 方羌u_p = p;%uk f 方差pf_p = p;%pf 方差tf=50;%模擬長(zhǎng)度x.array = x;%真實(shí)值數(shù)組c_x_cstimatc_array = lc_x_cstimatcj;%ekf 最優(yōu)估計(jì)值數(shù)組 u_x_estimate_anay = u_x_estimate;%ukf 最優(yōu)估計(jì)值數(shù)組 p_x_estimale_array = p_x_estima

26、te;%pf 最優(yōu)估計(jì)值數(shù)組 u_k = 1; % wj參數(shù) u_symmelry_number = 4; %對(duì)稱的點(diǎn)的個(gè)數(shù)u_total_number 二 2 * u_symmetry_number + 1; % 總的采樣點(diǎn)的個(gè)數(shù) hnear 0n = 500; %嘉子濾波的粒子數(shù)close all;%粒了濾波初始n個(gè)粒了for i = 1 : np_xpart(i) = p_x_estimate + sqrt(pflp) * randn;endfor k = 1 : tf%模擬系統(tǒng)x = linear * x + (25 * x / (1 + xa2) + 8 * cos( 1.2*(k-

27、l) + sqrt(q) * randn; % 狀態(tài)值y = (xa2 / 20) + sqrt(r) * randn; % 觀測(cè)值%擴(kuò)展卡爾曼濾波器%進(jìn)行估計(jì)第一階段的估計(jì)e_x_estimate_l = linear * e_x_estimate + 25 * e_x_estimate /(1 +e_x_estimatea2) + 8 * cos(1.2*(k-l); e_y_estimate = (e_x_estimale_l)a2/20; %這是根據(jù)k=l時(shí)估計(jì)值另1得到的觀測(cè)值；%相關(guān)矩陣e_a = linear + 25 * (1 -e_x_estimatea2)/( 1 +e_x

28、_estimatea2)a2);% 傳遞矩陣e_h = e_x_estimate_l/10; % 觀測(cè)矩陣%估計(jì)禹誤差e_p_estimale = e_a * e_p * e_a* + q;%護(hù)展卡爾曼增玉e_k = e_p_estimate * e_h'/(e_h * e_p_estimate * e_h' + r);%進(jìn)行石環(huán)值的更新鎗二階陵e_x_estimate_2 = e_x_estimate_l + e_k * (y - e_y_estimate);%或新后的命計(jì)值角誤差e_p_estimate_update = e_p_estimate - e_k * e_h *

29、 e_p_estimate;%運(yùn)入下一次迭代的參藪交化e_p = e_p_estimate_update;e_x_estimate = e_x_estimate_2;%粒子濾波器%粒了濾波器for i = 1 : np_xpartminus(i) = 0.5 * p_xpart(i) + 25 * p_xpart(i) / (1 + p_xpart(i)a2) + 8 * cos(1.2*(k-l) +sqrt(q) * randn; p_ypart = p_xpartminus(i)a2 / 20; % 預(yù)測(cè)值p_vhat = yp_ypart;%觀測(cè)和預(yù)測(cè)的差p_q(i) = (1 / s

30、qrt(r) / sqrt(2*pi) * exp(-p_vhata2 /2/r); % 各個(gè)粒子的權(quán)值 end%平均每一個(gè)估計(jì)的可能性p_qsum = sum(p_q);for i = 1 : np_q(i) = p_q(i) / p_qsum;%各個(gè)粒子進(jìn)行權(quán)值歸一化end%重采樣權(quán)重大的粒子多采點(diǎn)，權(quán)重小的粒子少采點(diǎn)，相當(dāng)于每一次都進(jìn)行重采樣；for i = 1 : np_u = rand;p_qlempsum = 0;forj = 1 : np_qtempsum = p_qtempsum + p_q(j)；if pqtempsum >= p_up_xpart(i) = p_xpa

31、rtminus(j); % 在這里 xpart(i)實(shí)現(xiàn)循環(huán)賦值；break;endendendp_x_eslimale = mean(p_xpart);u_x_par = u_x_estimate;for i = 2 : (u_symmetry_numbcr+1)u_x_par(i,:) = u_x_estimate + sqrt(u_symmetry_number+u_k) * u_p);enclfor i = (u_symmetry_number+2): u_total_numberu_x_par(i/) = u_x_estimate - sqrt(u_symmetry_number+u

32、_k) * u_p);encl%計(jì)算權(quán)值u_w_1 = u_k/( u_sy m metry _nu mber+u_k);u_w_nl = 1/(2 * (u_symmetry_number+u_k);%把這些粒子通過社遞方程彳 g到下一個(gè)決態(tài)for i = 1: u_total_numberu_x_par(i) = 0.5 * u_x_par(i) + 25 * u_x_par(i)/( 1 +u_x_par(i)a2) + 8 * cos(1.2*(k-i); end%傳遞后的均值和方差u_x_next = u_w_i * u_x_par( 1);for i = 2 : u_totalnu

33、mberu_x_next = u_x_next + u_w_n 1 * u_x_par(i);endu_p_next = q + u_w_l * (u_x_pa(l)-u_x_next) * (u_x_par(l)-u_x_next)；for i = 2 : u_totalnumberu_p_next = u_p_next + u_w_ni * (u_x_par(i)u_x_next) * (u_x_par(i)-u_x_next)： endfor i = 1 :u.totalnumberu_y_2obser(i,:) = u_x_p 如(i);end%通過觀測(cè)方程得到一系列的粒子for i

34、= 1: u_totalnumberu_y_2obser(i) = u_y_2obser(i)a2/20;end%通過觀測(cè)方程后的均值y.obseu_y_obse = u_w_l * u_y_2obser(l);for i = 2 : u_total_numbctu_y_obsc = u_y_obsc + u_w_n 1 * u_y_2obscr(i);end%pzz測(cè)量方差矩陣u_pzz = r + u_w_l * (u_y_2obscr( 1 )-u_y_obsc)*(u_y_2obscr( 1 )-u_y_obsc)f;for i = 2 : u_total_numbcru_pzz =

35、u_pzz + u_w_n 1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)： end%pxz狀態(tài)向量與測(cè)量值的協(xié)方差矩陣u_pxz = u_w_l * (u_x_par(l) - u_x_ncxt)* (u_y_2obsct(l)u_y_obsc)：for i = 2 : u_total_numberu_pxz = u_pxz + u_w_nl * (u_x_par(i) - u_x_nexl) * (u_y_2obser(i)- u_y_obse)*;end%卡爾曼增益u_k = u_pxz/u_pzz;%估計(jì)量的更希u_x_next_optimal = u_x_next + u_k * (y - u_y_obse);% 第一步的估計(jì)值 + 修正值； u_x_estimate = u_x_next_optimal;%方差的更新u_p_next_update = u_p_next - u_k * u_pzz * u_kf;u_p = u_p_next_update;%進(jìn)行畫li程序x_array = x_array,x;e_x_es ti ma