卡爾曼濾波專(zhuān)題培訓(xùn)課件

：卡爾曼濾波：卡爾曼濾波目

錄一.概述二.標(biāo)準(zhǔn)卡爾曼濾波卡爾曼濾波方程閉環(huán)卡爾曼濾波卡爾曼濾波特性及實(shí)現(xiàn)中的問(wèn)題三.擴(kuò)展卡爾曼濾波非線(xiàn)性系統(tǒng)線(xiàn)性化卡爾曼濾波擴(kuò)展卡爾曼濾波四.Schmidt卡爾曼濾波五.自適應(yīng)卡爾曼濾波六.平滑算法2目 錄一.概述2一、概述2022/12/303一、概述2022/12/2731.1RudolfEmilKalmanBorn1930inHungary

BSandMSfromMITPhD1957fromColumbiaFilterdevelopedin1960-61目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法KalmanRE.Anewapproachtolinearfilteringandpredictionproblems[J].JournalofFluidsEngineering,1960,82(1):35-45.（引用：18083）41.1RudolfEmilKalmanBorn193Kalman濾波是一種最優(yōu)估計(jì)算法，而非濾波器能夠?qū)崟r(shí)估計(jì)系統(tǒng)中的參數(shù)（如連續(xù)變化的位置、速度等信息）。估計(jì)量通過(guò)一系列受噪聲污染的觀測(cè)量來(lái)更新，觀測(cè)量必須是待估參數(shù)的函數(shù)，但是在給定的時(shí)刻，不要求觀測(cè)量能夠唯一確定當(dāng)時(shí)的參數(shù)值。1.2概述目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法5Kalman濾波是一種最優(yōu)估計(jì)算法，而非濾波器1.2概述目Kalman濾波是一種遞推線(xiàn)性最小方差估計(jì)在提供的初始估計(jì)基礎(chǔ)上，卡爾曼濾波通過(guò)遞歸運(yùn)算，用先驗(yàn)值和最新觀測(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)（新息）。卡爾曼濾波是一種貝葉斯估計(jì)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法最小方差估計(jì)線(xiàn)性最小方差估計(jì)遞推線(xiàn)性最小方差估計(jì)6Kalman濾波是一種遞推線(xiàn)性最小方差估計(jì)目錄最小1.3卡爾曼濾波的要素和流程實(shí)際系統(tǒng)系統(tǒng)模型觀測(cè)模型觀測(cè)向量及其協(xié)方差狀態(tài)向量及其協(xié)方差卡爾曼濾波算法（實(shí)線(xiàn)表示數(shù)據(jù)流一直有，虛線(xiàn)表示只在某些應(yīng)用中有，Ref：PaulGroves）目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法71.3卡爾曼濾波的要素和流程實(shí)際系統(tǒng)系統(tǒng)模型觀測(cè)模型觀測(cè)向

狀態(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ò)展KFSchmidtKF自適應(yīng)KF平滑算法8狀態(tài)向量（狀態(tài)）目錄81.4卡爾曼濾波的要素4個(gè)要素：2個(gè)模型、1組觀測(cè)量、1個(gè)算法2個(gè)模型系統(tǒng)模型也稱(chēng)過(guò)程模型或者時(shí)間傳遞模型，描述了狀態(tài)與誤差協(xié)方差矩陣隨時(shí)間的變化特性。對(duì)于選定狀態(tài)量，系統(tǒng)模型是確定的。觀測(cè)模型描述了觀測(cè)向量與狀態(tài)向量間的函數(shù)關(guān)系。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法91.4卡爾曼濾波的要素4個(gè)要素：2個(gè)模型、1組觀測(cè)量、1個(gè)1組觀測(cè)向量是一組針對(duì)同一時(shí)刻的系統(tǒng)特性的測(cè)量值，例如觀測(cè)量可以包括GNSS系統(tǒng)的位置測(cè)量值，或者INS與GNSS位置結(jié)果的差值。1個(gè)算法：卡爾曼濾波算法使用觀測(cè)向量、觀測(cè)模型和系統(tǒng)模型來(lái)獲得狀態(tài)向量的最優(yōu)估計(jì)，分為系統(tǒng)傳遞和測(cè)量更新兩個(gè)部分。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法101組觀測(cè)向量目錄10目錄1.5卡爾曼濾波的導(dǎo)航應(yīng)用慣性導(dǎo)航系統(tǒng)（INS）的精對(duì)準(zhǔn)和標(biāo)定單一導(dǎo)航（GNSS,無(wú)線(xiàn)電、水聲學(xué)、匹配）組合導(dǎo)航INS/GNSS組合導(dǎo)航及多傳感器組合導(dǎo)航INS/水聲組合導(dǎo)航INS/匹配導(dǎo)航…概述經(jīng)典KFEKFLKF11目錄1.5卡爾曼濾波的導(dǎo)航應(yīng)用慣性導(dǎo)航系統(tǒng)（IN二、Kalman濾波2022/12/3012二、Kalman濾波2022/12/27122.1卡爾曼濾波方程1.離散系統(tǒng)的數(shù)學(xué)描述設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測(cè)方程分別為：

Xk為k時(shí)刻的n維狀態(tài)向量（被估計(jì)量）Zk為k時(shí)刻的m維量測(cè)向量k-1到k時(shí)刻的系統(tǒng)一步狀態(tài)轉(zhuǎn)移矩陣（n×n階）Wk-1為k-1時(shí)刻的系統(tǒng)噪聲（r維）Γk-1為系統(tǒng)噪聲矩陣（n×r階）Hk為k時(shí)刻系統(tǒng)量測(cè)矩陣（m×n階）Vk為k時(shí)刻m維量測(cè)噪聲目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法132.1卡爾曼濾波方程1.離散系統(tǒng)的數(shù)學(xué)描述Xk為k時(shí)刻的要求{Wk}和{Vk}是互不相關(guān)的、零均值白噪聲序列：

Qk和Rk分別稱(chēng)為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，分別是已知值的非負(fù)定陣和正定陣；

δkj是Kroneckerδ函數(shù)，即：目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法14要求{Wk}和{Vk}是互不相關(guān)的、零均值白噪聲序列：Q

初始狀態(tài)的一、二階統(tǒng)計(jì)特性為：

Var{·}為對(duì){·}求方差的符號(hào)卡爾曼濾波要求mx0和Cx0為已知量，且要求X0與{Wk}和{Vk}都不相關(guān)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法15初始狀態(tài)的一、二階統(tǒng)計(jì)特性為：Var{·}為對(duì){·}求2.離散卡爾曼濾波方程狀態(tài)一步預(yù)測(cè)方程狀態(tài)估值計(jì)算方程濾波增益方程一步預(yù)測(cè)均方差方程估計(jì)均方差方程或

