卡爾曼濾波器原理

1、卡爾曼濾波器原理卡爾曼濾波器原理林斌林斌 P2011090261234內(nèi)容提綱內(nèi)容提綱發(fā)展概述算法前提算法推導(dǎo)算法總結(jié)卡爾曼濾波5算法擴(kuò)展Rudolf (Rudy) Emil Klmn（1930 ）卡爾曼濾波工程背景：1960s航空航天工程突飛猛進(jìn)電子計(jì)算機(jī)又方興未艾正式提出：一種關(guān)于線性過濾和預(yù)測(cè)難題的新方法1960A new approach to linear filtering and prediction problems ， 19601發(fā)展概述卡爾曼濾波1發(fā)展概述優(yōu)勢(shì)：1、采用物理意義較為直觀的時(shí)域狀態(tài)空間2、僅需要前后兩步的數(shù)據(jù)，數(shù)據(jù)存儲(chǔ)量較小3、使用比較簡(jiǎn)單的遞推算法，便于在計(jì)

2、算機(jī)上實(shí)現(xiàn)4、不僅適用于平穩(wěn)過程，還可以推廣到非平穩(wěn)隨機(jī)過程的情況發(fā)展：卡爾曼濾波器已成為推廣研究和應(yīng)用的主題，尤其是在自主或協(xié)助導(dǎo)航領(lǐng)域?？柭鼮V波2算法前提隨機(jī)離散系統(tǒng)模型定義隨機(jī)離散時(shí)間過程的狀態(tài)向量 ，該過程由以下離散隨機(jī)差分方程描述：111kkkkwBuAxxkkkvHxzmkRz nkRx 1 . 2假設(shè)系統(tǒng)滿足可觀性要求，定義觀測(cè)向量 ，得到觀測(cè)方程：2 . 2隨機(jī)信號(hào) 和 分別表示過程激勵(lì)噪聲和觀測(cè)噪聲，并假設(shè)它們是相互獨(dú)立并滿足正態(tài)分布的白噪聲。kvkwnx1 nxn nxn nx1 nx1mx1 mxn mx1卡爾曼濾波2算法前提即有：過程激勵(lì)噪聲觀測(cè)噪聲實(shí)際過程中過程激勵(lì)

