第五講：卡爾曼濾波

1、卡爾曼濾波算法及應(yīng)用目錄一. 概述二. 標(biāo)準(zhǔn)卡爾曼濾波n卡爾曼濾波方程n閉環(huán)卡爾曼濾波n卡爾曼濾波特性及實(shí)現(xiàn)中的問(wèn)題三. 擴(kuò)展卡爾曼濾波n非線性系統(tǒng)n線性化卡爾曼濾波n擴(kuò)展卡爾曼濾波四. Schmidt 卡爾曼濾波五. 自適應(yīng)卡爾曼濾波六. 平滑算法2一、概述一、概述2021-11-2631.1 Rudolf Emil Kalman4uBorn 1930 in Hungary uBS and MS from MIT uPhD 1957 from Columbia uFilter developed in 1960-61目目 錄錄 概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法

2、Kalman R E. A new approach to linear filtering and prediction problems J. Journal of Fluids Engineering, 1960, 82(1): 35-45. （引用：18083）5uKalman濾波是一種最優(yōu)估計(jì)估計(jì)算法，而非濾波器 能夠?qū)崟r(shí)估計(jì)估計(jì)系統(tǒng)中的參數(shù)參數(shù)（如連續(xù)變化的位置、速度等信息）。 估計(jì)量估計(jì)量通過(guò)一系列受噪聲污染的觀測(cè)量觀測(cè)量來(lái)更新， 觀測(cè)量觀測(cè)量必須是待估參數(shù)的函數(shù)待估參數(shù)的函數(shù)，但是在給定的時(shí)刻，不要求觀測(cè)量能夠唯一確定當(dāng)時(shí)的參數(shù)值。1.2 概述目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KF

3、Schmidt KF自適應(yīng) KF平滑算法6uKalman濾波是一種遞推線遞推線性最小方差估計(jì)性最小方差估計(jì)在提供的初始估計(jì)基礎(chǔ)上，卡爾曼濾波通過(guò)遞歸遞歸運(yùn)算，用先驗(yàn)值先驗(yàn)值和最新觀測(cè)數(shù)據(jù)最新觀測(cè)數(shù)據(jù)的加權(quán)平均來(lái)更新?tīng)顟B(tài)估計(jì)（老息老息+ +新息新息）。非遞歸算法（如標(biāo)準(zhǔn)最小二乘）中沒(méi)有先驗(yàn)估計(jì)，估計(jì)結(jié)果由全部觀測(cè)數(shù)據(jù)計(jì)算而來(lái)（新息新息） 。u卡爾曼濾波是一種貝葉斯估計(jì)目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法最小方差估計(jì)線性最小方差估計(jì)遞推線性最小方差估計(jì)71.3 卡爾曼濾波的要素和流程實(shí)際系統(tǒng)系統(tǒng)模型觀測(cè)模型觀測(cè)向量及其協(xié)方差狀態(tài)向量及其協(xié)方差卡爾曼濾波算法（實(shí)

4、線表示數(shù)據(jù)流一直有，虛線表示只在某些應(yīng)用中有，Ref：Paul Groves）目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法8u 狀態(tài)向量（狀態(tài)）是一組描述系統(tǒng)的參數(shù)。可以是常量，也可是時(shí)變量，是估計(jì)對(duì)象。與之相關(guān)聯(lián)的是誤差協(xié)方差矩陣，描述了狀態(tài)估計(jì)的不確定度及估計(jì)誤差間的相關(guān)度。目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法91.4 卡爾曼濾波的要素4個(gè)要素：2個(gè)模型、1組觀測(cè)量、1個(gè)算法 p 2 2個(gè)模型個(gè)模型u系統(tǒng)模型系統(tǒng)模型也稱過(guò)程模型或者時(shí)間傳遞模型，描述了狀態(tài)與誤差協(xié)方差矩陣隨時(shí)間的變化特性。對(duì)于選定狀態(tài)量，系統(tǒng)模型是確定的。