目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法162.離散卡爾曼濾波方程狀態(tài)一步預(yù)測(cè)方程狀態(tài)估值計(jì)算方程濾波時(shí)間更新方程量測(cè)修正方程目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法17時(shí)間更新量測(cè)修正目錄17濾波計(jì)算回路增益計(jì)算回路目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法18濾波計(jì)算回路增益計(jì)算回路目錄183.卡爾曼濾波示例目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法有一個(gè)質(zhì)點(diǎn)，沿X軸正方向運(yùn)動(dòng)，質(zhì)點(diǎn)從X=0開(kāi)始勻速直線(xiàn)運(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=10m/sXi-1XiXi+1位置觀測(cè)值193.卡爾曼濾波示例目錄有一個(gè)質(zhì)點(diǎn)，沿X軸正方向運(yùn)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法狀態(tài)量x=[X,V]，即以質(zhì)點(diǎn)的位置和速度作為卡爾曼濾波狀態(tài)量；系統(tǒng)狀態(tài)方程為Xk=Xk-1+Vk-1*dt；狀態(tài)轉(zhuǎn)移矩陣

Phi=[1dt;01]；系統(tǒng)噪聲協(xié)方差陣Q：即系統(tǒng)模型的不確定度，由于假設(shè)模型即質(zhì)點(diǎn)運(yùn)動(dòng)模型，因此可認(rèn)為模型的不確定度為0，即Q=[00;00]觀測(cè)矩陣H：由于只觀測(cè)了質(zhì)點(diǎn)位置，未觀測(cè)速度，因此觀測(cè)矩陣H=[10];觀測(cè)噪聲矩陣R：位置觀測(cè)量的方差為m2，即R=1觀測(cè)量向量Z：在真實(shí)狀態(tài)（真實(shí)位置）加上均值為零，方差為m2的白噪聲;卡爾曼濾波初始狀態(tài)：X0=0,V0=5m/s，初始狀態(tài)誤差協(xié)方差矩陣P=[10;01]設(shè)計(jì)卡爾曼濾波20目錄狀態(tài)量x=[X,V]，即以質(zhì)點(diǎn)的位置和目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波位置估計(jì)21目錄卡爾曼濾波位置估計(jì)21目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波速度估計(jì)22目錄卡爾曼濾波速度估計(jì)222.2閉環(huán)卡爾曼濾波1.全狀態(tài)濾波和誤差狀態(tài)濾波根據(jù)卡爾曼濾波狀態(tài)向量的選取不同，卡爾曼濾波可分為:全狀態(tài)卡爾曼濾波（TotalStateImplementation）和誤差狀態(tài)卡爾曼濾波（ErrorStateImplementation）組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進(jìn)行估計(jì)的主要對(duì)象導(dǎo)航參數(shù)

導(dǎo)航參數(shù)

位置λ，L

速度VX,VY,VZ姿態(tài)ψ,θ,γ導(dǎo)航參數(shù)用X表示目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法232.2閉環(huán)卡爾曼濾波1.全狀態(tài)濾波和誤差狀態(tài)濾波組合導(dǎo)航TotalState：以系統(tǒng)的固有屬性，如位置、速度和姿態(tài)等，為狀態(tài)向量的卡爾曼濾波，稱(chēng)為全狀態(tài)濾波或直接卡爾曼濾波ErrorState：以系統(tǒng)測(cè)量誤差值，如INS位置、速度和姿態(tài)等，為狀態(tài)向量的卡爾曼濾波稱(chēng)為誤差狀態(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ò)展KFSchmidtKF自適應(yīng)KF平滑算法模型可能是線(xiàn)性的，也可能是非線(xiàn)性的模型一般都是線(xiàn)性的24TotalState：以系統(tǒng)的固有屬性，如位置、速度和姿態(tài)2.開(kāi)環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-用導(dǎo)航參數(shù)誤差的估值去校正系統(tǒng)輸出的導(dǎo)航參數(shù)，得到綜合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估值目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法開(kāi)環(huán)（輸出校正）的卡爾曼濾波器2022/12/30252.開(kāi)環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-用導(dǎo)3.閉環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-采用反饋校正的間接法估計(jì)，是將慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)誤差

的估值

反饋到慣導(dǎo)系統(tǒng)內(nèi)，對(duì)誤差狀態(tài)進(jìn)行校正。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法閉環(huán)（反饋校正）的卡爾曼濾波器2022/12/30263.閉環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-采用開(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ò)一階近似的線(xiàn)性方程，狀態(tài)的數(shù)值越小，則近似的準(zhǔn)確性越高，因此，利用狀態(tài)反饋校正的系統(tǒng)狀態(tài)方程，更能接近真實(shí)地反映系統(tǒng)誤差狀態(tài)的動(dòng)態(tài)過(guò)程?？柭鼮V波算法中，反饋狀態(tài)估計(jì)的最佳時(shí)機(jī)是在測(cè)量更新后立即進(jìn)行。卡爾曼濾波的閉環(huán)和開(kāi)環(huán)可以混合使用，即一些狀態(tài)估計(jì)作為校正值被反饋，而另外一些不反饋。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法4.開(kāi)環(huán)與閉環(huán)卡爾曼濾波對(duì)比2022/12/3027開(kāi)環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩5.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：位置、速度和姿態(tài)只做開(kāi)環(huán)修正；而IMU誤差，如陀螺和加速度計(jì)零偏，比例因子誤差進(jìn)行反饋，修正IMU的原始觀測(cè)值。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法GINS軟件松組合算法架構(gòu)示意圖（混合濾波）2022/12/30285.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：目2.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ò)展KFSchmidtKF自適應(yīng)KF平滑算法292.3濾波特性及濾波實(shí)現(xiàn)中的問(wèn)題1.濾波收斂特性初始不確

狀態(tài)估計(jì)的收斂速度基本上取決于該狀態(tài)的可觀測(cè)性。如果觀測(cè)矩陣隨時(shí)間變化或者狀態(tài)之間通過(guò)狀態(tài)轉(zhuǎn)移矩陣存在時(shí)間依存關(guān)系，那么隨著迭代次數(shù)的增加，更多的狀態(tài)量變得可觀測(cè)。例：導(dǎo)航中用位置的變化率來(lái)確定速度許多參數(shù)的可觀測(cè)性依賴(lài)于系統(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ò)展KFSchmidtKF自適應(yīng)KF平滑算法30狀態(tài)估計(jì)的收斂速度基本上取決于該狀態(tài)的可觀測(cè)性。目2.濾波參數(shù)調(diào)整狀態(tài)估計(jì)誤差P/R太小狀態(tài)估計(jì)誤差狀態(tài)估計(jì)誤差P/R適中P/R太大時(shí)間時(shí)間時(shí)間目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法不同P/R比率下的卡爾曼濾波誤差傳遞（PaulGroves）為克服卡爾曼濾波模型的限制，噪聲建模必須足以囊括實(shí)際系統(tǒng)的行為，形象地說(shuō)，要將真實(shí)世界的“方形楔子”放到卡爾曼濾波模型的“圓洞”中，因此這個(gè)洞要被擴(kuò)寬。

例：若忽略1HzGPS定位結(jié)果誤差的時(shí)間相關(guān)性，應(yīng)將其放大（一般為2-3倍）以建模為白噪聲312.濾波參數(shù)調(diào)整狀態(tài)估計(jì)誤差P/R太小狀態(tài)估計(jì)誤差狀態(tài)估計(jì)

P陣非正定P陣的非正定容易導(dǎo)致濾波發(fā)散。使用高精度變量存儲(chǔ)（如double類(lèi)型），減小舍入誤差；縮放卡爾曼濾波標(biāo)度，使所有狀態(tài)不確定度在數(shù)值上具有相同量級(jí)；

P陣非對(duì)稱(chēng)使用式計(jì)算P陣，容易導(dǎo)致P陣非對(duì)稱(chēng)；每次系統(tǒng)傳遞及測(cè)量更新后通過(guò)P=(P+P’)/2來(lái)保持對(duì)稱(chēng)性；建議采用Joseph式P陣更新：

