《卡爾曼濾波》ppt課件

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

2、F平滑算法平滑算法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ì)算法，而非濾波器u可以實(shí)時(shí)估計(jì)系統(tǒng)中的參數(shù)如延續(xù)變化的位置、速度等信息。u估計(jì)量經(jīng)過一系列受噪聲污染的觀丈量來更新，u觀丈量必需是待估參數(shù)的函數(shù)，但是在給定的時(shí)辰，不要求觀丈量可以獨(dú)一確定當(dāng)時(shí)的參數(shù)值。1.2 概述目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF

3、自順應(yīng)自順應(yīng) KF平滑算法平滑算法.6uKalman濾波是一種遞推線性最小方差估計(jì)u在提供的初始估計(jì)根底上，卡爾曼濾波經(jīng)過遞歸運(yùn)算，用先驗(yàn)值和最新觀測數(shù)據(jù)的加權(quán)平均來更新形狀估計(jì)老息+新息。u非遞歸算法如規(guī)范最小二乘中沒有先驗(yàn)估計(jì)，估計(jì)結(jié)果由全部觀測數(shù)據(jù)計(jì)算而來新息 。u卡爾曼濾波是一種貝葉斯估計(jì)目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法最小方差估計(jì)線性最小方差估計(jì)遞推線性最小方差估計(jì).71.3 卡爾曼濾波的要素和流程實(shí)踐系統(tǒng)系統(tǒng)模型觀測模型觀測向量及其協(xié)方差形狀向量及其協(xié)方差卡爾曼濾波算法實(shí)線表示數(shù)據(jù)流不斷有，虛線表示只在某些運(yùn)用中有

4、，Ref：Paul Groves目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.8u 形狀向量形狀u是一組描畫系統(tǒng)的參數(shù)。u可以是常量，也可是時(shí)變量，是估計(jì)對象。u與之相關(guān)聯(lián)的是誤差協(xié)方差矩陣，描畫了形狀估計(jì)的不確定度及估計(jì)誤差間的相關(guān)度。目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.91.4 卡爾曼濾波的要素4個要素：2個模型、1組觀丈量、1個算法 2個模型系統(tǒng)模型也稱過程模型或者時(shí)間傳送模型，描畫了形狀與誤差協(xié)方差矩陣隨時(shí)間的變化特性。對于選定形狀量，系統(tǒng)模型是確定的。觀測模型描畫

5、了觀測向量與形狀向量間的函數(shù)關(guān)系。目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.10p 1 1組觀測向量組觀測向量p是一組針對同一時(shí)辰的系統(tǒng)特性的丈量值，例如觀丈量是一組針對同一時(shí)辰的系統(tǒng)特性的丈量值，例如觀丈量可以包括可以包括GNSSGNSS系統(tǒng)的位置丈量值，或者系統(tǒng)的位置丈量值，或者INSINS與與GNSSGNSS位置位置結(jié)果的差值。結(jié)果的差值。p 1 1個算法：個算法：p卡爾曼濾波算法卡爾曼濾波算法p運(yùn)用觀測向量、觀測模型和系統(tǒng)模型來獲得形狀向量的運(yùn)用觀測向量、觀測模型和系統(tǒng)模型來獲得形狀向量的最優(yōu)估計(jì)，分為系統(tǒng)傳送和丈量更新兩個部

6、分。最優(yōu)估計(jì)，分為系統(tǒng)傳送和丈量更新兩個部分。目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.目目 錄錄111.5 卡爾曼濾波的導(dǎo)航運(yùn)用p慣性導(dǎo)航系統(tǒng)INS的精對準(zhǔn)和標(biāo)定p單一導(dǎo)航GNSS, 無線電、水聲學(xué)、匹配p組合導(dǎo)航pINS/GNSS組合導(dǎo)航及多傳感器組合導(dǎo)航pINS/水聲組合導(dǎo)航pINS/匹配導(dǎo)航p 概述 經(jīng)典KF EKF LKF.二、二、KalmanKalman濾波濾波2021-12-1412.132.1 卡爾曼濾波方程1. 離散系統(tǒng)的數(shù)學(xué)描畫 設(shè)離散化后的系統(tǒng)形狀方程和量測方程分別為： kk,k 1k 1k 1k 1kkkkXX