5、u觀測(cè)模型描述了觀測(cè)向量與狀態(tài)向量間的函數(shù)關(guān)系。目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法10p 1 1組觀測(cè)組觀測(cè)向量向量是一組針對(duì)同一時(shí)刻的系統(tǒng)特性的測(cè)量值，例如觀測(cè)量可以包括GNSS系統(tǒng)的位置測(cè)量值，或者INS與GNSS位置結(jié)果的差值。p 1 1個(gè)算法個(gè)算法：卡爾曼濾波算法使用觀測(cè)向量、觀測(cè)模型和系統(tǒng)模型來(lái)獲得狀態(tài)向量的最優(yōu)估計(jì)，分為系統(tǒng)傳遞和測(cè)量更新兩個(gè)部分。目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法目目 錄錄111.5 卡爾曼濾波的導(dǎo)航應(yīng)用p慣性導(dǎo)航系統(tǒng)（INS）的精對(duì)準(zhǔn)和標(biāo)定p單一導(dǎo)航（GNSS, 無(wú)線電、水聲學(xué)、匹配

6、）p組合導(dǎo)航INS/GNSS組合導(dǎo)航及多傳感器組合導(dǎo)航INS/水聲組合導(dǎo)航INS/匹配導(dǎo)航p 概述 經(jīng)典KF EKF LKF二、二、KalmanKalman濾波濾波2021-11-2612132.1 卡爾曼濾波方程1. 離散系統(tǒng)的數(shù)學(xué)描述u 設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測(cè)方程分別為： kk,k 1k 1k 1k 1kkkkXXWZH XV目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法14p要求WWk k和V Vk k是互不相關(guān)的、零均值白噪聲序列：TkjkkjTkjkkjE W WQE V VRp Q Qk k和R Rk k分別稱為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，分別

7、是已知值的非負(fù)定陣和正定陣； kj0(kj)1(kj)p k k j j 是Kronecker 函數(shù)，即：目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法15u 初始狀態(tài)的一、二階統(tǒng)計(jì)特性為：0 x0E Xm0 x0Var XCu Var 為對(duì)求方差的符號(hào)u卡爾曼濾波要求mx0和Cx0為已知量，u且要求X0與Wk和Vk都不相關(guān)目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法162. 離散卡爾曼濾波方程k/k 1k,k 1k 1XX-= p 狀態(tài)狀態(tài)一步預(yù)測(cè)方程一步預(yù)測(cè)方程kk/k 1kkkk/k 1XXK (ZH X)p 狀態(tài)狀態(tài)估值計(jì)算方程

8、估值計(jì)算方程TT1kk/k 1kkk/k 1kkKPH (H PHR )p 濾波濾波增益方程增益方程TTk/k 1k,k 1 k 1k,k 1k 1k 1k 1PPQ-=+ GGp 一一步預(yù)測(cè)均方差方程步預(yù)測(cè)均方差方程TTkkkk/k 1kkkkkP(IK H )P(IK H )K R Kp 估計(jì)估計(jì)均方差方程均方差方程kkkk/k 1P(IK H )P或 目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法17時(shí)間更新方程量測(cè)修正方程k/k 1k,k 1k 1XXkk/k 1kkkk/k 1XXK (ZH X)TT1kk/k 1kkk/k 1kkKPH (H PHR )

9、TTk/k 1k,k 1k 1 k,k 1k 1k 1k 1PPQTTkkkk/k 1kkkkkP(IK H )P(IK H )K R K目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法18TT1kk/k 1kk k/k 1kkKPH (H PHR )TTk/k 1k,k 1 k 1k,k 1k 1k 1k 1PPQTTkkkk/k 1kkkkkP(IK H )P(IK H )K R Kk 1Pkk1k,k 1Tk 1k 1k 1QkRkHkRkHk/k 1PkKk,k 1k/k 1k,k 1k 1XXkk/k 1kkkk/k 1XXK (ZH X)k 1Xkk1kX

10、kZkP濾波計(jì)算回路增益計(jì)算回路目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法193. 卡爾曼濾波示例目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法有一個(gè)質(zhì)點(diǎn)，沿X軸正方向運(yùn)動(dòng)，質(zhì)點(diǎn)從X = 0 開(kāi)始勻速直線運(yùn)動(dòng), 速度為V = 10m/s，則每一時(shí)刻質(zhì)點(diǎn)的真實(shí)位置（參考真值）為：X = X0+V * t；實(shí)際上，我們每隔 0.1s 可以測(cè)量一次質(zhì)點(diǎn)的位置，但位置測(cè)量值存在誤差（假設(shè)是均值為0的白噪聲序列）根據(jù)我們對(duì)質(zhì)點(diǎn)的位置觀測(cè)量，用卡爾曼濾波方法計(jì)算每一時(shí)刻質(zhì)點(diǎn)的位置和速度Xv = 10 m/sXi-1XiXi+1位置觀測(cè)值20目目