平方根濾波傳遞而非P,可把動(dòng)態(tài)范圍減小兩個(gè)量級(jí)，從而減小舍入誤差影響。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法

3.數(shù)值計(jì)算問(wèn)題32P陣非正定目錄

3.數(shù)值計(jì)算問(wèn)題32三、擴(kuò)展Kalman濾波（EKF）2022/12/3033三、擴(kuò)展Kalman濾波（EKF）2022/12/2733

標(biāo)準(zhǔn)卡爾曼濾波的線(xiàn)性假設(shè)在標(biāo)準(zhǔn)的卡爾曼濾波中，觀測(cè)模型假設(shè)為線(xiàn)性（Z是X的線(xiàn)性函數(shù)），但實(shí)際情況往往并非如此（如GNSS導(dǎo)航濾波器中，觀測(cè)模型是強(qiáng)非線(xiàn)性的）在標(biāo)準(zhǔn)卡爾曼濾波中，系統(tǒng)模型也被假設(shè)為線(xiàn)性的（X的時(shí)間導(dǎo)數(shù)是X的線(xiàn)性函數(shù)）問(wèn)題：在全狀態(tài)INS/GNSS組合導(dǎo)航中，狀態(tài)量為絕對(duì)位置、速度和姿態(tài)，因?yàn)殡y以一直保持系統(tǒng)的線(xiàn)性近似，完成所有的系統(tǒng)反饋并不總是可行的；

擴(kuò)展/線(xiàn)性化卡爾曼濾波為卡爾曼濾波的非線(xiàn)性形式目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法1.問(wèn)題描述3.1非線(xiàn)性系統(tǒng)34標(biāo)準(zhǔn)卡爾曼濾波的線(xiàn)性假設(shè)目錄1.問(wèn)題描述3.1目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.非線(xiàn)性系統(tǒng)描述一般非線(xiàn)性系統(tǒng)（連續(xù)）和離散系統(tǒng)可由以下方程來(lái)表示：擴(kuò)展卡爾曼濾波研究的非線(xiàn)性系統(tǒng)可由以下方程來(lái)表示或?yàn)楹?jiǎn)化問(wèn)題，需對(duì)噪聲的統(tǒng)計(jì)特性給以符合實(shí)際又便于處理的假定或Wt或{Wk-1}，Vt或{Vk}均為彼此不相關(guān)的零均值白噪聲序列，與初始狀態(tài)X(0)或X0也不相關(guān)35目錄2.非線(xiàn)性系統(tǒng)描述一般非線(xiàn)性系統(tǒng)（連續(xù)）和離如何解決非線(xiàn)性問(wèn)題采用近似的方法通常線(xiàn)性化方法目前應(yīng)用比較廣泛的是對(duì)非線(xiàn)性模型的線(xiàn)性化非線(xiàn)性系統(tǒng)能否應(yīng)用卡爾曼濾波的關(guān)鍵基本假設(shè)：非線(xiàn)性微分方程的理論解一定存在；理論解與實(shí)際解差能夠用一個(gè)線(xiàn)性微分方程表示，即線(xiàn)性擾動(dòng)方程。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法3.解決方案2022/12/3036如何解決非線(xiàn)性問(wèn)題采用近似的方法通常線(xiàn)性化方法目前應(yīng)用比較廣3.2非線(xiàn)性系統(tǒng)的線(xiàn)性化t實(shí)際狀態(tài)