7、WZH XV目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.14p要求Wk和Vk是互不相關(guān)的、零均值白噪聲序列：TkjkkjTkjkkjE W WQE V VRp Qk和Rk分別稱為系統(tǒng)噪聲和量測噪聲的方差矩陣，分別是知值的非負(fù)定陣和正定陣； kj0(kj)1(kj)p k j k j 是是Kronecker Kronecker 函數(shù)，即：函數(shù)，即：目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.15u 初始形狀的一、二階統(tǒng)計(jì)特性為：0 x0E Xm0 x0Var XCu Var 為對求

8、方差的符號u卡爾曼濾波要求mx0和Cx0為知量，u且要求X0與Wk和Vk都不相關(guān)目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.162. 離散卡爾曼濾波方程k/k 1k,k 1k 1XX-= p 形狀一步預(yù)測方程形狀一步預(yù)測方程kk/k 1kkkk/k 1XXK (ZH X)p 形狀估值計(jì)算方程形狀估值計(jì)算方程TT1kk/k 1kkk/k 1kkKPH (H PHR )p 濾波增益方程濾波增益方程TTk/k 1k,k 1 k 1k,k 1k 1k 1k 1PPQ-=+ GGp 一步預(yù)測均方差方程一步預(yù)測均方差方程TTkkkk/k 1kkkk

9、kP(IK H )P(IK H )K R Kp 估計(jì)均方差方程估計(jì)均方差方程kkkk/k 1P(IK H )P或 目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.17時(shí)間更新方程量測修正方程k/k 1k,k 1k 1XXkk/k 1kkkk/k 1XXK (ZH X)TT1kk/k 1kkk/k 1kkKPH (H PHR )TTk/k 1k,k 1k 1 k,k 1k 1k 1k 1PPQTTkkkk/k 1kkkkkP(IK H )P(IK H )K R K目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng)

10、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 1Xkk1kXkZkP濾波計(jì)算回路增益計(jì)算回路目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.193. 卡爾曼濾波例如目目 錄錄概述概述規(guī)范規(guī)范 KF

11、擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法有一個質(zhì)點(diǎn)，沿X軸正方向運(yùn)動，質(zhì)點(diǎn)從X = 0 開場勻速直線運(yùn)動, 速度為V = 10m/s，那么每一時(shí)辰質(zhì)點(diǎn)的真實(shí)位置參考真值為：X = X0+V * t；實(shí)踐上，我們每隔 0.1s 可以丈量一次質(zhì)點(diǎn)的位置，但位置丈量值存在誤差假設(shè)是均值為0的白噪聲序列根據(jù)我們對質(zhì)點(diǎn)的位置觀丈量，用卡爾曼濾波方法計(jì)算每一時(shí)辰質(zhì)點(diǎn)的位置和速度Xv = 10 m/sXi-1XiXi+1位置觀測值.20目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法l形狀量 x = X, V，即以質(zhì)點(diǎn)的位置和速

12、度作為卡爾曼濾波形狀量；l系統(tǒng)形狀方程為 Xk = Xk-1 + Vk-1 * dt ；l形狀轉(zhuǎn)移矩陣 Phi = 1 dt; 0 1；l系統(tǒng)噪聲協(xié)方差陣Q： 即系統(tǒng)模型的不確定度，由于假設(shè)模型即質(zhì)點(diǎn)運(yùn)動模型，因此可以為模型的不確定度為0，即 Q = 0 0; 0 0l觀測矩陣 H：由于只觀測了質(zhì)點(diǎn)位置，未觀測速度，因此觀測矩陣 H = 1 0;l觀測噪聲矩陣R：位置觀丈量的方差為 m2，即 R = 1l觀丈量向量Z：在真實(shí)形狀真實(shí)位置加上均值為零，方差為 m2的白噪聲;l卡爾曼濾波初始形狀：X0 = 0, V0 = 5 m/s，初始形狀誤差協(xié)方差矩陣P = 1 0; 0 1p 設(shè)計(jì)卡爾曼濾波

13、.21目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法卡爾曼濾波位置估計(jì)012345678910020406080100120時(shí)間 (s)位置 (m) 參考真值測量值卡爾曼濾波估值.22目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法卡爾曼濾波速度估計(jì)0246810-30-20-1001020304050時(shí)間 (s)速度 (m/s) 參考真值位置觀測量微分滑動平均法卡爾曼濾波.232.2 閉環(huán)卡爾曼濾波1. 全形狀濾波和誤差形狀濾波根據(jù)卡爾曼濾波形狀向量的選取不同，卡爾曼濾波可分為:全形狀卡