11、錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法l狀態(tài)量 x = X, V，即以質(zhì)點(diǎn)的位置和速度作為卡爾曼濾波狀態(tài)量；l系統(tǒng)狀態(tài)方程為 Xk = Xk-1 + Vk-1 * dt ；l狀態(tài)轉(zhuǎn)移矩陣 Phi = 1 dt; 0 1；l系統(tǒng)噪聲協(xié)方差陣Q： 即系統(tǒng)模型的不確定度，由于假設(shè)模型即質(zhì)點(diǎn)運(yùn)動(dòng)模型，因此可認(rèn)為模型的不確定度為0，即 Q = 0 0; 0 0l觀測(cè)矩陣 H：由于只觀測(cè)了質(zhì)點(diǎn)位置，未觀測(cè)速度，因此觀測(cè)矩陣 H = 1 0;l觀測(cè)噪聲矩陣R：位置觀測(cè)量的方差為 m2，即 R = 1l觀測(cè)量向量Z：在真實(shí)狀態(tài)（真實(shí)位置）加上均值為零，方差為 m2的白噪聲;l卡爾

12、曼濾波初始狀態(tài)：X0 = 0, V0 = 5 m/s，初始狀態(tài)誤差協(xié)方差矩陣P = 1 0; 0 1p 設(shè)計(jì)卡爾曼濾波21目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法卡爾曼濾波位置估計(jì)012345678910020406080100120時(shí)間 (s)位置 (m) 參考真值測(cè)量值卡爾曼濾波估值22目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法卡爾曼濾波速度估計(jì)0246810-30-20-1001020304050時(shí)間 (s)速度 (m/s) 參考真值位置觀測(cè)量微分滑動(dòng)平均法卡爾曼濾波232.2 閉環(huán)卡爾曼濾波1. 全狀態(tài)濾波和誤差狀態(tài)濾波

13、根據(jù)卡爾曼濾波狀態(tài)向量的選取不同，卡爾曼濾波可分為: 全狀態(tài)卡爾曼濾波（Total State Implementation）和 誤差狀態(tài)卡爾曼濾波（Error State Implementation）組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進(jìn)行估計(jì)的主要對(duì)象位置，L 速度VX ,VY ,VZ姿態(tài),導(dǎo)航參數(shù)用X表示目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法24Total State：以系統(tǒng)的固有屬性，如位置、速度和姿態(tài)等，為狀態(tài)向量的卡爾曼濾波，稱為全狀態(tài)濾波或直接卡爾曼濾波Error State：以系統(tǒng)測(cè)量誤差值，如INS位置、速度和姿態(tài)等，為狀態(tài)向量的卡爾曼濾波稱為誤差

14、狀態(tài)濾波或間接卡爾曼濾波直接法直接法間接法間接法以各種導(dǎo)航參數(shù)X為主要狀態(tài)濾波器估值的主要部分即是導(dǎo)航參數(shù)的估值以某種導(dǎo)航系統(tǒng)輸出導(dǎo)航參數(shù)的誤差為主要狀態(tài)濾波器估值的主要部分即是導(dǎo)航參數(shù)誤差的估值目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法模型可能是線性的，也可能是非線性的模型一般都是線性的252. 開(kāi)環(huán)卡爾曼濾波25慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-XIXINXXD- DNXIX用導(dǎo)航參數(shù)誤差的估值 去校正系統(tǒng)輸出的導(dǎo)航參數(shù)，得到綜合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估值XX目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法開(kāi)環(huán)（輸出校正）的卡爾曼濾波

15、器263. 閉環(huán)卡爾曼濾波26慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)IXINXXD- DNXIXIXIX采用反饋校正的間接法估計(jì)，是將慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)誤差 的估值 反饋到慣導(dǎo)系統(tǒng)內(nèi)，對(duì)誤差狀態(tài)進(jìn)行校正。IXIX目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法閉環(huán)（反饋校正）的卡爾曼濾波器2727 開(kāi)環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩種校正方法的性質(zhì)是一樣的，具有同樣的精度。 閉環(huán)濾波的反饋校正使得卡爾曼濾波狀態(tài)值為小量；開(kāi)環(huán)因無(wú)反饋，狀態(tài)值會(huì)隨時(shí)間不斷變大。狀態(tài)方程都是經(jīng)過(guò)一階近似的線性方程，狀態(tài)的數(shù)值越小，則近似的準(zhǔn)確性越高，因此，利用狀態(tài)反饋校正