X標(biāo)稱(chēng)狀態(tài)X*圍繞X*線(xiàn)性化：線(xiàn)性化卡爾曼濾波（LinearizedKalmanFilter，LKF）圍繞實(shí)際狀態(tài)X（濾波估計(jì)狀態(tài)，或?qū)嶋H軌跡）進(jìn)行線(xiàn)性化：擴(kuò)展卡爾曼濾波（ExtendedKalmanFilter，EKF）目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法泰勒展開(kāi)法進(jìn)行線(xiàn)性化狀態(tài)373.2非線(xiàn)性系統(tǒng)的線(xiàn)性化t實(shí)際狀態(tài)X標(biāo)稱(chēng)狀態(tài)X*圍繞X線(xiàn)性化卡爾曼濾波：圍繞標(biāo)稱(chēng)狀態(tài)對(duì)非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性化或當(dāng)Wt或{Wk-1}，Vt或{Vk}恒為0時(shí)，系統(tǒng)模型的解稱(chēng)為非線(xiàn)性方程的理論解，又稱(chēng)“標(biāo)稱(chēng)軌跡”或標(biāo)稱(chēng)狀態(tài)。通常記為Xn(t)或Xkn,和Zn(t)或Zkn，則有：目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法38線(xiàn)性化卡爾曼濾波：圍繞標(biāo)稱(chēng)狀態(tài)對(duì)非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性化或當(dāng)Wt3.3線(xiàn)性化卡爾曼濾波或定義：非線(xiàn)性系統(tǒng)的真軌跡運(yùn)動(dòng)與標(biāo)稱(chēng)軌跡運(yùn)動(dòng)的偏差為：如果這些偏差足夠小，那么，可以圍繞標(biāo)稱(chēng)狀態(tài)把X(t）和Z(t）展開(kāi)成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法393.3線(xiàn)性化卡爾曼濾波或定義：如果這些偏差足夠小，那么，3.3線(xiàn)性化卡爾曼濾波或則有：非線(xiàn)性系統(tǒng)能否應(yīng)用卡爾曼濾波的關(guān)鍵目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2022/12/30403.3線(xiàn)性化卡爾曼濾波或則有：非線(xiàn)性系統(tǒng)能否應(yīng)用卡爾曼濾由于線(xiàn)性化卡爾曼濾波存在上述問(wèn)題，改用另一種近似方法，即在最優(yōu)化狀態(tài)估計(jì)或附近進(jìn)行泰勒展開(kāi)，線(xiàn)性化。3.4擴(kuò)展卡爾曼濾波標(biāo)稱(chēng)解難以解算真軌跡與標(biāo)稱(chēng)軌跡之間的狀態(tài)差△X(t)或△Xk不能確保其足夠小，從而導(dǎo)致線(xiàn)性化誤差較大，模型的線(xiàn)性近似度變?nèi)?。LKF的缺陷目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法擴(kuò)展卡爾曼濾波2022/12/3041由于線(xiàn)性化卡爾曼濾波存在上述問(wèn)題，改用另一種近似方法，即在最目錄概述KF方程EKFLKF或定義非線(xiàn)性系統(tǒng)的真軌跡運(yùn)動(dòng)與實(shí)際軌跡運(yùn)動(dòng)的偏差為：若偏差足夠小，則可以圍繞最優(yōu)化狀態(tài)估計(jì)把X(t）和Z(t）展開(kāi)成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。2022/12/3042目錄概述或定義若偏差足夠小，則可以圍繞最優(yōu)化狀態(tài)或則有：EKF假設(shè)狀態(tài)向量估計(jì)的誤差遠(yuǎn)比狀態(tài)向量本身小，因此可用線(xiàn)性系統(tǒng)模型計(jì)算狀態(tài)向量殘差。標(biāo)準(zhǔn)的誤差協(xié)方差傳遞公式可采用在最優(yōu)狀態(tài)估計(jì)附近進(jìn)行線(xiàn)性化的狀態(tài)轉(zhuǎn)移矩陣，即目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法43或則有：EKF假設(shè)狀態(tài)向量估計(jì)的誤差遠(yuǎn)比狀態(tài)向量本身小，因四、Schmidt卡爾曼濾波2022/12/3044四、Schmidt卡爾曼濾波2022/12/27444.1時(shí)間相關(guān)噪聲目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法1.觀測(cè)噪聲

標(biāo)準(zhǔn)卡爾曼濾波的觀測(cè)噪聲假設(shè)一般假設(shè)所有的觀測(cè)噪聲是時(shí)間不相關(guān)的，即觀測(cè)噪聲是白噪聲。問(wèn)題：該假設(shè)常不成立；卡爾曼濾波強(qiáng)制把新息中的時(shí)間相關(guān)部分歸因狀態(tài)，因此，時(shí)間相關(guān)的觀測(cè)噪聲可能會(huì)破壞狀態(tài)估計(jì)。處理觀測(cè)噪聲時(shí)間相關(guān)性問(wèn)題的方法將時(shí)間相關(guān)噪聲擴(kuò)展至卡爾曼濾波的狀態(tài)向量中，進(jìn)行估計(jì)；

放大觀測(cè)噪聲協(xié)方差矩陣R，進(jìn)而減小卡爾曼濾波的增益；利用Schmidt卡爾曼濾波2022/12/30454.1時(shí)間相關(guān)噪聲目錄1.觀測(cè)噪聲標(biāo)準(zhǔn)卡爾曼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.系統(tǒng)噪聲

標(biāo)準(zhǔn)卡爾曼濾波的系統(tǒng)噪聲假設(shè)標(biāo)準(zhǔn)的卡爾曼濾波中，一般假設(shè)所有的系統(tǒng)噪聲是時(shí)間不相關(guān)的，即觀測(cè)噪聲是白噪聲。問(wèn)題：系統(tǒng)常含有顯著的系統(tǒng)性噪聲和其它時(shí)間相關(guān)噪聲；這些噪聲可能由于可觀測(cè)性較差未被選為狀態(tài)量；但會(huì)影響被估計(jì)的狀態(tài)處理系統(tǒng)噪聲時(shí)間相關(guān)性問(wèn)題的方法當(dāng)相關(guān)時(shí)間較短時(shí)：建模為白噪聲，但需覆蓋會(huì)影響卡爾曼濾波收斂的相關(guān)噪聲；

當(dāng)相關(guān)時(shí)間超過(guò)1min：采用帶不確定參數(shù)的Schmidt卡爾曼濾波；2022/12/3046目錄2.系統(tǒng)噪聲標(biāo)準(zhǔn)卡爾曼濾波的系統(tǒng)噪聲假設(shè)24.2Schmidt卡爾曼濾波目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法

2022/12/30474.2Schmidt卡爾曼濾波目錄

2022/1目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法Schmidt卡爾曼率濾波對(duì)應(yīng)的誤差協(xié)方差系統(tǒng)傳遞式為:將誤差協(xié)方差P,相關(guān)噪聲協(xié)方差W,相關(guān)矩陣U分解為不同傳遞方程:2022/12/3048目錄Schmidt卡爾曼率濾波對(duì)應(yīng)的誤差協(xié)方差系目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法類(lèi)似于觀測(cè)矩陣H,定義J為待估計(jì)參數(shù)到觀測(cè)向量的耦合矩陣,則卡爾曼濾波增益變?yōu)?Schmidt卡爾曼濾波誤差協(xié)方差,相關(guān)噪聲和相關(guān)矩陣更新為:2022/12/3049目錄類(lèi)似于觀測(cè)矩陣H,定義J為待估計(jì)參數(shù)到觀測(cè)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法

2022/12/3050目錄

2022/12/2750五、自適應(yīng)卡爾曼濾波2022/12/3051五、自適應(yīng)卡爾曼濾波2022/12/27515.自適應(yīng)卡爾曼濾波目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(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噪聲由位置的點(diǎn)位中誤差或誤差建模問(wèn)題：有些情況上述參數(shù)是無(wú)法獲取的，例如：1）MEMSIMU在出廠(chǎng)時(shí)未經(jīng)過(guò)嚴(yán)格標(biāo)定；2）如果沒(méi)有飛機(jī)武器掛倉(cāng)的先驗(yàn)振動(dòng)環(huán)境信息，則無(wú)法獲得觀測(cè)噪聲協(xié)方差陣；解決辦法Kalman濾波自行估計(jì)矩陣Q或/和R，即自適應(yīng)卡爾曼濾波2022/12/30525.自適應(yīng)卡爾曼濾波目錄1.問(wèn)題描述多數(shù)應(yīng)用中基于新息的自適應(yīng)估計(jì)（InnovationBasedAdaptiveEstimation,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)慎目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.基于新息的自適應(yīng)估計(jì)2022/12/3053基于新息的自適應(yīng)估計(jì)（InnovationBasedAd多模型自適應(yīng)估計(jì)（MultipleModelAdaptiveEstimation,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ù)。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法3.多模型自適應(yīng)估計(jì)2022/12/3054多模型自適應(yīng)估計(jì)（MultipleModelAdapti完整的狀態(tài)向量估計(jì)和誤差協(xié)方差計(jì)算公式如下：

IAE（新息）

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

實(shí)時(shí)處理/后處理差異卡爾曼濾波器是為實(shí)時(shí)應(yīng)用而設(shè)計(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è)量

卡爾曼平滑器是卡爾曼濾波器的擴(kuò)展，不僅適用所關(guān)注時(shí)間點(diǎn)之前的觀測(cè)新息，且適用之后的新息；非實(shí)時(shí)應(yīng)用中，平滑處理能夠得到更高的精度。常用平滑算法包含兩類(lèi)主要方法：1）正向-方向?yàn)V波；2）RTS平滑算法576.平滑算法目錄1.問(wèn)題描述實(shí)時(shí)處理/后處理目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.正向-反向?yàn)V波算法通過(guò)正向?yàn)V波和反向?yàn)V波（兩個(gè)相互獨(dú)立的濾波器）結(jié)果的加權(quán)平均，平滑結(jié)果由下式給定：下標(biāo)f和b分別表示正向和反向；k表示兩個(gè)濾波器中的相同時(shí)間點(diǎn)正向-反向卡爾曼平滑算法的狀態(tài)不確定度正向?yàn)V波反向?yàn)V波狀態(tài)不確定度時(shí)間前向?yàn)V波2022/12/3058目錄2.正向-反向?yàn)V波算法通過(guò)正向?yàn)V波和反向?yàn)V波目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法3.RTS平滑算法：RTS（Rauch,TungandStriebel,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)行平滑時(shí)，RTS算法更有效，若只需對(duì)單點(diǎn)進(jìn)行平滑時(shí)，正向-反向算法更有效

2022/12/3059目錄3.RTS平滑算法：RTS（Rauch,Tu附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法參考:

AlexBlekhman,/matlabcentral/fileexchange%SettruetrajectoryNsamples=100;dt=.1;t=0:dt:dt*Nsamples;Vtrue=10;

%XtrueisavectoroftruepositionsofthetrainXinitial=0;Xtrue=Xinitial+Vtrue*t;

%＝＝Motionequations%Previousstate(initialguess):Ourguessisthatthetrainstartsat0withvelocity

thatequalsto50%oftherealvelocityXk_prev=[0;0.5*Vtrue];

%CurrentstateestimateXk=[];2022/12/3060陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄參考:附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法Phi=[1dt;01];sigma_model=1;P=[sigma_model^20;0sigma_model^2];Q=[00;

00];

%Misthemeasurementmatrix.M=[10];sigma_meas=1;%1m/secR=sigma_meas^2;