14、爾曼濾波Total State Implementation和誤差形狀卡爾曼濾波Error State Implementation組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進(jìn)展估計(jì)的主要對象位置，L 速度VX ,VY ,VZ姿態(tài),導(dǎo)航參數(shù)用X表示目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.24Total State：以系統(tǒng)的固有屬性，如位置、速度和姿態(tài)等，為形狀向量的卡爾曼濾波，稱為全形狀濾波或直接卡爾曼濾波Error State：以系統(tǒng)丈量誤差值，如INS位置、速度和姿態(tài)等，為形狀向量的卡爾曼濾波稱為誤差形狀濾波或間接卡爾曼濾波直接法直接法間接法間

15、接法以各種導(dǎo)航參數(shù)X為主要形狀濾波器估值的主要部分即是導(dǎo)航參數(shù)的估值以某種導(dǎo)航系統(tǒng)輸出導(dǎo)航參數(shù)的誤差為主要形狀濾波器估值的主要部分即是導(dǎo)航參數(shù)誤差的估值目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法模型能夠是線性的，也能夠是非線性的模型普通都是線性的.252. 開環(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目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法開環(huán)輸出校正的卡爾

16、曼濾波器.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)，對誤差形狀進(jìn)展校正。IXIX目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法閉環(huán)反響校正的卡爾曼濾波器.2727 開環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波那么是校正系統(tǒng)內(nèi)部的形狀。兩種校正方法的性質(zhì)是一樣的，具有同樣的精度。 閉環(huán)濾波的反響校正使得卡爾曼濾波形狀值為小量；開環(huán)因無反響，形狀值會隨時(shí)間不斷變大。形狀方程都是經(jīng)過一階近似的線性方程，形狀的數(shù)值越小，那么近似

17、的準(zhǔn)確性越高，因此，利用形狀反響校正的系統(tǒng)形狀方程，更能接近真實(shí)地反映系統(tǒng)誤差形狀的動態(tài)過程。 卡爾曼濾波算法中，反響形狀估計(jì)的最正確時(shí)機(jī)是在丈量更新后立刻進(jìn)展。 卡爾曼濾波的閉環(huán)和開環(huán)可以混合運(yùn)用，即一些形狀估計(jì)作為校正值被反響，而另外一些不反響。目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法4. 開環(huán)與閉環(huán)卡爾曼濾波對比.285. 混合卡爾曼濾波例如28在松組合算法中，21維向量，其中：位置、速度和姿態(tài)只做開環(huán)修正；而IMU誤差，如陀螺和加速度計(jì)零偏，比例因子誤差進(jìn)展反響，修正IMU的原始觀測值。目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展

18、 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法GINS軟件松組合算法架構(gòu)表示圖混合濾波.292.3 濾波特性及濾波實(shí)現(xiàn)中的問題1. 濾波收斂特性初始不確定度平衡不確定度時(shí)間形狀不確定度收斂過程中的卡爾曼濾波形狀不確定度* 注：形狀不確定度是誤差協(xié)方差矩陣P對角元素的平方根 當(dāng)卡爾曼濾波形狀不確定度接近平衡點(diǎn)，每次丈量更新后形狀不確定度的降低量與系統(tǒng)噪聲呵斥的不確定度的添加量是匹配的； 在平衡點(diǎn)，不確定度所反映出的估計(jì)置信度程度根本固定.目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.30 形狀估計(jì)的收斂速度根本上取決于該形狀的

19、可觀測性。 假設(shè)觀測矩陣隨時(shí)間變化或者形狀之間經(jīng)過形狀轉(zhuǎn)移矩陣存在時(shí)間依存關(guān)系，那么隨著迭代次數(shù)的添加，更多的形狀量變得可觀測。 例：導(dǎo)航中用位置的變化率來確定速度 許多參數(shù)的可觀測性依賴于系統(tǒng)的動態(tài)性 例：姿態(tài)不變時(shí)，INS姿態(tài)誤差和加速度計(jì)偏向不是獨(dú)立可觀測的； 陀螺儀誤差那么需求載體有更高的動態(tài)性，方可觀測 假設(shè)兩個形狀對觀丈量有同樣的影響，以一樣方式隨時(shí)間變化，并且具有一樣的動態(tài)特性，那么它們非獨(dú)立可觀量，在濾波設(shè)計(jì)時(shí)，應(yīng)將其組合在一同，以免浪費(fèi)計(jì)算資源目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.312. 濾波參數(shù)調(diào)整形狀估計(jì)誤