16、的系統(tǒng)狀態(tài)方程，更能接近真實(shí)地反映系統(tǒng)誤差狀態(tài)的動(dòng)態(tài)過(guò)程。 卡爾曼濾波算法中，反饋狀態(tài)估計(jì)的最佳時(shí)機(jī)是在測(cè)量更新后立即進(jìn)行。 卡爾曼濾波的閉環(huán)和開(kāi)環(huán)可以混合使用，即一些狀態(tài)估計(jì)作為校正值被反饋，而另外一些不反饋。目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法4. 開(kāi)環(huán)與閉環(huán)卡爾曼濾波對(duì)比285. 混合卡爾曼濾波示例28在松組合算法中，21維向量，其中： 位置、速度和姿態(tài)只做開(kāi)環(huán)修正； 而IMU誤差，如陀螺和加速度計(jì)零偏，比例因子誤差進(jìn)行反饋，修正IMU的原始觀測(cè)值。目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法GINS軟件松組合算法架構(gòu)示

17、意圖（混合濾波）292.3 濾波特性及濾波實(shí)現(xiàn)中的問(wèn)題1. 濾波收斂特性初始不確定度平衡不確定度時(shí)間狀態(tài)不確定度收斂過(guò)程中的卡爾曼濾波狀態(tài)不確定度* 注：狀態(tài)不確定度是誤差協(xié)方差矩陣P對(duì)角元素的平方根 當(dāng)卡爾曼濾波狀態(tài)不確定度接近平衡點(diǎn)，每次測(cè)量更新后狀態(tài)不確定度的降低量與系統(tǒng)噪聲造成的不確定度的增加量是匹配的； 在平衡點(diǎn)，不確定度所反映出的估計(jì)置信度水平基本固定.目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法30 狀態(tài)估計(jì)的狀態(tài)估計(jì)的收斂速度基本上取決于該狀態(tài)的可觀測(cè)性收斂速度基本上取決于該狀態(tài)的可觀測(cè)性。如果觀測(cè)矩陣隨時(shí)間變化或者狀態(tài)之間通過(guò)狀態(tài)轉(zhuǎn)移矩陣存在時(shí)間

18、依存關(guān)系，那么隨著迭代次數(shù)的增加，更多的狀態(tài)量變得可觀測(cè)。例例：導(dǎo)航中用位置的變化率來(lái)確定速度 許多參數(shù)的可觀測(cè)性依賴于系統(tǒng)的動(dòng)態(tài)性許多參數(shù)的可觀測(cè)性依賴于系統(tǒng)的動(dòng)態(tài)性例：例：姿態(tài)不變時(shí)，INS姿態(tài)誤差和加速度計(jì)偏差不是獨(dú)立可觀測(cè)的；陀螺儀誤差則需要載體有更高的動(dòng)態(tài)性，方可觀測(cè) 如果兩個(gè)狀態(tài)對(duì)觀測(cè)量有同樣的影響，以相同方式隨時(shí)間變化，并且具有相同的動(dòng)態(tài)特性，則它們非獨(dú)立可觀量，在濾波設(shè)計(jì)時(shí)，應(yīng)將其組合在一起，以免浪費(fèi)計(jì)算資源目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法312. 濾波參數(shù)調(diào)整狀態(tài)估計(jì)誤差P/R太小狀態(tài)估計(jì)誤差狀態(tài)估計(jì)誤差P/R適中P/R太大時(shí)間時(shí)間時(shí)

19、間目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法不同P/R比率下的卡爾曼濾波誤差傳遞（Paul Groves）為克服卡爾曼濾波模型的限制，噪聲建模必須足以囊括實(shí)際系統(tǒng)的行為，形象地說(shuō)，要將真實(shí)世界的“方形楔子”放到卡爾曼濾波模型的“圓洞”中，因此這個(gè)洞要被擴(kuò)寬 。 例：例：若忽略1 Hz GPS定位結(jié)果誤差的時(shí)間相關(guān)性，應(yīng)將其放大（一般為2-3倍）以建模為白噪聲32 P陣非正定P陣的非正定容易導(dǎo)致濾波發(fā)散。l使用高精度變量存儲(chǔ)（如double類型），減小舍入誤差；l縮放卡爾曼濾波標(biāo)度，使所有狀態(tài)不確定度在數(shù)值上具有相同量級(jí)； P陣非對(duì)稱使用式 計(jì)算P陣，容易導(dǎo)致P陣