%==Kalmaniteration%BuffersforlaterdisplayXk_buffer=zeros(2,Nsamples+1);Xk_buffer(:,1)=Xk_prev;Z_buffer=zeros(1,Nsamples+1);2022/12/3061陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄Phi附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法fork=1:NsamplesZ=Xtrue(k+1)+sigma_meas*randn;Z_buffer(k+1)=Z;

%KalmaniterationP1=Phi*P*Phi'+Q;S=M*P1*M'+R;goestothemeasurement.%IfKislow,moreweightgoestothemodelprediction.K=P1*M'*inv(S);P=P1-K*M*P1;

Xk=Phi*Xk_prev+K*(Z-M*Phi*Xk_prev);Xk_buffer(:,k+1)=Xk;

%ForthenextiterationXk_prev=Xk;end;2022/12/3062陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄for附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法figure;plot(t,Xtrue,'k-.',t,Z_buffer,'r.',t,Xk_buffer(1,:),'b');

xlabel('時(shí)間(s)','fontsize',15);ylabel('位置(m)','fontsize',15);legend('參考真值','測(cè)量值','卡爾曼濾波估值','fontsize',15);gridon

2022/12/3063陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄figu附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法%==VelocityanalysisInstantaneousVelocity=[0(Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples))/dt];%滑動(dòng)平均WindowSize=5;InstantaneousVelocityRunningAverage=filter(ones(1,WindowSize)/WindowSize,1,InstantaneousVelocity);figure,plot(t,ones(size(t))*Vtrue,'g','LineWidth',2);holdon;plot(t,InstantaneousVelocity,'r-*','LineWidth',2);plot(t,InstantaneousVelocityRunningAverage,'k.','LineWidth',2);plot(t,Xk_buffer(2,:),'b','LineWidth',2);xlabel('時(shí)間(s)','fontsize',15);ylabel('速度(m/s)','fontsize',15);legend('參考真值','位置觀測(cè)量微分','滑動(dòng)平均法','卡爾曼濾波',15);set(gca,'fontsize',15)gridon2022/12/3064陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄%==：卡爾曼濾波：卡爾曼濾波目

錄一.概述二.標(biāo)準(zhǔn)卡爾曼濾波卡爾曼濾波方程閉環(huán)卡爾曼濾波卡爾曼濾波特性及實(shí)現(xiàn)中的問(wèn)題三.擴(kuò)展卡爾曼濾波非線(xiàn)性系統(tǒng)線(xiàn)性化卡爾曼濾波擴(kuò)展卡爾曼濾波四.Schmidt卡爾曼濾波五.自適應(yīng)卡爾曼濾波六.平滑算法66目 錄一.概述2一、概述2022/12/3067一、概述2022/12/2731.1RudolfEmilKalmanBorn1930inHungary

BSandMSfromMITPhD1957fromColumbiaFilterdevelopedin1960-61目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法KalmanRE.Anewapproachtolinearfilteringandpredictionproblems[J].JournalofFluidsEngineering,1960,82(1):35-45.（引用：18083）681.1RudolfEmilKalmanBorn193Kalman濾波是一種最優(yōu)估計(jì)算法，而非濾波器能夠?qū)崟r(shí)估計(jì)系統(tǒng)中的參數(shù)（如連續(xù)變化的位置、速度等信息）。估計(jì)量通過(guò)一系列受噪聲污染的觀測(cè)量來(lái)更新，觀測(cè)量必須是待估參數(shù)的函數(shù)，但是在給定的時(shí)刻，不要求觀測(cè)量能夠唯一確定當(dāng)時(shí)的參數(shù)值。1.2概述目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法69Kalman濾波是一種最優(yōu)估計(jì)算法，而非濾波器1.2概述目Kalman濾波是一種遞推線(xiàn)性最小方差估計(jì)在提供的初始估計(jì)基礎(chǔ)上，卡爾曼濾波通過(guò)遞歸運(yùn)算，用先驗(yàn)值和最新觀測(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)（新息）?？柭鼮V波是一種貝葉斯估計(jì)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法最小方差估計(jì)線(xiàn)性最小方差估計(jì)遞推線(xiàn)性最小方差估計(jì)70Kalman濾波是一種遞推線(xiàn)性最小方差估計(jì)目錄最小1.3卡爾曼濾波的要素和流程實(shí)際系統(tǒng)系統(tǒng)模型觀測(cè)模型觀測(cè)向量及其協(xié)方差狀態(tài)向量及其協(xié)方差卡爾曼濾波算法（實(shí)線(xiàn)表示數(shù)據(jù)流一直有，虛線(xiàn)表示只在某些應(yīng)用中有，Ref：PaulGroves）目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法711.3卡爾曼濾波的要素和流程實(shí)際系統(tǒng)系統(tǒng)模型觀測(cè)模型觀測(cè)向

狀態(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ò)展KFSchmidtKF自適應(yīng)KF平滑算法72狀態(tài)向量（狀態(tài)）目錄81.4卡爾曼濾波的要素4個(gè)要素：2個(gè)模型、1組觀測(cè)量、1個(gè)算法2個(gè)模型系統(tǒng)模型也稱(chēng)過(guò)程模型或者時(shí)間傳遞模型，描述了狀態(tài)與誤差協(xié)方差矩陣隨時(shí)間的變化特性。對(duì)于選定狀態(tài)量，系統(tǒng)模型是確定的。觀測(cè)模型描述了觀測(cè)向量與狀態(tài)向量間的函數(shù)關(guān)系。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法731.4卡爾曼濾波的要素4個(gè)要素：2個(gè)模型、1組觀測(cè)量、1個(gè)1組觀測(cè)向量是一組針對(duì)同一時(shí)刻的系統(tǒng)特性的測(cè)量值，例如觀測(cè)量可以包括GNSS系統(tǒng)的位置測(cè)量值，或者INS與GNSS位置結(jié)果的差值。1個(gè)算法：卡爾曼濾波算法使用觀測(cè)向量、觀測(cè)模型和系統(tǒng)模型來(lái)獲得狀態(tài)向量的最優(yōu)估計(jì)，分為系統(tǒng)傳遞和測(cè)量更新兩個(gè)部分。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法741組觀測(cè)向量目錄10目錄1.5卡爾曼濾波的導(dǎo)航應(yīng)用慣性導(dǎo)航系統(tǒng)（INS）的精對(duì)準(zhǔn)和標(biāo)定單一導(dǎo)航（GNSS,無(wú)線(xiàn)電、水聲學(xué)、匹配）組合導(dǎo)航INS/GNSS組合導(dǎo)航及多傳感器組合導(dǎo)航INS/水聲組合導(dǎo)航INS/匹配導(dǎo)航…概述經(jīng)典KFEKFLKF75目錄1.5卡爾曼濾波的導(dǎo)航應(yīng)用慣性導(dǎo)航系統(tǒng)（IN二、Kalman濾波2022/12/3076二、Kalman濾波2022/12/27122.1卡爾曼濾波方程1.離散系統(tǒng)的數(shù)學(xué)描述設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測(cè)方程分別為：