20、差P/R太小形狀估計(jì)誤差形狀估計(jì)誤差P/R適中P/R太大時(shí)間時(shí)間時(shí)間目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法不同P/R比率下的卡爾曼濾波誤差傳送Paul Groves為抑制卡爾曼濾波模型的限制，噪聲建模必需足以囊括實(shí)踐系統(tǒng)的行為，籠統(tǒng)地說，要將真實(shí)世界的“方形楔子放到卡爾曼濾波模型的“圓洞中，因此這個洞要被擴(kuò)寬 。 例：假設(shè)忽略1 Hz GPS定位結(jié)果誤差的時(shí)間相關(guān)性，應(yīng)將其放大普通為2-3倍以建模為白噪聲.32 P陣非正定 P陣的非正定容易導(dǎo)致濾波發(fā)散。 運(yùn)用高精度變量存儲如double類型，減小舍入誤差； 縮放卡爾曼濾波標(biāo)度，使一

21、切形狀不確定度在數(shù)值上具有一樣量級； P陣非對稱 運(yùn)用式 計(jì)算P陣，容易導(dǎo)致P陣非對稱； 每次系統(tǒng)傳送及丈量更新后經(jīng)過 P=(P+P)/2來堅(jiān)持對稱性； 建議采用Joseph式P陣更新： 平方根濾波 傳送 而非P,可把動態(tài)范圍減小兩個量級，從而減小舍入誤差影響。kkkk/k 1P(IK H )P目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法 TTkkkk/k 1kkkkkP(IK H )P(IK H )K R K3. 數(shù)值計(jì)算問題.三、擴(kuò)展三、擴(kuò)展KalmanKalman濾波濾波EKFEKF2021-12-1433.34u 規(guī)范卡爾曼濾波的

22、線性假設(shè)u在規(guī)范的卡爾曼濾波中，觀測模型假設(shè)為線性Z是X的線性函數(shù)，但實(shí)踐情況往往并非如此如GNSS導(dǎo)航濾波器中，觀測模型是強(qiáng)非線性的u在規(guī)范卡爾曼濾波中，系統(tǒng)模型也被假設(shè)為線性的X的時(shí)間導(dǎo)數(shù)是X的線性函數(shù)u問題：在全形狀I(lǐng)NS/GNSS組合導(dǎo)航中，形狀量為絕對位置、速度和姿態(tài)，由于難以不斷堅(jiān)持系統(tǒng)的線性近似，完成一切的系統(tǒng)反響并不總是可行的；u 擴(kuò)展/線性化卡爾曼濾波u 為卡爾曼濾波的非線性方式目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法1. 問題描畫3.1 非線性系統(tǒng).35目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt

