《卡爾曼濾波方法》

1、3 卡爾曼濾波方法3.1 卡爾曼濾波的特點及應(yīng)用領(lǐng)域3.2 系統(tǒng)的狀態(tài)空間描述3.3 卡爾曼濾波的直觀推導(dǎo)3.4 卡爾曼濾波的遞推運算方程3.5 卡爾曼濾波的結(jié)構(gòu)圖3.6 卡爾曼濾波的應(yīng)用實例3.7 聯(lián)邦卡爾曼濾波3.8 聯(lián)邦卡爾曼濾波的應(yīng)用實例3.9 Unscented卡爾曼濾波.23.1 卡爾曼濾波的特點及應(yīng)用領(lǐng)域卡爾曼濾波卡爾曼濾波(Kalman Filtering)是是1960年由年由R.E.Kalman首首次提出的一種估計方法。之所以稱為濾波，是因為它是一次提出的一種估計方法。之所以稱為濾波，是因為它是一種排除隨機干擾，提高檢測精度的一種手段。種排除隨機干擾，提高檢測精度的一種手段。

2、KF是基于是基于最小方差準(zhǔn)則最小方差準(zhǔn)則推導(dǎo)出來的一種線性濾波器。推導(dǎo)出來的一種線性濾波器。KF是一種時域是一種時域遞推算法遞推算法，根據(jù)上一狀態(tài)的估計值和當(dāng)前，根據(jù)上一狀態(tài)的估計值和當(dāng)前狀態(tài)的觀測值推出當(dāng)前狀態(tài)，不需存儲大量的歷史數(shù)據(jù)，狀態(tài)的觀測值推出當(dāng)前狀態(tài)，不需存儲大量的歷史數(shù)據(jù)，便于計算機實現(xiàn)。便于計算機實現(xiàn)。KF要求明確已知系統(tǒng)模型。即在應(yīng)用卡爾曼濾波之前，要求明確已知系統(tǒng)模型。即在應(yīng)用卡爾曼濾波之前，首先要建立首先要建立系統(tǒng)模型系統(tǒng)模型和和觀測模型觀測模型，并假定過程噪聲、觀測，并假定過程噪聲、觀測噪聲為高斯白噪聲。噪聲為高斯白噪聲。應(yīng)用領(lǐng)域：機器人導(dǎo)航、目標(biāo)跟蹤、組合導(dǎo)航等。其中