Xk為k時(shí)刻的n維狀態(tài)向量（被估計(jì)量）Zk為k時(shí)刻的m維量測(cè)向量k-1到k時(shí)刻的系統(tǒng)一步狀態(tài)轉(zhuǎn)移矩陣（n×n階）Wk-1為k-1時(shí)刻的系統(tǒng)噪聲（r維）Γk-1為系統(tǒng)噪聲矩陣（n×r階）Hk為k時(shí)刻系統(tǒng)量測(cè)矩陣（m×n階）Vk為k時(shí)刻m維量測(cè)噪聲目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法772.1卡爾曼濾波方程1.離散系統(tǒng)的數(shù)學(xué)描述Xk為k時(shí)刻的要求{Wk}和{Vk}是互不相關(guān)的、零均值白噪聲序列：

Qk和Rk分別稱(chēng)為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，分別是已知值的非負(fù)定陣和正定陣；

δkj是Kroneckerδ函數(shù)，即：目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法78要求{Wk}和{Vk}是互不相關(guān)的、零均值白噪聲序列：Q

初始狀態(tài)的一、二階統(tǒng)計(jì)特性為：

Var{·}為對(duì){·}求方差的符號(hào)卡爾曼濾波要求mx0和Cx0為已知量，且要求X0與{Wk}和{Vk}都不相關(guān)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法79初始狀態(tài)的一、二階統(tǒng)計(jì)特性為：Var{·}為對(duì){·}求2.離散卡爾曼濾波方程狀態(tài)一步預(yù)測(cè)方程狀態(tài)估值計(jì)算方程濾波增益方程一步預(yù)測(cè)均方差方程估計(jì)均方差方程或

目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法802.離散卡爾曼濾波方程狀態(tài)一步預(yù)測(cè)方程狀態(tài)估值計(jì)算方程濾波時(shí)間更新方程量測(cè)修正方程目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法81時(shí)間更新量測(cè)修正目錄17濾波計(jì)算回路增益計(jì)算回路目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法82濾波計(jì)算回路增益計(jì)算回路目錄183.卡爾曼濾波示例目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法有一個(gè)質(zhì)點(diǎn)，沿X軸正方向運(yùn)動(dòng)，質(zhì)點(diǎn)從X=0開(kāi)始勻速直線(xiàn)運(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=10m/sXi-1XiXi+1位置觀測(cè)值833.卡爾曼濾波示例目錄有一個(gè)質(zhì)點(diǎn)，沿X軸正方向運(yùn)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法狀態(tài)量x=[X,V]，即以質(zhì)點(diǎn)的位置和速度作為卡爾曼濾波狀態(tài)量；系統(tǒng)狀態(tài)方程為Xk=Xk-1+Vk-1*dt；狀態(tài)轉(zhuǎn)移矩陣

Phi=[1dt;01]；系統(tǒng)噪聲協(xié)方差陣Q：即系統(tǒng)模型的不確定度，由于假設(shè)模型即質(zhì)點(diǎn)運(yùn)動(dòng)模型，因此可認(rèn)為模型的不確定度為0，即Q=[00;00]觀測(cè)矩陣H：由于只觀測(cè)了質(zhì)點(diǎn)位置，未觀測(cè)速度，因此觀測(cè)矩陣H=[10];觀測(cè)噪聲矩陣R：位置觀測(cè)量的方差為m2，即R=1觀測(cè)量向量Z：在真實(shí)狀態(tài)（真實(shí)位置）加上均值為零，方差為m2的白噪聲;卡爾曼濾波初始狀態(tài)：X0=0,V0=5m/s，初始狀態(tài)誤差協(xié)方差矩陣P=[10;01]設(shè)計(jì)卡爾曼濾波84目錄狀態(tài)量x=[X,V]，即以質(zhì)點(diǎn)的位置和目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波位置估計(jì)85目錄卡爾曼濾波位置估計(jì)21目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波速度估計(jì)86目錄卡爾曼濾波速度估計(jì)222.2閉環(huán)卡爾曼濾波1.全狀態(tài)濾波和誤差狀態(tài)濾波根據(jù)卡爾曼濾波狀態(tài)向量的選取不同，卡爾曼濾波可分為:全狀態(tài)卡爾曼濾波（TotalStateImplementation）和誤差狀態(tài)卡爾曼濾波（ErrorStateImplementation）組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進(jìn)行估計(jì)的主要對(duì)象導(dǎo)航參數(shù)

導(dǎo)航參數(shù)

位置λ，L

速度VX,VY,VZ姿態(tài)ψ,θ,γ導(dǎo)航參數(shù)用X表示目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法872.2閉環(huán)卡爾曼濾波1.全狀態(tài)濾波和誤差狀態(tài)濾波組合導(dǎo)航TotalState：以系統(tǒng)的固有屬性，如位置、速度和姿態(tài)等，為狀態(tài)向量的卡爾曼濾波，稱(chēng)為全狀態(tài)濾波或直接卡爾曼濾波ErrorState：以系統(tǒng)測(cè)量誤差值，如INS位置、速度和姿態(tài)等，為狀態(tài)向量的卡爾曼濾波稱(chēng)為誤差狀態(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ò)展KFSchmidtKF自適應(yīng)KF平滑算法模型可能是線(xiàn)性的，也可能是非線(xiàn)性的模型一般都是線(xiàn)性的88TotalState：以系統(tǒng)的固有屬性，如位置、速度和姿態(tài)2.開(kāi)環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-用導(dǎo)航參數(shù)誤差的估值去校正系統(tǒng)輸出的導(dǎo)航參數(shù)，得到綜合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估值目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法開(kāi)環(huán)（輸出校正）的卡爾曼濾波器2022/12/30892.開(kāi)環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-用導(dǎo)3.閉環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-采用反饋校正的間接法估計(jì)，是將慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)誤差

的估值

反饋到慣導(dǎo)系統(tǒng)內(nèi)，對(duì)誤差狀態(tài)進(jìn)行校正。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法閉環(huán)（反饋校正）的卡爾曼濾波器2022/12/30903.閉環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-采用開(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ò)一階近似的線(xiàn)性方程，狀態(tài)的數(shù)值越小，則近似的準(zhǔn)確性越高，因此，利用狀態(tài)反饋校正的系統(tǒng)狀態(tài)方程，更能接近真實(shí)地反映系統(tǒng)誤差狀態(tài)的動(dòng)態(tài)過(guò)程。卡爾曼濾波算法中，反饋狀態(tài)估計(jì)的最佳時(shí)機(jī)是在測(cè)量更新后立即進(jìn)行?？柭鼮V波的閉環(huán)和開(kāi)環(huán)可以混合使用，即一些狀態(tài)估計(jì)作為校正值被反饋，而另外一些不反饋。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法4.開(kāi)環(huán)與閉環(huán)卡爾曼濾波對(duì)比2022/12/3091開(kāi)環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩5.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：位置、速度和姿態(tài)只做開(kāi)環(huán)修正；而IMU誤差，如陀螺和加速度計(jì)零偏，比例因子誤差進(jìn)行反饋，修正IMU的原始觀測(cè)值。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法GINS軟件松組合算法架構(gòu)示意圖（混合濾波）2022/12/30925.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：目2.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ò)展KFSchmidtKF自適應(yīng)KF平滑算法932.3濾波特性及濾波實(shí)現(xiàn)中的問(wèn)題1.濾波收斂特性初始不確