23、KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法2. 非線性系統(tǒng)描畫u普通非線性系統(tǒng)延續(xù)和離散系統(tǒng)可由以下方程來表示：u擴(kuò)展卡爾曼濾波研討的非線性系統(tǒng)可由以下方程來表示( )( ),( ), ( )( ), ( ), X tf X t W t tZ th X t V t t= &或11,1, kkkkkkXf XWkZh X V k-=-= 為簡化問題，需對噪聲的統(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

24、，Vt或Vk 均為彼此不相關(guān)的零均值白噪聲序列，與初始形狀X(0)或X0也不相關(guān) .36根本假設(shè)：非線性微分方程的實(shí)際解一定存在；實(shí)際解與實(shí)踐解差可以用一個線性微分方程表示，即線性擾動方程。目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法3. 處理方案.373.2 非線性系統(tǒng)的線性化t實(shí)踐形狀 X標(biāo)稱形狀 X*n圍繞X*線性化： 線性化卡爾曼濾波Linearized Kalman Filter， LKFn圍繞實(shí)踐形狀X濾波估計(jì)形狀，或?qū)嵺`軌跡進(jìn)展線性化：擴(kuò)展卡爾曼濾波Extended Kalman Filter， EKF目目 錄錄概述概述規(guī)范

25、規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法泰勒展開法進(jìn)展線性化形狀.38n線性化卡爾曼濾波：圍繞標(biāo)稱形狀對非線性系統(tǒng)進(jìn)展線性化或n當(dāng)Wt或Wk-1，Vt或Vk恒為0時(shí)，系統(tǒng)模型的解稱為非線性方程的實(shí)際解，又稱“標(biāo)稱軌跡或標(biāo)稱形狀。通常記為Xn(t)或Xkn,和Zn(t)或Zkn，那么有：( )( ), ( )( ), nnnnXtf Xt tZth Xt t1,1, nnkknkkXf XkZh Xk目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.393.3 線性化卡爾曼濾波或u 定義：u非線性系統(tǒng)的真軌

26、跡運(yùn)動與標(biāo)稱軌跡運(yùn)動的偏向?yàn)椋? )( )( )( )( )( )nnX tX tXtZ tZ tZtnkkknkkkXXXZZZn假設(shè)這些偏向足夠小，那么，可以圍繞標(biāo)稱形狀把X(t和Z(t展開成泰勒(Taylor)級數(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目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmi

27、dt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.403.3 線性化卡爾曼濾波或n那么有 ：( )( )( )( )( )( )( )( )( )( )( )nnnnX tXtFtX tG t W tZ tZtHtX tV t=+ D+=+ D+ &( )( )( )( )( )( )( )( )( )nnX tFtX tG t W tZ tHtX tV tD= D+D= D+ &目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法( )( )( )( )( )( )nnX tX tXtZ tZ tZtD=-D=- .n由于線性化卡爾

28、曼濾波存在上述問題，改用另一種近似方法，即在最優(yōu)化形狀估計(jì) 或 附近進(jìn)展泰勒展開，線性化。413.4 擴(kuò)展卡爾曼濾波n標(biāo)稱解難以解算n真軌跡與標(biāo)稱軌跡之間的形狀差X(t)或Xk不能確保其足夠小，從而導(dǎo)致線性化誤差較大，模型的線性近似度變?nèi)?。uLKF的缺陷kX( )X t目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.目目 錄錄42 概述 KF方程 EKF LKF或u 定義u非線性系統(tǒng)的真軌跡運(yùn)動與實(shí)踐軌跡運(yùn)動的偏向?yàn)椋? )( )( )( )( )( )X tX tX tZ tZ tZ t=-=-kkkkkkXXXZZZ=- =- n假設(shè)偏

29、向足夠小，那么可以圍繞最優(yōu)化形狀估計(jì)把X(t和Z(t展開成泰勒(Taylor)級數(shù)，并且可取一次近似值。( )( )( )( )( )( )( )( )( ), ( )( ), ( )( )( )( )( ), ( )( ), ( )( )( )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=+=+ &(

30、)()( )( )( )( )( )( )( )( )( )X tF tX tG t W tZ tH tX tV t=+=+nEKF假設(shè)形狀向量估計(jì)的誤差遠(yuǎn)比形狀向量本身小，因此可用線性系統(tǒng)模型計(jì)算形狀向量殘差。n規(guī)范的誤差協(xié)方差傳送公式可采用在最優(yōu)形狀估計(jì)附近進(jìn)展線性化的形狀轉(zhuǎn)移矩陣，即()1111( )expkkkxxkf xFFx-=-=目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.四、四、SchmidtSchmidt卡爾曼濾波卡爾曼濾波2021-12-1444.454.1 時(shí)間相關(guān)噪聲目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KF

31、Schmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法1. 觀測噪聲u 規(guī)范卡爾曼濾波的觀測噪聲假設(shè)u普通假設(shè)一切的觀測噪聲是時(shí)間不相關(guān)的，即觀測噪聲是白噪聲。u 問題：u該假設(shè)常不成立；卡爾曼濾波強(qiáng)迫把新息中的時(shí)間相關(guān)部分歸因形狀，因此，時(shí)間相關(guān)的觀測噪聲能夠會破壞形狀估計(jì)。u 處置觀測噪聲時(shí)間相關(guān)性問題的方法u 將時(shí)間相關(guān)噪聲擴(kuò)展至卡爾曼濾波的形狀向量中，進(jìn)展估計(jì)；u 放大觀測噪聲協(xié)方差矩陣R，進(jìn)而減小卡爾曼濾波的增益；u利用Schmidt卡爾曼濾波.46目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法2. 系統(tǒng)噪聲u 規(guī)范卡爾曼濾波的系

32、統(tǒng)噪聲假設(shè)u規(guī)范的卡爾曼濾波中，普通假設(shè)一切的系統(tǒng)噪聲是時(shí)間不相關(guān)的，即觀測噪聲是白噪聲。u 問題：u系統(tǒng)常含有顯著的系統(tǒng)性噪聲和其它時(shí)間相關(guān)噪聲；u這些噪聲能夠由于可觀測性較差未被選為形狀量；u但會影響被估計(jì)的形狀u 處置系統(tǒng)噪聲時(shí)間相關(guān)性問題的方法u 當(dāng)相關(guān)時(shí)間較短時(shí)：建模為白噪聲，但需覆蓋會影響卡爾曼濾波收斂的相關(guān)噪聲；u 當(dāng)相關(guān)時(shí)間超越1min：采用帶不確定參數(shù)的Schmidt卡爾曼濾波；.474.2 Schmidt卡爾曼濾波目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法Schmidt 卡爾曼濾波有效地增大誤差協(xié)方差陣P,以此來對時(shí)

33、間相關(guān)噪聲進(jìn)展建模,到達(dá)與卡爾曼濾波增益類似的效果.Schmidt卡爾曼濾波符號: - 時(shí)間相關(guān)系統(tǒng)噪聲和時(shí)間相關(guān)觀測噪聲的未估計(jì)參數(shù)的協(xié)方差陣.U -相關(guān)矩陣,是對未估計(jì)參數(shù)和形狀量之間相關(guān)性的建模 - 相關(guān)噪聲參數(shù)的轉(zhuǎn)移矩陣 -系統(tǒng)噪聲協(xié)方差陣 - 轉(zhuǎn)移矩陣,描畫形狀如何隨未知參數(shù)變化的模型.48目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法Schmidt 卡爾曼率濾波對應(yīng)的誤差協(xié)方差系統(tǒng)傳送式為:將誤差協(xié)方差P, 相關(guān)噪聲協(xié)方差W, 相關(guān)矩陣U分解為不同傳送方程:.49目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF

34、自順應(yīng)自順應(yīng) KF平滑算法平滑算法類似于觀測矩陣H, 定義J 為待估計(jì)參數(shù)到觀測向量的耦合矩陣,那么卡爾曼濾波增益變?yōu)?Schmidt 卡爾曼濾波誤差協(xié)方差, 相關(guān)噪聲和相關(guān)矩陣更新為:.50目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法從Schmidt卡爾曼濾波方程中可以看出:假設(shè)待估參數(shù)與觀測向量無直接相關(guān)時(shí), 那么 J 為零矩陣, 增益矩陣的方式與規(guī)范卡爾曼濾波一樣,但是誤差協(xié)方差變大了.假設(shè)沒有形狀隨待估計(jì)參數(shù)的變化而變化時(shí)，即形狀量不是待估參數(shù)的函數(shù)，那么零矩陣,此時(shí)誤差協(xié)方差陣的方式與規(guī)范卡爾曼濾波方式一樣;留意: 待估參數(shù)可像

35、形狀量一樣，影響系統(tǒng)模型和觀測模型.五、自順應(yīng)卡爾曼濾波五、自順應(yīng)卡爾曼濾波2021-12-1451.525. 自順應(yīng)卡爾曼濾波目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法1. 問題描畫多數(shù)運(yùn)用中，Kalman濾波中的系統(tǒng)噪聲協(xié)方差陣Q和觀測噪聲協(xié)方差陣R可事先經(jīng)過系統(tǒng)丈量、仿真和實(shí)驗(yàn)等方法確定。例：在INS/GNSS組合導(dǎo)航中，Q陣常由IMU的性能參數(shù)如VRW, ARW，零偏的一階高斯馬爾可夫過程參數(shù)確定GNSS噪聲由位置的點(diǎn)位中誤差或誤差建模 問題：有些情況上述參數(shù)是無法獲取的，例如：1MEMS IMU在出廠時(shí)未經(jīng)過嚴(yán)厲標(biāo)定；2假設(shè)沒

36、有飛機(jī)武器掛倉的先驗(yàn)振動環(huán)境信息，那么無法獲得觀測噪聲協(xié)方差陣； 處理方法Kalman濾波自行估計(jì)矩陣Q或/和R，即自順應(yīng)卡爾曼濾波.基于新息的自順應(yīng)估計(jì)Innovation Based Adaptive Estimation, IAE，要從丈量新息統(tǒng)計(jì)中計(jì)算Q、R。計(jì)算最后n個丈量信息的協(xié)方差上述協(xié)方差矩陣C用于計(jì)算R和/或Q當(dāng)處置第一組觀測新息統(tǒng)計(jì)值時(shí)，必需提供R和Q初始值，初始值的選定須謹(jǐn)慎53目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法2. 基于新息的自順應(yīng)估計(jì),1kTjjz kjknCzzn-=-=%,/1,Tkkkz kTkk

37、k kkz kQK CKRCH PH-=-%.多模型自順應(yīng)估計(jì)Multiple Model Adaptive Estimation, MMAE利用一組并行的卡爾曼濾波器進(jìn)展計(jì)算，每一個濾波器對應(yīng)于不同的系統(tǒng)噪聲和/或觀測噪聲協(xié)方差矩陣Q和R。第i個卡爾曼濾波器被分配的概率為隨時(shí)間推移，最優(yōu)濾波假設(shè)的概率會接近1，而其它的接近0；為充分利用計(jì)算處置才干，可剔除弱的濾波假設(shè)，并周期性地細(xì)分最強(qiáng)的假設(shè)用于精化濾波器參數(shù)。54目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法3. 多模型自順應(yīng)估計(jì).完好的形狀向量估計(jì)和誤差協(xié)方差計(jì)算公式如下： IAE新

38、息 和 MMAE多模型方法比較MMAE計(jì)算量大IAE中，Q,R,P和濾波增益均能夠是形狀估計(jì)的函數(shù)；而他們在MMAE濾波器組規(guī)范卡爾曼濾波而非EKF中是相互獨(dú)立的，因此MMAE濾波器更趨于穩(wěn)定55目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法.六、平滑算法六、平滑算法2021-12-1456.576. 6. 平滑算法平滑算法目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法1. 問題描畫u 實(shí)時(shí)處置/后處置差別u卡爾曼濾波器是為實(shí)時(shí)運(yùn)用而設(shè)計(jì)的，用于估計(jì)給定時(shí)辰的系統(tǒng)特性，所用新息是這一時(shí)辰

39、之前的系統(tǒng)可以觀丈量。u當(dāng)需求在某時(shí)間發(fā)生后獲得系統(tǒng)特性時(shí)，卡爾曼濾波仍舊只用1/2的觀測新息，由于卡爾曼濾波器沒有運(yùn)用所關(guān)注時(shí)間點(diǎn)之后的觀丈量u 卡爾曼平滑器u是卡爾曼濾波器的擴(kuò)展，不僅適用所關(guān)注時(shí)間點(diǎn)之前的觀測新息，且適用之后的新息；非實(shí)時(shí)運(yùn)用中，平滑處置可以得到更高的精度。u 常用平滑算法u包含兩類主要方法：1正向-方向?yàn)V波；2RTS平滑算法.58目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法2. 正向-反向?yàn)V波算法經(jīng)過正向?yàn)V波和反向?yàn)V波兩個相互獨(dú)立的濾波器結(jié)果的加權(quán)平均，平滑結(jié)果由下式給定：下標(biāo)f和b分別表示正向和反向；k表示兩個濾

40、波器中的一樣時(shí)間點(diǎn)正向-反向卡爾曼平滑算法的形狀不確定度正向?yàn)V波反向?yàn)V波形狀不確定度時(shí)間前向?yàn)V波.59目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法3. RTS平滑算法：RTSRauch,Tung and Striebel, RTS傳統(tǒng)的卡爾曼濾波隨時(shí)間向前運(yùn)轉(zhuǎn)，在每次系統(tǒng)傳送和丈量更新后記錄形狀向量X、誤差協(xié)方差陣P及形狀轉(zhuǎn)移矩陣 ，一旦運(yùn)算到數(shù)據(jù)的結(jié)尾，那么開場反向從末尾到起始點(diǎn)進(jìn)展數(shù)據(jù)平滑每次迭代的平滑增益為：平滑后的形狀向量和誤差協(xié)方差陣為：當(dāng)需對每個點(diǎn)進(jìn)展平滑時(shí)，RTS算法更有效，假設(shè)只需對單點(diǎn)進(jìn)展平滑時(shí)，正向-反向算法更有效.6

41、0附錄：卡爾曼濾波例如MATLAB源代碼目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(yīng) KF平滑算法平滑算法參考參考:Alex Blekhman, mathworks/matlabcentral/fileexchange% 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; % Moti

42、on equations% Previous state (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源代碼目目 錄錄概述概述規(guī)范規(guī)范 KF擴(kuò)展擴(kuò)展 KFSchmidt KF自順應(yīng)自順應(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 = sigm