3、，應(yīng)用領(lǐng)域：機器人導(dǎo)航、目標(biāo)跟蹤、組合導(dǎo)航等。其中，組合導(dǎo)航組合導(dǎo)航是卡爾曼濾波最成功的應(yīng)用領(lǐng)域。是卡爾曼濾波最成功的應(yīng)用領(lǐng)域。.33.2 系統(tǒng)的狀態(tài)空間描述連續(xù)系統(tǒng)模型：連續(xù)系統(tǒng)模型：-狀態(tài)方程-觀測方程維維向向量量函函數(shù)數(shù)。維維和和維維隨隨機機測測量量噪噪聲聲；維維系系統(tǒng)統(tǒng)隨隨機機干干擾擾；維維觀觀測測向向量量；維維狀狀態(tài)態(tài)向向量量；mnthtfmtVptWmtZntX)(),()()()()(.4系統(tǒng)的狀態(tài)空間描述(續(xù)).53.3 卡爾曼濾波的直觀推導(dǎo)則設(shè)想：則設(shè)想：，的最優(yōu)線性估計的最優(yōu)線性估計且已得到且已得到次測量結(jié)果次測量結(jié)果時刻已獲得時刻已獲得假設(shè)在假設(shè)在1| 11121,kkk

4、kkXXZZZZkk1|1|1| 11,1|kkkkkkkkkkkXHZXX 得得預(yù)測測量估計偏差預(yù)測測量估計偏差：1|1|1|kkkkkkkkkXHZZZZ 利用此偏差利用此偏差修正預(yù)測估計修正預(yù)測估計：11|kkkkkkkkkXHZKXX待定校正增益陣.6卡爾曼濾波的直觀推導(dǎo)(續(xù)) 增益陣的求法增益陣的求法：極極小小化化使使目目標(biāo)標(biāo)函函數(shù)數(shù)|kkTkkXXEkkkkkkkkkkXXXXXX|11| 定義：定義：濾濾波波器器的的遞遞推推運運算算方方程程卡卡爾爾曼曼求求得得增增益益陣陣，進進而而求求得得極極小小，使使|TkkkkkkXXEP.73.4 卡爾曼濾波的遞推運算方程111,1kkkk

5、kkXX狀態(tài)預(yù)測TkkkkkTkkkkkkkkQPP1,11,1,111,1方差預(yù)測111kTkkkkTkkkkRHPHHPK增益矩陣11|kkkkkkkZKXX狀態(tài)估值方差估值1kkkkkkPHKIP時間更新/預(yù)測測量更新/修正000000PPXX初始條件11kkkkkkXHZZ新息序列.83.5 卡爾曼濾波的結(jié)構(gòu)圖延時一步1, kkkZ+-1| kkZkKkX+當(dāng)前估計值上一步估計值1|kkXkH一步預(yù)測1kX 上述遞推公式，稱為卡爾曼濾波器。實際上，卡爾曼上述遞推公式，稱為卡爾曼濾波器。實際上，卡爾曼濾波器也是一個系統(tǒng)，其結(jié)構(gòu)框圖如下：濾波器也是一個系統(tǒng)，其結(jié)構(gòu)框圖如下：.93.6 卡爾

6、曼濾波的應(yīng)用實例(艦船導(dǎo)航)狀態(tài)變量狀態(tài)變量TENKSVVkX)(1 1、系統(tǒng)模型、系統(tǒng)模型 卡爾曼濾波最成功的工程應(yīng)用是設(shè)計運載體的高精度組合導(dǎo)航卡爾曼濾波最成功的工程應(yīng)用是設(shè)計運載體的高精度組合導(dǎo)航系統(tǒng)。下面以艦船導(dǎo)航問題為例，介紹其具體應(yīng)用。系統(tǒng)。下面以艦船導(dǎo)航問題為例，介紹其具體應(yīng)用。式中，式中，艦艦船船航航向向變變化化率率。艦艦船船航航向向；船船舶舶相相對對于于水水的的速速度度；量量；海海流流北北向向和和東東向向速速度度分分和和經(jīng)經(jīng)度度弧弧長長和和緯緯度度弧弧長長；和和KSVVEN.10 為獨立的零均值高為獨立的零均值高斯白噪聲斯白噪聲, b為海流相關(guān)時間常數(shù)。為海流相關(guān)時間常數(shù)。)