20、非對(duì)稱；l每次系統(tǒng)傳遞及測(cè)量更新后通過(guò) P=(P+P)/2來(lái)保持對(duì)稱性；l建議采用Joseph式P陣更新： 平方根濾波傳遞 而非P,可把動(dòng)態(tài)范圍減小兩個(gè)量級(jí)，從而減小舍入誤差影響。kkkk/k 1P(IK H )P目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法TTkkkk/k 1kkkkkP(IK H )P(IK H )K R K3. 數(shù)值計(jì)算問(wèn)題三、擴(kuò)展三、擴(kuò)展KalmanKalman濾波濾波（EKFEKF）2021-11-263334u 標(biāo)準(zhǔn)卡爾曼濾波的線性假設(shè) 在標(biāo)準(zhǔn)的卡爾曼濾波中，觀測(cè)模型觀測(cè)模型假設(shè)為線性（Z Z是X X的線性函數(shù)），但實(shí)際情況往往并非如此

21、（如GNSS導(dǎo)航濾波器中，觀測(cè)模型是強(qiáng)非線性的） 在標(biāo)準(zhǔn)卡爾曼濾波中，系統(tǒng)模型系統(tǒng)模型也被假設(shè)為線性的（X的時(shí)間導(dǎo)數(shù)是X的線性函數(shù)）問(wèn)題：?jiǎn)栴}：在全狀態(tài)INS/GNSS組合導(dǎo)航中，狀態(tài)量為絕對(duì)位置、速度和姿態(tài)，因?yàn)殡y以一直保持系統(tǒng)的線性近似，完成所有的系統(tǒng)反饋并不總是可行的；u 擴(kuò)展擴(kuò)展/ /線性化線性化卡爾曼濾波 為卡爾曼濾波的非線性形式目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法1. 問(wèn)題描述3.1 非線性系統(tǒng)35目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法2. 非線性系統(tǒng)描述u一般非線性系統(tǒng)（連續(xù)）和離散系統(tǒng)可由以下方程來(lái)表示：

22、u擴(kuò)展卡爾曼濾波研究的非線性系統(tǒng)可由以下方程來(lái)表示( )( ),( ), ( )( ), ( ), X tf X t W t tZ th X t V t t= &或11,1, kkkkkkXf XWkZh X V k-=-= 為簡(jiǎn)化問(wèn)題，需對(duì)噪聲的統(tǒng)計(jì)特性給以符合實(shí)際又便于處理的假定或( )( ), ( )( ),( )( )X tf X t tZ thG t W tV tX t t=+&111,1, kkkkkkkXf XkZh XkWV-=-=+ G+ Wt或Wk-1，Vt或Vk 均為彼此不相關(guān)的零均值白噪聲序列，與初始狀態(tài)X(0)或X0也不相關(guān) 36基本假設(shè)： 非線性微分

23、方程的理論解一定存在； 理論解與實(shí)際解差能夠用一個(gè)線性微分方程表示，即線性線性擾動(dòng)擾動(dòng)方程方程。目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法3. 解決方案373.2 非線性系統(tǒng)的線性化t實(shí)際狀態(tài) X標(biāo)稱狀態(tài) X*n圍繞X*線性化： 線性化卡爾曼濾波（Linearized Kalman Filter， ）n圍繞實(shí)際狀態(tài)X（濾波估計(jì)狀態(tài)，或?qū)嶋H軌跡）進(jìn)行線性化：擴(kuò)展卡爾曼濾波（Extended Kalman Filter）目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法泰勒展開(kāi)法進(jìn)行線性化狀態(tài)38n線性化卡爾曼濾波：圍繞標(biāo)稱狀態(tài)對(duì)非線性系統(tǒng)進(jìn)

24、行線性化或n當(dāng)WWt t或WWk-1k-1，V Vt t或V Vk k恒為0時(shí)，系統(tǒng)模型的解稱為非線性方程的理論解理論解，又稱“標(biāo)稱軌跡”或標(biāo)稱狀態(tài)。通常記為X Xn n(t)(t)或X Xk kn n, ,和Z Zn n(t)(t)或Z Zk kn n，則有：( )( ), ( )( ), nnnnXtf Xt tZth Xt t1,1, nnkknkkXf XkZh Xk目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法393.3 線性化卡爾曼濾波或u 定義：非線性系統(tǒng)的真軌跡運(yùn)動(dòng)與標(biāo)稱軌跡運(yùn)動(dòng)的偏差為：( )( )( )( )( )( )nnX tX tXtZ t

