卡爾曼濾波器原理

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

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

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

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

5、x kkxHz kzkz kx kx 權(quán)重 殘差這里的權(quán)重系數(shù)也成為卡爾曼增益。至此卡爾曼濾波計算原型公式基本結(jié)束，下面要確定出最優(yōu)系數(shù)K和在最優(yōu)系數(shù)下的后驗估計誤差的協(xié)方差矩陣的遞推方法)(kkkkkzzKxxkz 卡爾曼濾波推導(dǎo)后驗協(xié)方差矩陣按照定義，我們從誤差協(xié)方差 開始推導(dǎo)如下：帶入 再帶入3算法推導(dǎo)kP)cov(kkkxxPkP)(cov(kkkkkkxHzKxxP)(kkkkkxHzKxxkkkvHxz)(cov(kkkkkkkxHvHxKxxP卡爾曼濾波整理測量誤差向量，得：因為噪聲項與其他項不相關(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：使后驗估計 的協(xié)方差 達到最小。（換一個概念）也是使向量的二范數(shù)的數(shù)學(xué)期望值最小化的一個過程。這等同于后驗估計的協(xié)方差矩陣的跡最小化3算法推導(dǎo)kKkx kPkkxx|2kkxxEkkkPtrxxP|2卡爾曼濾波首先展開3.4式，得：記：上面的式子可以寫為：3算法推導(dǎo)TkTkkTkTkkkkkKRHP

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

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

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

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