3、噪聲協(xié)方差矩陣Q和觀測(cè)噪聲協(xié)方差矩陣R可能隨著每次迭代計(jì)算而變化，但在這里我們假設(shè)它們?yōu)槌?shù)。另外，狀態(tài)轉(zhuǎn)移矩陣A，輸入矩陣B和測(cè)量方程2.2中的矩陣H，在實(shí)際過程中都可能隨時(shí)間變化而變化，但在這兒假設(shè)為常數(shù)。3 . 24 . 2), 0(RNvk), 0(QNwk卡爾曼濾波2算法前提先驗(yàn)估計(jì)和后驗(yàn)估計(jì)，及其誤差定義 為根據(jù)上一次迭代計(jì)算結(jié)果而產(chǎn)生的估計(jì)值，稱為先驗(yàn)估計(jì)。定義 為根據(jù)當(dāng)前計(jì)算結(jié)果而產(chǎn)生的估計(jì)值，稱為后驗(yàn)估計(jì)。定義先驗(yàn)估計(jì)誤差為定義后驗(yàn)估計(jì)誤差為nkRx nkRx kkkxxenx1 真值5 . 2kkkxxenx1 真值6 . 2卡爾曼濾波2算法前提定義先驗(yàn)估計(jì)誤差的協(xié)方差為定

4、義 后驗(yàn)估計(jì)誤差的協(xié)方差為)cov(,kkkkeeeEPT)cov(,EkkkkeeePT7 . 28 . 2卡爾曼濾波目的描述：在系統(tǒng)結(jié)構(gòu)已知的情況下，給定k時(shí)刻的狀態(tài)觀測(cè)向量 ，求k時(shí)刻的系統(tǒng)狀態(tài)向量的最優(yōu)估計(jì) ，使得 最小?；舅悸罚?、根據(jù) 計(jì)算出k時(shí)刻的先驗(yàn)估計(jì) 同時(shí)也產(chǎn)生了先驗(yàn)估計(jì)的協(xié)方差矩陣 的遞推公式3算法推導(dǎo)kzkx kP1kxkx 11kkkBuxAx1 . 3kPQAPAPTkk12 . 3卡爾曼濾波2、根據(jù)先驗(yàn)估計(jì) 計(jì)算出k時(shí)刻的觀測(cè)向量的估計(jì)3、計(jì)算實(shí)測(cè)值 與估計(jì) 的差，以此來修正之前的先驗(yàn)估計(jì) ，得到后驗(yàn)估計(jì)把 帶入：3算法推導(dǎo))(kkkkkxHzKxx3 . 3k

5、x kkxHz kzkz kx kx 權(quán)重 殘差這里的權(quán)重系數(shù)也成為卡爾曼增益。至此卡爾曼濾波計(jì)算原型公式基本結(jié)束，下面要確定出最優(yōu)系數(shù)K和在最優(yōu)系數(shù)下的后驗(yàn)估計(jì)誤差的協(xié)方差矩陣的遞推方法)(kkkkkzzKxxkz 卡爾曼濾波推導(dǎo)后驗(yàn)協(xié)方差矩陣按照定義，我們從誤差協(xié)方差 開始推導(dǎo)如下：帶入 再帶入3算法推導(dǎo)kP)cov(kkkxxPkP)(cov(kkkkkkxHzKxxP)(kkkkkxHzKxxkkkvHxz)(cov(kkkkkkkxHvHxKxxP卡爾曼濾波整理測(cè)量誤差向量，得：因?yàn)樵肼曧?xiàng)與其他項(xiàng)不相關(guān)，協(xié)方差=0，所以有：利用協(xié)方差矩陣性質(zhì)，提出常數(shù)矩陣，得：3算法推導(dǎo))(cov(

6、kkkkkkvKxxHKIP)cov()(cov(kkkkkkvKxxHKIPTkkkTkkkkkKvKHKIxxHKIP)cov()(cov()(卡爾曼濾波如果記 ，則有：3算法推導(dǎo)kkkPxx)cov(TkkTkkkkRKKHKIPHKIP)()(4 . 3卡爾曼濾波推導(dǎo)最優(yōu)卡爾曼增益最優(yōu)化K：使后驗(yàn)估計(jì) 的協(xié)方差 達(dá)到最小。（換一個(gè)概念）也是使向量的二范數(shù)的數(shù)學(xué)期望值最小化的一個(gè)過程。這等同于后驗(yàn)估計(jì)的協(xié)方差矩陣的跡最小化3算法推導(dǎo)kKkx kPkkxx|2kkxxEkkkPtrxxP|2卡爾曼濾波首先展開3.4式，得：記：上面的式子可以寫為：3算法推導(dǎo)TkTkkTkTkkkkkKRHP

7、HKKHPPHKPP)(RHPHSTkkTkkkTkTkkkkkKSKKHPPHKPP5 . 3卡爾曼濾波把 對(duì) 求導(dǎo)，并令導(dǎo)數(shù)=0，則可以得到 取最小值時(shí)的最優(yōu)化 的值。解得：3算法推導(dǎo))(kPtrTTCBdABACdtr)(引入常用數(shù)學(xué)公式？kK)(kPtrkK022)()()()(kkTkkkTTkkTkTkkkSKHPSKKSHPPHdKPdtr11)(RHPHHPSHPKTkTkkTkk6 . 3卡爾曼濾波化簡(jiǎn)后驗(yàn)誤差協(xié)方差公式在卡爾曼增益等于上面導(dǎo)出的最優(yōu)值時(shí)，計(jì)算后驗(yàn)協(xié)方差的公式可以進(jìn)行簡(jiǎn)化。對(duì)于卡爾曼增益公式在卡爾曼增益公式兩側(cè)同時(shí)右乘得：把上式帶入3.5式，可以消去后面的兩項(xiàng)

8、，得：3算法推導(dǎo)1kTkkSHPKTkkKSTkTkTkkkKHPKSKkkkkPHKPP3算法推導(dǎo)整理，得：這個(gè)公式的計(jì)算比較簡(jiǎn)單，所以實(shí)際中總是使用這個(gè)公式，但是需注意這公式僅在使用最優(yōu)卡爾曼增益的時(shí)候它才成立。如果算術(shù)精度總是很低而導(dǎo)致數(shù)值穩(wěn)定性出現(xiàn)問題，或者特意使用非最優(yōu)卡爾曼增益，那么就不能使用這個(gè)簡(jiǎn)化；必須使用3.5式表示的后驗(yàn)誤差協(xié)方差公式。kkkPHKIP)(7 . 3卡爾曼濾波卡爾曼增益的物理意義其中：H矩陣為常量； 與過程激勵(lì)噪聲的協(xié)方差矩陣Q有關(guān)；R為測(cè)量噪聲的協(xié)方差矩陣。取值范圍：當(dāng) R 趨向于零時(shí)，有：當(dāng) 趨向于零時(shí)，有：3算法推導(dǎo)0lim0kPKkkPRHPHHPK

9、TkTkkkP10lim HKkR, 01HKk卡爾曼濾波意義：決定了最優(yōu)估計(jì)組成比例的“調(diào)節(jié)器”當(dāng) R 趨向于零時(shí)，有：測(cè)量噪聲 V=0此時(shí)3.3式改為系統(tǒng)表現(xiàn)為完全取測(cè)量值作為狀態(tài)的后驗(yàn)估計(jì)值，而系統(tǒng)的先驗(yàn)狀態(tài)估計(jì)完全被拋棄。反之當(dāng) 趨向于零時(shí)，根據(jù)式3.2可知，Q=0易知，此時(shí)系統(tǒng)完全拋棄測(cè)量值，取先驗(yàn)估計(jì)值3算法推導(dǎo)kP10lim HKkRmx1kkkkkzHxHzHxx11)(nx1卡爾曼濾波算法描述卡爾曼濾波器用反饋控制的方法估計(jì)過程狀態(tài)。估計(jì)過程某一時(shí)刻的狀態(tài)，然后以（含噪聲）測(cè)量值得方式獲得反饋，因此卡爾曼濾波器可以分為兩個(gè)部分：1、時(shí)間更新方程負(fù)責(zé)向前推算當(dāng)前狀態(tài)向量和誤差協(xié)

10、方差估計(jì)的值，為下一個(gè)時(shí)間狀態(tài)構(gòu)成先驗(yàn)估計(jì)。2、測(cè)量更新方程負(fù)責(zé)將先驗(yàn)估計(jì)和新的測(cè)量變量結(jié)合已構(gòu)成改進(jìn)后的后驗(yàn)估計(jì)。4算法總結(jié)卡爾曼濾波時(shí)間更新方程測(cè)量更新方程4算法總結(jié)11kkkBuxAx1 . 42 . 41)(RHPHHPKTkTkk3 . 44 . 4)(kkkkxHzKxxQAPAPTkk 15 . 4kkkPHKIP)(卡爾曼濾波4算法總結(jié)BAHKkx kx kz1kx1kukz 時(shí)間更新（預(yù)測(cè)）測(cè)量更新（校正）算法框圖1、狀態(tài)估計(jì)向量的運(yùn)算流程圖卡爾曼濾波4算法總結(jié)算法框圖2、誤差協(xié)方差的運(yùn)算流程圖AATI -KkHkPQ1kP時(shí)間更新（預(yù)測(cè)）測(cè)量更新（修正）kP卡爾曼濾波濾波器參數(shù)調(diào)整在卡爾曼濾波器實(shí)際實(shí)現(xiàn)時(shí)，測(cè)量噪聲R一般可以觀測(cè)得到，是濾波器的已知條件