25、Z tZtnkkknkkkXXXZZZn如果這些偏差足夠小，那么，可以圍繞標(biāo)稱狀態(tài)圍繞標(biāo)稱狀態(tài)把X(t）和Z(t）展開(kāi)成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。( )( )( )( )( )( )( )( )( ), ( )( ), ( )( )( )( )( ), ( )( ), ( )( )( )nnx txtx txtnnx txtx txtf X t tX tf X t tX tG t W tX th X t tZ th X t tX tV tX t目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法403.3 線性化卡爾曼濾波或n則有 ：( )( )( )

26、( )( )( )( )( )( )( )( )nnnnX tXtFtX tG t W tZ tZtHtX tV t=+ D+=+ D+ &( )( )( )( )( )( )( )( )( )nnX tFtX tG t W tZ tHtX tV tD= D+D= D+ &目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法( )( )( )( )( )( )nnX tX tXtZ tZ tZtD=-D=- n由于線性化卡爾曼濾波存在上述問(wèn)題，改用另一種近似方法，即在最優(yōu)化狀態(tài)估計(jì)最優(yōu)化狀態(tài)估計(jì) 或 附近進(jìn)行泰勒展開(kāi)，線性化。413.4 擴(kuò)展卡爾曼濾波n標(biāo)

27、稱解難以解算n真軌跡與標(biāo)稱軌跡之間的狀態(tài)差X(t)或Xk不能確保其足夠小，從而導(dǎo)致線性化誤差較大，模型的線性近似度變?nèi)酢LKF的缺陷kX( )X t目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法目目 錄錄42 概述 KF方程 EKF LKF或u 定義非線性系統(tǒng)的真軌跡運(yùn)動(dòng)與實(shí)際軌跡運(yùn)動(dòng)的偏差為：( )( )( )( )( )( )X tX tX tZ tZ tZ t=-=-kkkkkkXXXZZZ=- =- n若偏差足夠小，則可以圍繞最優(yōu)化狀態(tài)估計(jì)最優(yōu)化狀態(tài)估計(jì)把X(t）和Z(t）展開(kāi)成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。( )( )( )( )( )(

28、 )( )( )( ), ( )( ), ( )( )( )( )( ), ( )( ), ( )( )( )x tX tx tX tx tX tx tX tf X t tX tf X t tX tG t W tX th X t tZ th X t tX tV tX t=+=+&43或n則有 ：( )( )( )( )( )( )( )( )( )( )( )X tX tF tX tG t W tZ tZ tH tX tV t=+=+ &()()( )( )( )( )( )( )( )( )( )X tF tX tG t W tZ tH tX tV t=+=+nEKF假設(shè)狀態(tài)

29、向量估計(jì)的誤差遠(yuǎn)比狀態(tài)向量本身小，因此可用線性系統(tǒng)模型計(jì)算狀態(tài)向量殘差。n標(biāo)準(zhǔn)的誤差協(xié)方差傳遞公式可采用在最最優(yōu)狀態(tài)估計(jì)優(yōu)狀態(tài)估計(jì)附近進(jìn)行線性化的狀態(tài)轉(zhuǎn)移矩陣，即()1111( )expkkkxxkf xFFx-=-=目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法四四、SchmidtSchmidt卡爾曼濾波卡爾曼濾波2021-11-2644454.1 時(shí)間相關(guān)噪聲目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法1. 觀測(cè)噪聲u 標(biāo)準(zhǔn)卡爾曼濾波的觀測(cè)噪聲假設(shè)一般假設(shè)所有的觀測(cè)噪聲是時(shí)間不相關(guān)的，即觀測(cè)噪聲是白噪聲。u 問(wèn)題：該假設(shè)常不成立；卡

30、爾曼濾波強(qiáng)制把新息中的時(shí)間相關(guān)部分歸因狀態(tài)，因此，時(shí)間相關(guān)的觀測(cè)噪聲可能會(huì)破壞狀態(tài)估計(jì)。u 處理觀測(cè)噪聲時(shí)間相關(guān)性問(wèn)題的方法 將時(shí)間相關(guān)噪聲擴(kuò)展至卡爾曼濾波的狀態(tài)向量中，進(jìn)行估計(jì)； 放大觀測(cè)噪聲協(xié)方差矩陣R，進(jìn)而減小卡爾曼濾波的增益； 利用Schmidt卡爾曼濾波46目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法2. 系統(tǒng)噪聲u 標(biāo)準(zhǔn)卡爾曼濾波的系統(tǒng)噪聲假設(shè)標(biāo)準(zhǔn)的卡爾曼濾波中，一般假設(shè)所有的系統(tǒng)噪聲是時(shí)間不相關(guān)的，即觀測(cè)噪聲是白噪聲。u 問(wèn)題：系統(tǒng)常含有顯著的系統(tǒng)性噪聲和其它時(shí)間相關(guān)噪聲；這些噪聲可能由于可觀測(cè)性較差未被選為狀態(tài)量；但會(huì)影響被估計(jì)的狀態(tài)u 處理系統(tǒng)噪

31、聲時(shí)間相關(guān)性問(wèn)題的方法 當(dāng)相關(guān)時(shí)間較短時(shí)：建模為白噪聲，但需覆蓋會(huì)影響卡爾曼濾波收斂的相關(guān)噪聲； 當(dāng)相關(guān)時(shí)間超過(guò)1min：采用帶不確定參數(shù)的Schmidt卡爾曼濾波；474.2 Schmidt卡爾曼濾波目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法48目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法Schmidt 卡爾曼率濾波對(duì)應(yīng)的誤差協(xié)方差系統(tǒng)傳遞式為:將誤差協(xié)方差P, 相關(guān)噪聲協(xié)方差W, 相關(guān)矩陣U分解為不同傳遞方程:49目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法類似于觀測(cè)矩陣H, 定義J 為待估計(jì)參數(shù)到

32、觀測(cè)向量的耦合矩陣,則卡爾曼濾波增益變?yōu)?Schmidt 卡爾曼濾波誤差協(xié)方差, 相關(guān)噪聲和相關(guān)矩陣更新為:50目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法五、自適應(yīng)卡爾曼濾波五、自適應(yīng)卡爾曼濾波2021-11-2651525. 自適應(yīng)卡爾曼濾波目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法1. 問(wèn)題描述多數(shù)應(yīng)用中，Kalman濾波中的系統(tǒng)噪聲協(xié)方差陣Q和觀測(cè)噪聲協(xié)方差陣R可事先通過(guò)系統(tǒng)測(cè)量、仿真和實(shí)驗(yàn)等方法確定。例： 在INS/GNSS組合導(dǎo)航中，Q陣常由IMU的性能參數(shù)如VRW, ARW，零偏的一階高斯馬爾可夫過(guò)程參數(shù)確定 GNSS

33、噪聲由位置的點(diǎn)位中誤差或誤差建模u 問(wèn)題：有些情況上述參數(shù)是無(wú)法獲取的，例如：1）MEMS IMU在出廠時(shí)未經(jīng)過(guò)嚴(yán)格標(biāo)定；2）如果沒(méi)有飛機(jī)武器掛倉(cāng)的先驗(yàn)振動(dòng)環(huán)境信息，則無(wú)法獲得觀測(cè)噪聲協(xié)方差陣；u 解決辦法Kalman濾波自行估計(jì)矩陣Q或/和R，即自適應(yīng)卡爾曼濾波基于新息的自適應(yīng)估計(jì)（Innovation Based Adaptive Estimation, IAE），要從測(cè)量新息統(tǒng)計(jì)中計(jì)算Q、R。 計(jì)算最后n個(gè)測(cè)量信息的協(xié)方差 上述協(xié)方差矩陣C用于計(jì)算R和/或Q當(dāng)處理第一組觀測(cè)新息統(tǒng)計(jì)值時(shí)，必須提供R和Q初始值，初始值的選定須謹(jǐn)慎53目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適

34、應(yīng) KF平滑算法2. 基于新息的自適應(yīng)估計(jì),1kTjjz kjknCzzn-=-=%,/1,Tkkkz kTkkk kkz kQK CKRCH PH-=-%多模型自適應(yīng)估計(jì)（Multiple Model Adaptive Estimation, MMAE）利用一組并行的卡爾曼濾波器進(jìn)行計(jì)算，每一個(gè)濾波器對(duì)應(yīng)于不同的系統(tǒng)噪聲和/或觀測(cè)噪聲協(xié)方差矩陣Q和R。 第i個(gè)卡爾曼濾波器被分配的概率為 隨時(shí)間推移，最優(yōu)濾波假設(shè)的概率會(huì)接近1，而其它的接近0； 為充分利用計(jì)算處理能力，可剔除弱的濾波假設(shè)，并周期性地細(xì)分最強(qiáng)的假設(shè)用于精化濾波器參數(shù)。54目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適

35、應(yīng) KF平滑算法3. 多模型自適應(yīng)估計(jì)完整的狀態(tài)向量估計(jì)和誤差協(xié)方差計(jì)算公式如下：u IAE（新息） 和 MMAE（多模型）方法比較 MMAE計(jì)算量大 IAE中，Q,R,P和濾波增益均可能是狀態(tài)估計(jì)的函數(shù)；而他們?cè)贛MAE濾波器組（標(biāo)準(zhǔn)卡爾曼濾波而非EKF）中是相互獨(dú)立的，因此MMAE濾波器更趨于穩(wěn)定55目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法六六、平滑算法、平滑算法2021-11-2656576. 6. 平滑算法平滑算法目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法1. 問(wèn)題描述u 實(shí)時(shí)處理/后處理差異卡爾曼濾波器是為實(shí)時(shí)應(yīng)用而設(shè)

36、計(jì)的，用于估計(jì)給定時(shí)刻的系統(tǒng)特性，所用新息是這一時(shí)刻之前的系統(tǒng)能夠觀測(cè)量。當(dāng)需要在某時(shí)間發(fā)生后獲得系統(tǒng)特性時(shí)，卡爾曼濾波仍舊只用1/2的觀測(cè)新息，因?yàn)榭柭鼮V波器沒(méi)有使用所關(guān)注時(shí)間點(diǎn)之后的觀測(cè)量u 卡爾曼平滑器是卡爾曼濾波器的擴(kuò)展，不僅適用所關(guān)注時(shí)間點(diǎn)之前的觀測(cè)新息，且適用之后的新息；非實(shí)時(shí)應(yīng)用中，平滑處理能夠得到更高的精度。u 常用平滑算法包含兩類主要方法：1）正向-方向?yàn)V波；2）RTS平滑算法58目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法2. 正向-反向?yàn)V波算法通過(guò)正向?yàn)V波和反向?yàn)V波（兩個(gè)相互獨(dú)立的濾波器）結(jié)果的加權(quán)平均，平滑結(jié)果由下式給定：下標(biāo)f和b分別表