7、7 , 2 , 1(ii狀態(tài)方程狀態(tài)方程7654321sincos,KSbVVbVVKSVKSVWtXfXNNEEEN43214321vKvSvvzzzz觀測方程觀測方程 量測系統(tǒng)由量測系統(tǒng)由GPS和航位推算系統(tǒng)和航位推算系統(tǒng)(DR)組成，組成，GPS輸出艦船的經(jīng)緯度輸出艦船的經(jīng)緯度和和。 DR由羅經(jīng)和計程儀組成，分別輸出由羅經(jīng)和計程儀組成，分別輸出航向航向K和船相對于水流的速度和船相對于水流的速度S。.112 2、初始條件和參數(shù)選取、初始條件和參數(shù)選取.123 3、仿真結(jié)果、仿真結(jié)果(a)(a).133 3、仿真結(jié)果、仿真結(jié)果(b)(b).143 3、仿真結(jié)果、仿真結(jié)果(c)(c).153.

8、7 聯(lián)邦卡爾曼濾波卡爾曼濾波最成功的工程應(yīng)用是設(shè)計運載體的高精度組合卡爾曼濾波最成功的工程應(yīng)用是設(shè)計運載體的高精度組合導(dǎo)航系統(tǒng)。為了與聯(lián)邦濾波方法相區(qū)別，將普通的卡爾曼導(dǎo)航系統(tǒng)。為了與聯(lián)邦濾波方法相區(qū)別，將普通的卡爾曼濾波稱為集中卡爾曼濾波。濾波稱為集中卡爾曼濾波。由于對導(dǎo)航精度要求的提高，導(dǎo)航設(shè)備越來越多。另一方由于對導(dǎo)航精度要求的提高，導(dǎo)航設(shè)備越來越多。另一方面，現(xiàn)代系統(tǒng)向大系統(tǒng)和復(fù)雜系統(tǒng)的方向發(fā)展。這種情況面，現(xiàn)代系統(tǒng)向大系統(tǒng)和復(fù)雜系統(tǒng)的方向發(fā)展。這種情況下采用集中式卡爾曼實現(xiàn)組合導(dǎo)航，存在兩個問題：下采用集中式卡爾曼實現(xiàn)組合導(dǎo)航，存在兩個問題： 計算負擔(dān)重。濾波器計算量以狀態(tài)維數(shù)的三次

9、方劇增，無法滿足導(dǎo)計算負擔(dān)重。濾波器計算量以狀態(tài)維數(shù)的三次方劇增，無法滿足導(dǎo)航的實時性要求；航的實時性要求； 容錯性能差，不利于故障診斷。當(dāng)任一導(dǎo)航子系統(tǒng)發(fā)生故障，且沒容錯性能差，不利于故障診斷。當(dāng)任一導(dǎo)航子系統(tǒng)發(fā)生故障，且沒有及時檢測出并隔離時，則整個導(dǎo)航系統(tǒng)都會被污染，使輸出的信有及時檢測出并隔離時，則整個導(dǎo)航系統(tǒng)都會被污染，使輸出的信息不可靠。息不可靠。為解決上述問題，出現(xiàn)了為解決上述問題，出現(xiàn)了分散化濾波分散化濾波的思想和方法。其中，的思想和方法。其中， Carlson在在1988年提出的聯(lián)邦濾波由于設(shè)計靈活、計算量小、年提出的聯(lián)邦濾波由于設(shè)計靈活、計算量小、容錯性好而受到重視，已被美

10、國空軍確定為新一代導(dǎo)航系容錯性好而受到重視，已被美國空軍確定為新一代導(dǎo)航系統(tǒng)的通用濾波器。統(tǒng)的通用濾波器。.16 聯(lián)邦濾波器示意圖子濾波器子濾波器1狀態(tài)估計狀態(tài)估計1主濾波器主濾波器信息融合信息分配全局狀態(tài)估計全局狀態(tài)估計子濾波器子濾波器2子濾波器子濾波器n狀態(tài)估計狀態(tài)估計2狀態(tài)估計狀態(tài)估計n.17 方法思想在諸多非相似導(dǎo)航子系統(tǒng)中選擇導(dǎo)航信息全面、輸出速率在諸多非相似導(dǎo)航子系統(tǒng)中選擇導(dǎo)航信息全面、輸出速率高、可靠性好的子系統(tǒng)（如慣性導(dǎo)航系統(tǒng)）與其余導(dǎo)航子高、可靠性好的子系統(tǒng)（如慣性導(dǎo)航系統(tǒng)）與其余導(dǎo)航子系統(tǒng)兩兩結(jié)合，形成若干子濾波器。系統(tǒng)兩兩結(jié)合，形成若干子濾波器。各子濾波器并行運行，獲得建

11、立在局部量測基礎(chǔ)上的局部各子濾波器并行運行，獲得建立在局部量測基礎(chǔ)上的局部最優(yōu)估計。最優(yōu)估計。各局部最優(yōu)估計在第二級濾波器（即主濾波器）內(nèi)按融合各局部最優(yōu)估計在第二級濾波器（即主濾波器）內(nèi)按融合算法合成，獲得建立在所有量測基礎(chǔ)上的全局估計。算法合成，獲得建立在所有量測基礎(chǔ)上的全局估計。全局估計再按信息守恒原則反饋給各子濾波器。如此反復(fù)全局估計再按信息守恒原則反饋給各子濾波器。如此反復(fù)遞推。遞推。F 實際設(shè)計的聯(lián)邦濾波器是全局次優(yōu)的，但是對于自主性要求很高的重要運載體來說，導(dǎo)航系統(tǒng)的可靠性比精度更重要。采用聯(lián)邦濾波器設(shè)計組合導(dǎo)航系統(tǒng)，雖然損失了少許精度，但得到的是組合導(dǎo)航系統(tǒng)的強容錯能力。.18

12、 聯(lián)邦濾波器算法信息融合信息融合其中，其中， 為為 N 個不相關(guān)的局部狀態(tài)估計，個不相關(guān)的局部狀態(tài)估計， 為相應(yīng)的估計誤差協(xié)方差為相應(yīng)的估計誤差協(xié)方差 。NXXX,21NNPPP,2211,11NiiiiggXPPX111NiiigPP 對于來自各子濾波器信息（狀態(tài)估計值和誤差方差對于來自各子濾波器信息（狀態(tài)估計值和誤差方差估計），在主濾波器按以下規(guī)則進行融合：估計），在主濾波器按以下規(guī)則進行融合：.19信息分配信息分配,giXX ,1giiiPP1111mNiiQQQ其中，其中， 為信息分配系數(shù)，為信息分配系數(shù)， 為為主濾波器的信息分配系數(shù)，滿足守恒原則主濾波器的信息分配系數(shù)，滿足守恒原則m

13、Nii, 1,11mNii,1QQmim 在進入下一次遞推之前，需將主濾波器中的信息在進入下一次遞推之前，需將主濾波器中的信息（狀態(tài)、方差）在各子濾波器中按如下規(guī)則進行分配：（狀態(tài)、方差）在各子濾波器中按如下規(guī)則進行分配： 聯(lián)邦濾波器算法.20 聯(lián)邦卡爾曼濾波器結(jié)構(gòu)信息分配卡爾曼濾波信息融合聯(lián)邦卡爾曼濾波器.21 聯(lián)邦卡爾曼濾波器結(jié)構(gòu)圖 注：組合導(dǎo)航中，一般選擇可靠性好的系統(tǒng)作為參考注：組合導(dǎo)航中，一般選擇可靠性好的系統(tǒng)作為參考系統(tǒng)，如慣性導(dǎo)航系統(tǒng)。系統(tǒng)，如慣性導(dǎo)航系統(tǒng)。.22聯(lián)邦卡爾曼濾波器的應(yīng)用01002003004005006007008009001000-150-100-5005010

14、0150聯(lián) 邦 卡 爾 曼 濾 波 算 法集 中 卡 爾 曼 濾 波 算 法時間/s經(jīng)度弧長估計誤差經(jīng)度弧長估計誤差/m 仍然以艦船導(dǎo)航為例，模型與前面相同。為了與集中卡仍然以艦船導(dǎo)航為例，模型與前面相同。為了與集中卡爾曼濾波器的效果相比較，同時采用兩種濾波器濾波。爾曼濾波器的效果相比較，同時采用兩種濾波器濾波。01002003004005006007008009001000-200-150-100-50050100150聯(lián) 邦 卡 爾 曼 濾 波 算 法集 中 卡 爾 曼 濾 波 算 法維度弧長估計誤差維度弧長估計誤差/m時間/s.23聯(lián)邦卡爾曼濾波器的應(yīng)用(續(xù)) 圖中可以看出，聯(lián)邦卡爾曼濾

15、波與集中卡爾曼濾波算法圖中可以看出，聯(lián)邦卡爾曼濾波與集中卡爾曼濾波算法的估計精度相當(dāng)，但聯(lián)邦卡爾曼濾波相對于集中卡爾曼濾的估計精度相當(dāng)，但聯(lián)邦卡爾曼濾波相對于集中卡爾曼濾波，不僅有更好的容錯性，而且可以并行運算。波，不僅有更好的容錯性，而且可以并行運算。1002003004005006007008009001000-0.8-0.6-0.4-0.200.20.40.6聯(lián) 邦 卡 爾 曼 濾 波 算 法集 中 卡 爾 曼 濾 波 算 法海流北向速度分量估計誤差海流北向速度分量估計誤差/(m/s)時間/s1002003004005006007008009001000-0.5-0.4-0.3-0.2-

16、0.100.10.20.30.4聯(lián) 邦 卡 爾 曼 濾 波 算 法集 中 卡 爾 曼 濾 波 算 法海流東向速度分量估計誤差海流東向速度分量估計誤差/(m/s)時間/s.243.9 Unscented卡爾曼濾波 UKF方法的建立是基于如下的事實：逼近任意的分布比方法的建立是基于如下的事實：逼近任意的分布比逼近它的任何非線性函數(shù)更容易。逼近它的任何非線性函數(shù)更容易。UKF的基礎(chǔ)是的基礎(chǔ)是Unscented變換變換(Unscented Transformation, UT)，其基本思想是用一組，其基本思想是用一組確定的離散采樣點確定的離散采樣點(稱為稱為Sigma點點)來近似狀態(tài)變量的分布。來近似

17、狀態(tài)變量的分布。UKF假定狀態(tài)滿足高斯分布，因此只需逼近其均值和方差。假定狀態(tài)滿足高斯分布，因此只需逼近其均值和方差。 對于如下的非線性變換：對于如下的非線性變換：假設(shè)已知假設(shè)已知n維隨機向量維隨機向量 x 的均值的均值 和協(xié)方差和協(xié)方差 ，則對上述，則對上述非線性系統(tǒng)進行濾波的目的是計算非線性系統(tǒng)進行濾波的目的是計算 y 的均值的均值 和和 方差方差 。xxPyyP)(xfy - (3-1).250,2, 1,)(, 1,)(ixnniPnxniPnxixixi1. 構(gòu)造構(gòu)造 Sigma 點點ixxPxx構(gòu)構(gòu)造造一一組組采采樣樣點點的的附附近近在在，和和協(xié)協(xié)方方差差的的均均值值根根據(jù)據(jù)隨隨機

18、機向向量量0為可調(diào)尺度參數(shù)，調(diào)整它可以提高逼近精度。為可調(diào)尺度參數(shù)，調(diào)整它可以提高逼近精度。2. 利用利用Sigma點進行非線性傳播點進行非線性傳播 將上面構(gòu)造的將上面構(gòu)造的Sigma點直接按照點直接按照(3-1)的關(guān)系作非線性的關(guān)系作非線性變換，則產(chǎn)生一組變換樣本點變換，則產(chǎn)生一組變換樣本點)(iifY.26Unscented卡爾曼濾波(續(xù))變換樣本點變換樣本點Yi 即可近似表示即可近似表示 y 的分布。下面利用的分布。下面利用 Yi 來計算來計算 y的均值和方差。的均值和方差。3. 計算計算 y 的均值和方差的均值和方差ipimiYWy0)(TiipiciyyYyYWP)(0)(nininWWcimi2, 1),(2/10),/()()(其中，其中，.27UKF的具體應(yīng)用過程 Unscented卡爾曼濾波算法基于如下的非線性離散狀態(tài)卡爾曼濾波算法基于如下的非線性離散狀態(tài)空間模型：空間模型： kkkkwxfx)(1kkkkvxhy)( 時間更新時間更