狀態(tài)估計(jì)的收斂速度基本上取決于該狀態(tài)的可觀測(cè)性。如果觀測(cè)矩陣隨時(shí)間變化或者狀態(tài)之間通過(guò)狀態(tài)轉(zhuǎn)移矩陣存在時(shí)間依存關(guān)系，那么隨著迭代次數(shù)的增加，更多的狀態(tài)量變得可觀測(cè)。例：導(dǎo)航中用位置的變化率來(lái)確定速度許多參數(shù)的可觀測(cè)性依賴(lài)于系統(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ò)展KFSchmidtKF自適應(yīng)KF平滑算法94狀態(tài)估計(jì)的收斂速度基本上取決于該狀態(tài)的可觀測(cè)性。目2.濾波參數(shù)調(diào)整狀態(tài)估計(jì)誤差P/R太小狀態(tài)估計(jì)誤差狀態(tài)估計(jì)誤差P/R適中P/R太大時(shí)間時(shí)間時(shí)間目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法不同P/R比率下的卡爾曼濾波誤差傳遞（PaulGroves）為克服卡爾曼濾波模型的限制，噪聲建模必須足以囊括實(shí)際系統(tǒng)的行為，形象地說(shuō)，要將真實(shí)世界的“方形楔子”放到卡爾曼濾波模型的“圓洞”中，因此這個(gè)洞要被擴(kuò)寬。

例：若忽略1HzGPS定位結(jié)果誤差的時(shí)間相關(guān)性，應(yīng)將其放大（一般為2-3倍）以建模為白噪聲952.濾波參數(shù)調(diào)整狀態(tài)估計(jì)誤差P/R太小狀態(tài)估計(jì)誤差狀態(tài)估計(jì)

P陣非正定P陣的非正定容易導(dǎo)致濾波發(fā)散。使用高精度變量存儲(chǔ)（如double類(lèi)型），減小舍入誤差；縮放卡爾曼濾波標(biāo)度，使所有狀態(tài)不確定度在數(shù)值上具有相同量級(jí)；

P陣非對(duì)稱(chēng)使用式計(jì)算P陣，容易導(dǎo)致P陣非對(duì)稱(chēng)；每次系統(tǒng)傳遞及測(cè)量更新后通過(guò)P=(P+P’)/2來(lái)保持對(duì)稱(chēng)性；建議采用Joseph式P陣更新：

平方根濾波傳遞而非P,可把動(dòng)態(tài)范圍減小兩個(gè)量級(jí)，從而減小舍入誤差影響。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法

3.數(shù)值計(jì)算問(wèn)題96P陣非正定目錄

3.數(shù)值計(jì)算問(wèn)題32三、擴(kuò)展Kalman濾波（EKF）2022/12/3097三、擴(kuò)展Kalman濾波（EKF）2022/12/2733

標(biāo)準(zhǔn)卡爾曼濾波的線(xiàn)性假設(shè)在標(biāo)準(zhǔn)的卡爾曼濾波中，觀測(cè)模型假設(shè)為線(xiàn)性（Z是X的線(xiàn)性函數(shù)），但實(shí)際情況往往并非如此（如GNSS導(dǎo)航濾波器中，觀測(cè)模型是強(qiáng)非線(xiàn)性的）在標(biāo)準(zhǔn)卡爾曼濾波中，系統(tǒng)模型也被假設(shè)為線(xiàn)性的（X的時(shí)間導(dǎo)數(shù)是X的線(xiàn)性函數(shù)）問(wèn)題：在全狀態(tài)INS/GNSS組合導(dǎo)航中，狀態(tài)量為絕對(duì)位置、速度和姿態(tài)，因?yàn)殡y以一直保持系統(tǒng)的線(xiàn)性近似，完成所有的系統(tǒng)反饋并不總是可行的；

擴(kuò)展/線(xiàn)性化卡爾曼濾波為卡爾曼濾波的非線(xiàn)性形式目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法1.問(wèn)題描述3.1非線(xiàn)性系統(tǒng)98標(biāo)準(zhǔn)卡爾曼濾波的線(xiàn)性假設(shè)目錄1.問(wèn)題描述3.1目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.非線(xiàn)性系統(tǒng)描述一般非線(xiàn)性系統(tǒng)（連續(xù)）和離散系統(tǒng)可由以下方程來(lái)表示：擴(kuò)展卡爾曼濾波研究的非線(xiàn)性系統(tǒng)可由以下方程來(lái)表示或?yàn)楹?jiǎn)化問(wèn)題，需對(duì)噪聲的統(tǒng)計(jì)特性給以符合實(shí)際又便于處理的假定或Wt或{Wk-1}，Vt或{Vk}均為彼此不相關(guān)的零均值白噪聲序列，與初始狀態(tài)X(0)或X0也不相關(guān)99目錄2.非線(xiàn)性系統(tǒng)描述一般非線(xiàn)性系統(tǒng)（連續(xù)）和離如何解決非線(xiàn)性問(wèn)題采用近似的方法通常線(xiàn)性化方法目前應(yīng)用比較廣泛的是對(duì)非線(xiàn)性模型的線(xiàn)性化非線(xiàn)性系統(tǒng)能否應(yīng)用卡爾曼濾波的關(guān)鍵基本假設(shè)：非線(xiàn)性微分方程的理論解一定存在；理論解與實(shí)際解差能夠用一個(gè)線(xiàn)性微分方程表示，即線(xiàn)性擾動(dòng)方程。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法3.解決方案2022/12/30100如何解決非線(xiàn)性問(wèn)題采用近似的方法通常線(xiàn)性化方法目前應(yīng)用比較廣3.2非線(xiàn)性系統(tǒng)的線(xiàn)性化t實(shí)際狀態(tài)