37、示正向和反向；k表示兩個(gè)濾波器中的相同時(shí)間點(diǎn)正向-反向卡爾曼平滑算法的狀態(tài)不確定度正向?yàn)V波反向?yàn)V波狀態(tài)不確定度時(shí)間前向?yàn)V波59目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法3. RTS平滑算法：RTS（Rauch,Tung and Striebel, RTS）傳統(tǒng)的卡爾曼濾波隨時(shí)間向前運(yùn)行，在每次系統(tǒng)傳遞和測(cè)量更新后記錄狀態(tài)向量X、誤差協(xié)方差陣P及狀態(tài)轉(zhuǎn)移矩陣 ，一旦運(yùn)算到數(shù)據(jù)的結(jié)尾，則開(kāi)始反向從末尾到起始點(diǎn)進(jìn)行數(shù)據(jù)平滑 每次迭代的平滑增益為： 平滑后的狀態(tài)向量和誤差協(xié)方差陣為：當(dāng)需對(duì)每個(gè)點(diǎn)進(jìn)行平滑每個(gè)點(diǎn)進(jìn)行平滑時(shí)，RTS算法更有效，若只需對(duì)單點(diǎn)單點(diǎn)進(jìn)行平滑時(shí)，正向

38、-反向算法更有效陳起金 | 卡爾曼濾波60附錄：卡爾曼濾波示例MATLAB源代碼目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法參考參考:Alex Blekhman, http:/ Set true trajectory Nsamples=100;dt = .1;t=0:dt:dt*Nsamples;Vtrue = 10; % Xtrue is a vector of true positions of the train Xinitial = 0;Xtrue = Xinitial + Vtrue * t; % Motion equations% Previous s

39、tate (initial guess): Our guess is that the train starts at 0 with velocitythat equals to 50% of the real velocityXk_prev = 0; 0.5*Vtrue; % Current state estimateXk=;陳起金 | 卡爾曼濾波61附錄：卡爾曼濾波示例MATLAB源代碼目目 錄錄概述標(biāo)準(zhǔn) KF擴(kuò)展 KFSchmidt KF自適應(yīng) KF平滑算法Phi = 1 dt; 0 1;sigma_model = 1;P = sigma_model2 0; 0 sigma_model2;Q = 0 0; 0 0; % M is the measurement matrix. M = 1 0;sigma_meas = 1; % 1 m/secR = sigma_meas2; % = Kalman iteration% Buffers for later displayXk_buffer = zeros(2,Nsamples