X標(biāo)稱(chēng)狀態(tài)X*圍繞X*線(xiàn)性化：線(xiàn)性化卡爾曼濾波（LinearizedKalmanFilter，LKF）圍繞實(shí)際狀態(tài)X（濾波估計(jì)狀態(tài)，或?qū)嶋H軌跡）進(jìn)行線(xiàn)性化：擴(kuò)展卡爾曼濾波（ExtendedKalmanFilter，EKF）目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法泰勒展開(kāi)法進(jìn)行線(xiàn)性化狀態(tài)1013.2非線(xiàn)性系統(tǒng)的線(xiàn)性化t實(shí)際狀態(tài)X標(biāo)稱(chēng)狀態(tài)X*圍繞X線(xiàn)性化卡爾曼濾波：圍繞標(biāo)稱(chēng)狀態(tài)對(duì)非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性化或當(dāng)Wt或{Wk-1}，Vt或{Vk}恒為0時(shí)，系統(tǒng)模型的解稱(chēng)為非線(xiàn)性方程的理論解，又稱(chēng)“標(biāo)稱(chēng)軌跡”或標(biāo)稱(chēng)狀態(tài)。通常記為Xn(t)或Xkn,和Zn(t)或Zkn，則有：目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法102線(xiàn)性化卡爾曼濾波：圍繞標(biāo)稱(chēng)狀態(tài)對(duì)非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性化或當(dāng)Wt3.3線(xiàn)性化卡爾曼濾波或定義：非線(xiàn)性系統(tǒng)的真軌跡運(yùn)動(dòng)與標(biāo)稱(chēng)軌跡運(yùn)動(dòng)的偏差為：如果這些偏差足夠小，那么，可以圍繞標(biāo)稱(chēng)狀態(tài)把X(t）和Z(t）展開(kāi)成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法1033.3線(xiàn)性化卡爾曼濾波或定義：如果這些偏差足夠小，那么，3.3線(xiàn)性化卡爾曼濾波或則有：非線(xiàn)性系統(tǒng)能否應(yīng)用卡爾曼濾波的關(guān)鍵目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2022/12/301043.3線(xiàn)性化卡爾曼濾波或則有：非線(xiàn)性系統(tǒng)能否應(yīng)用卡爾曼濾由于線(xiàn)性化卡爾曼濾波存在上述問(wèn)題，改用另一種近似方法，即在最優(yōu)化狀態(tài)估計(jì)或附近進(jìn)行泰勒展開(kāi)，線(xiàn)性化。3.4擴(kuò)展卡爾曼濾波標(biāo)稱(chēng)解難以解算真軌跡與標(biāo)稱(chēng)軌跡之間的狀態(tài)差△X(t)或△Xk不能確保其足夠小，從而導(dǎo)致線(xiàn)性化誤差較大，模型的線(xiàn)性近似度變?nèi)?。LKF的缺陷目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法擴(kuò)展卡爾曼濾波2022/12/30105由于線(xiàn)性化卡爾曼濾波存在上述問(wèn)題，改用另一種近似方法，即在最目錄概述KF方程EKFLKF或定義非線(xiàn)性系統(tǒng)的真軌跡運(yùn)動(dòng)與實(shí)際軌跡運(yùn)動(dòng)的偏差為：若偏差足夠小，則可以圍繞最優(yōu)化狀態(tài)估計(jì)把X(t）和Z(t）展開(kāi)成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。2022/12/30106目錄概述或定義若偏差足夠小，則可以圍繞最優(yōu)化狀態(tài)或則有：EKF假設(shè)狀態(tài)向量估計(jì)的誤差遠(yuǎn)比狀態(tài)向量本身小，因此可用線(xiàn)性系統(tǒng)模型計(jì)算狀態(tài)向量殘差。標(biāo)準(zhǔn)的誤差協(xié)方差傳遞公式可采用在最優(yōu)狀態(tài)估計(jì)附近進(jìn)行線(xiàn)性化的狀態(tài)轉(zhuǎn)移矩陣，即目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法107或則有：EKF假設(shè)狀態(tài)向量估計(jì)的誤差遠(yuǎn)比狀態(tài)向量本身小，因四、Schmidt卡爾曼濾波2022/12/30108四、Schmidt卡爾曼濾波2022/12/27444.1時(shí)間相關(guān)噪聲目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法1.觀測(cè)噪聲

標(biāo)準(zhǔn)卡爾曼濾波的觀測(cè)噪聲假設(shè)一般假設(shè)所有的觀測(cè)噪聲是時(shí)間不相關(guān)的，即觀測(cè)噪聲是白噪聲。問(wèn)題：該假設(shè)常不成立；卡爾曼濾波強(qiáng)制把新息中的時(shí)間相關(guān)部分歸因狀態(tài)，因此，時(shí)間相關(guān)的觀測(cè)噪聲可能會(huì)破壞狀態(tài)估計(jì)。處理觀測(cè)噪聲時(shí)間相關(guān)性問(wèn)題的方法將時(shí)間相關(guān)噪聲擴(kuò)展至卡爾曼濾波的狀態(tài)向量中，進(jìn)行估計(jì)；

放大觀測(cè)噪聲協(xié)方差矩陣R，進(jìn)而減小卡爾曼濾波的增益；利用Schmidt卡爾曼濾波2022/12/301094.1時(shí)間相關(guān)噪聲目錄1.觀測(cè)噪聲標(biāo)準(zhǔn)卡爾曼目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.系統(tǒng)噪聲

標(biāo)準(zhǔn)卡爾曼濾波的系統(tǒng)噪聲假設(shè)標(biāo)準(zhǔn)的卡爾曼濾波中，一般假設(shè)所有的系統(tǒng)噪聲是時(shí)間不相關(guān)的，即觀測(cè)噪聲是白噪聲。問(wèn)題：系統(tǒng)常含有顯著的系統(tǒng)性噪聲和其它時(shí)間相關(guān)噪聲；這些噪聲可能由于可觀測(cè)性較差未被選為狀態(tài)量；但會(huì)影響被估計(jì)的狀態(tài)處理系統(tǒng)噪聲時(shí)間相關(guān)性問(wèn)題的方法當(dāng)相關(guān)時(shí)間較短時(shí)：建模為白噪聲，但需覆蓋會(huì)影響卡爾曼濾波收斂的相關(guān)噪聲；

當(dāng)相關(guān)時(shí)間超過(guò)1min：采用帶不確定參數(shù)的Schmidt卡爾曼濾波；2022/12/30110目錄2.系統(tǒng)噪聲標(biāo)準(zhǔn)卡爾曼濾波的系統(tǒng)噪聲假設(shè)24.2Schmidt卡爾曼濾波目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法

2022/12/301114.2Schmidt卡爾曼濾波目錄

2022/1目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法Schmidt卡爾曼率濾波對(duì)應(yīng)的誤差協(xié)方差系統(tǒng)傳遞式為:將誤差協(xié)方差P,相關(guān)噪聲協(xié)方差W,相關(guān)矩陣U分解為不同傳遞方程:2022/12/30112目錄Schmidt卡爾曼率濾波對(duì)應(yīng)的誤差協(xié)方差系目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法類(lèi)似于觀測(cè)矩陣H,定義J為待估計(jì)參數(shù)到觀測(cè)向量的耦合矩陣,則卡爾曼濾波增益變?yōu)?Schmidt卡爾曼濾波誤差協(xié)方差,相關(guān)噪聲和相關(guān)矩陣更新為:2022/12/30113目錄類(lèi)似于觀測(cè)矩陣H,定義J為待估計(jì)參數(shù)到觀測(cè)目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法

2022/12/30114目錄

2022/12/2750五、自適應(yīng)卡爾曼濾波2022/12/30115五、自適應(yīng)卡爾曼濾波2022/12/27515.自適應(yīng)卡爾曼濾波目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(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噪聲由位置的點(diǎn)位中誤差或誤差建模問(wèn)題：有些情況上述參數(shù)是無(wú)法獲取的，例如：1）MEMSIMU在出廠(chǎng)時(shí)未經(jīng)過(guò)嚴(yán)格標(biāo)定；2）如果沒(méi)有飛機(jī)武器掛倉(cāng)的先驗(yàn)振動(dòng)環(huán)境信息，則無(wú)法獲得觀測(cè)噪聲協(xié)方差陣；解決辦法Kalman濾波自行估計(jì)矩陣Q或/和R，即自適應(yīng)卡爾曼濾波2022/12/301165.自適應(yīng)卡爾曼濾波目錄1.問(wèn)題描述多數(shù)應(yīng)用中基于新息的自適應(yīng)估計(jì)（InnovationBasedAdaptiveEstimation,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)慎目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法2.基于新息的自適應(yīng)估計(jì)2022/12/30117基于新息的自適應(yīng)估計(jì)（InnovationBasedAd多模型自適應(yīng)估計(jì)（MultipleModelAdaptiveEstimation,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ù)。目錄概述標(biāo)準(zhǔn)KF擴(kuò)展KFSchmidtKF自適應(yīng)KF平滑算法3.多模型自適應(yīng)估計(jì)2022/12/30118多模型自適應(yīng)估計(jì)（MultipleModelAdapti完整的狀態(tài)向量估計(jì)和誤差協(xié)方差計(jì)算公式如下：

IAE（新息）

和MMAE（多模型）方法比較MMAE計(jì)算量大IAE中，Q,R,P和濾波增益均可能是狀態(tài)估計(jì)的函數(shù)；而他們?cè)贛MAE濾波器組（標(biāo)準(zhǔn)卡爾曼濾波而非