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

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

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

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

狀態(tài)向量（狀態(tài)）是一組描述系統(tǒng)的參數(shù)。可以是常量，也可是時變量，是估計對象。與之相關(guān)聯(lián)的是誤差協(xié)方差矩陣，描述了狀態(tài)估計的不確定度及估計誤差間的相關(guān)度。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法8狀態(tài)向量（狀態(tài)）目錄81.4卡爾曼濾波的要素4個要素：2個模型、1組觀測量、1個算法2個模型系統(tǒng)模型也稱過程模型或者時間傳遞模型，描述了狀態(tài)與誤差協(xié)方差矩陣隨時間的變化特性。對于選定狀態(tài)量，系統(tǒng)模型是確定的。觀測模型描述了觀測向量與狀態(tài)向量間的函數(shù)關(guān)系。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法91.4卡爾曼濾波的要素4個要素：2個模型、1組觀測量、1個1組觀測向量是一組針對同一時刻的系統(tǒng)特性的測量值，例如觀測量可以包括GNSS系統(tǒng)的位置測量值，或者INS與GNSS位置結(jié)果的差值。1個算法：卡爾曼濾波算法使用觀測向量、觀測模型和系統(tǒng)模型來獲得狀態(tài)向量的最優(yōu)估計，分為系統(tǒng)傳遞和測量更新兩個部分。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法101組觀測向量目錄10目錄1.5卡爾曼濾波的導(dǎo)航應(yīng)用慣性導(dǎo)航系統(tǒng)（INS）的精對準(zhǔn)和標(biāo)定單一導(dǎo)航（GNSS,無線電、水聲學(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)方程和量測方程分別為：

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

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

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

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

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

目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法162.離散卡爾曼濾波方程狀態(tài)一步預(yù)測方程狀態(tài)估值計算方程濾波時間更新方程量測修正方程目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法17時間更新量測修正目錄17濾波計算回路增益計算回路目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法18濾波計算回路增益計算回路目錄183.卡爾曼濾波示例目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法有一個質(zhì)點，沿X軸正方向運動，質(zhì)點從X=0開始勻速直線運動,速度為V=10m/s，則每一時刻質(zhì)點的真實位置（參考真值）為：X=X0+V*t；實際上，我們每隔0.1s可以測量一次質(zhì)點的位置，但位置測量值存在誤差（假設(shè)是均值為0的白噪聲序列）根據(jù)我們對質(zhì)點的位置觀測量，用卡爾曼濾波方法計算每一時刻質(zhì)點的位置和速度Xv=10m/sXi-1XiXi+1位置觀測值193.卡爾曼濾波示例目錄有一個質(zhì)點，沿X軸正方向運目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法狀態(tài)量x=[X,V]，即以質(zhì)點的位置和速度作為卡爾曼濾波狀態(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ì)點運動模型，因此可認(rèn)為模型的不確定度為0，即Q=[00;00]觀測矩陣H：由于只觀測了質(zhì)點位置，未觀測速度，因此觀測矩陣H=[10];觀測噪聲矩陣R：位置觀測量的方差為m2，即R=1觀測量向量Z：在真實狀態(tài)（真實位置）加上均值為零，方差為m2的白噪聲;卡爾曼濾波初始狀態(tài)：X0=0,V0=5m/s，初始狀態(tài)誤差協(xié)方差矩陣P=[10;01]設(shè)計卡爾曼濾波20目錄狀態(tài)量x=[X,V]，即以質(zhì)點的位置和目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波位置估計21目錄卡爾曼濾波位置估計21目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波速度估計22目錄卡爾曼濾波速度估計222.2閉環(huán)卡爾曼濾波1.全狀態(tài)濾波和誤差狀態(tài)濾波根據(jù)卡爾曼濾波狀態(tài)向量的選取不同，卡爾曼濾波可分為:全狀態(tài)卡爾曼濾波（TotalStateImplementation）和誤差狀態(tài)卡爾曼濾波（ErrorStateImplementation）組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進行估計的主要對象導(dǎo)航參數(shù)

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

位置λ，L

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

的估值

反饋到慣導(dǎo)系統(tǒng)內(nèi)，對誤差狀態(tài)進行校正。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法閉環(huán)（反饋校正）的卡爾曼濾波器2022/12/30263.閉環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-采用開環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩種校正方法的性質(zhì)是一樣的，具有同樣的精度。閉環(huán)濾波的反饋校正使得卡爾曼濾波狀態(tài)值為小量；開環(huán)因無反饋，狀態(tài)值會隨時間不斷變大。狀態(tài)方程都是經(jīng)過一階近似的線性方程，狀態(tài)的數(shù)值越小，則近似的準(zhǔn)確性越高，因此，利用狀態(tài)反饋校正的系統(tǒng)狀態(tài)方程，更能接近真實地反映系統(tǒng)誤差狀態(tài)的動態(tài)過程?？柭鼮V波算法中，反饋狀態(tài)估計的最佳時機是在測量更新后立即進行?？柭鼮V波的閉環(huán)和開環(huán)可以混合使用，即一些狀態(tài)估計作為校正值被反饋，而另外一些不反饋。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法4.開環(huán)與閉環(huán)卡爾曼濾波對比2022/12/3027開環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩5.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：位置、速度和姿態(tài)只做開環(huán)修正；而IMU誤差，如陀螺和加速度計零偏，比例因子誤差進行反饋，修正IMU的原始觀測值。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法GINS軟件松組合算法架構(gòu)示意圖（混合濾波）2022/12/30285.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：目2.3濾波特性及濾波實現(xiàn)中的問題1.濾波收斂特性初始不確定度平衡不確定度時間狀態(tài)不確定度收斂過程中的卡爾曼濾波狀態(tài)不確定度*注：狀態(tài)不確定度是誤差協(xié)方差矩陣P對角元素的平方根當(dāng)卡爾曼濾波狀態(tài)不確定度接近平衡點，每次測量更新后狀態(tài)不確定度的降低量與系統(tǒng)噪聲造成的不確定度的增加量是匹配的；在平衡點，不確定度所反映出的估計置信度水平基本固定.目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法292.3濾波特性及濾波實現(xiàn)中的問題1.濾波收斂特性初始不確

狀態(tài)估計的收斂速度基本上取決于該狀態(tài)的可觀測性。如果觀測矩陣隨時間變化或者狀態(tài)之間通過狀態(tài)轉(zhuǎn)移矩陣存在時間依存關(guān)系，那么隨著迭代次數(shù)的增加，更多的狀態(tài)量變得可觀測。例：導(dǎo)航中用位置的變化率來確定速度許多參數(shù)的可觀測性依賴于系統(tǒng)的動態(tài)性例：姿態(tài)不變時，INS姿態(tài)誤差和加速度計偏差不是獨立可觀測的；陀螺儀誤差則需要載體有更高的動態(tài)性，方可觀測如果兩個狀態(tài)對觀測量有同樣的影響，以相同方式隨時間變化，并且具有相同的動態(tài)特性，則它們非獨立可觀量，在濾波設(shè)計時，應(yīng)將其組合在一起，以免浪費計算資源目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法30狀態(tài)估計的收斂速度基本上取決于該狀態(tài)的可觀測性。目2.濾波參數(shù)調(diào)整狀態(tài)估計誤差P/R太小狀態(tài)估計誤差狀態(tài)估計誤差P/R適中P/R太大時間時間時間目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法不同P/R比率下的卡爾曼濾波誤差傳遞（PaulGroves）為克服卡爾曼濾波模型的限制，噪聲建模必須足以囊括實際系統(tǒng)的行為，形象地說，要將真實世界的“方形楔子”放到卡爾曼濾波模型的“圓洞”中，因此這個洞要被擴寬。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2022/1目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法Schmidt卡爾曼率濾波對應(yīng)的誤差協(xié)方差系統(tǒng)傳遞式為:將誤差協(xié)方差P,相關(guān)噪聲協(xié)方差W,相關(guān)矩陣U分解為不同傳遞方程:2022/12/3048目錄Schmidt卡爾曼率濾波對應(yīng)的誤差協(xié)方差系目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法類似于觀測矩陣H,定義J為待估計參數(shù)到觀測向量的耦合矩陣,則卡爾曼濾波增益變?yōu)?Schmidt卡爾曼濾波誤差協(xié)方差,相關(guān)噪聲和相關(guān)矩陣更新為:2022/12/3049目錄類似于觀測矩陣H,定義J為待估計參數(shù)到觀測目錄概述標(biāo)準(zhǔn)KF擴展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擴展KFSchmidtKF自適應(yīng)KF平滑算法1.問題描述多數(shù)應(yīng)用中，Kalman濾波中的系統(tǒng)噪聲協(xié)方差陣Q和觀測噪聲協(xié)方差陣R可事先通過系統(tǒng)測量、仿真和實驗等方法確定。例：在INS/GNSS組合導(dǎo)航中，Q陣常由IMU的性能參數(shù)如VRW,ARW，零偏的一階高斯馬爾可夫過程參數(shù)確定GNSS噪聲由位置的點位中誤差或誤差建模問題：有些情況上述參數(shù)是無法獲取的，例如：1）MEMSIMU在出廠時未經(jīng)過嚴(yán)格標(biāo)定；2）如果沒有飛機武器掛倉的先驗振動環(huán)境信息，則無法獲得觀測噪聲協(xié)方差陣；解決辦法Kalman濾波自行估計矩陣Q或/和R，即自適應(yīng)卡爾曼濾波2022/12/30525.自適應(yīng)卡爾曼濾波目錄1.問題描述多數(shù)應(yīng)用中基于新息的自適應(yīng)估計（InnovationBasedAdaptiveEstimation,IAE），要從測量新息統(tǒng)計中計算Q、R。計算最后n個測量信息的協(xié)方差上述協(xié)方差矩陣C用于計算R和/或Q當(dāng)處理第一組觀測新息統(tǒng)計值時，必須提供R和Q初始值，初始值的選定須謹(jǐn)慎目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法2.基于新息的自適應(yīng)估計2022/12/3053基于新息的自適應(yīng)估計（InnovationBasedAd多模型自適應(yīng)估計（MultipleModelAdaptiveEstimation,MMAE）利用一組并行的卡爾曼濾波器進行計算，每一個濾波器對應(yīng)于不同的系統(tǒng)噪聲和/或觀測噪聲協(xié)方差矩陣Q和R。第i個卡爾曼濾波器被分配的概率為隨時間推移，最優(yōu)濾波假設(shè)的概率會接近1，而其它的接近0；為充分利用計算處理能力，可剔除弱的濾波假設(shè)，并周期性地細(xì)分最強的假設(shè)用于精化濾波器參數(shù)。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法3.多模型自適應(yīng)估計2022/12/3054多模型自適應(yīng)估計（MultipleModelAdapti完整的狀態(tài)向量估計和誤差協(xié)方差計算公式如下：

IAE（新息）

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

實時處理/后處理差異卡爾曼濾波器是為實時應(yīng)用而設(shè)計的，用于估計給定時刻的系統(tǒng)特性，所用新息是這一時刻之前的系統(tǒng)能夠觀測量。當(dāng)需要在某時間發(fā)生后獲得系統(tǒng)特性時，卡爾曼濾波仍舊只用1/2的觀測新息，因為卡爾曼濾波器沒有使用所關(guān)注時間點之后的觀測量

卡爾曼平滑器是卡爾曼濾波器的擴展，不僅適用所關(guān)注時間點之前的觀測新息，且適用之后的新息；非實時應(yīng)用中，平滑處理能夠得到更高的精度。常用平滑算法包含兩類主要方法：1）正向-方向濾波；2）RTS平滑算法576.平滑算法目錄1.問題描述實時處理/后處理目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法2.正向-反向濾波算法通過正向濾波和反向濾波（兩個相互獨立的濾波器）結(jié)果的加權(quán)平均，平滑結(jié)果由下式給定：下標(biāo)f和b分別表示正向和反向；k表示兩個濾波器中的相同時間點正向-反向卡爾曼平滑算法的狀態(tài)不確定度正向濾波反向濾波狀態(tài)不確定度時間前向濾波2022/12/3058目錄2.正向-反向濾波算法通過正向濾波和反向濾波目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法3.RTS平滑算法：RTS（Rauch,TungandStriebel,RTS）傳統(tǒng)的卡爾曼濾波隨時間向前運行，在每次系統(tǒng)傳遞和測量更新后記錄狀態(tài)向量X、誤差協(xié)方差陣P及狀態(tài)轉(zhuǎn)移矩陣，一旦運算到數(shù)據(jù)的結(jié)尾，則開始反向從末尾到起始點進行數(shù)據(jù)平滑每次迭代的平滑增益為：平滑后的狀態(tài)向量和誤差協(xié)方差陣為：當(dāng)需對每個點進行平滑時，RTS算法更有效，若只需對單點進行平滑時，正向-反向算法更有效

2022/12/3059目錄3.RTS平滑算法：RTS（Rauch,Tu附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴展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擴展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擴展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擴展KFSchmidtKF自適應(yīng)KF平滑算法figure;plot(t,Xtrue,'k-.',t,Z_buffer,'r.',t,Xk_buffer(1,:),'b');

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

2022/12/3063陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄figu附錄：卡爾曼濾波示例MATLAB源代碼目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法%==VelocityanalysisInstantaneousVelocity=[0(Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples))/dt];%滑動平均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('時間(s)','fontsize',15);ylabel('速度(m/s)','fontsize',15);legend('參考真值','位置觀測量微分','滑動平均法','卡爾曼濾波',15);set(gca,'fontsize',15)gridon2022/12/3064陳起金|卡爾曼濾波|卡爾曼濾波附錄：卡爾曼濾波示例MATLAB源代碼目錄%==：卡爾曼濾波：卡爾曼濾波目

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

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

狀態(tài)向量（狀態(tài)）是一組描述系統(tǒng)的參數(shù)?？梢允浅Ａ浚部墒菚r變量，是估計對象。與之相關(guān)聯(lián)的是誤差協(xié)方差矩陣，描述了狀態(tài)估計的不確定度及估計誤差間的相關(guān)度。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法72狀態(tài)向量（狀態(tài)）目錄81.4卡爾曼濾波的要素4個要素：2個模型、1組觀測量、1個算法2個模型系統(tǒng)模型也稱過程模型或者時間傳遞模型，描述了狀態(tài)與誤差協(xié)方差矩陣隨時間的變化特性。對于選定狀態(tài)量，系統(tǒng)模型是確定的。觀測模型描述了觀測向量與狀態(tài)向量間的函數(shù)關(guān)系。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法731.4卡爾曼濾波的要素4個要素：2個模型、1組觀測量、1個1組觀測向量是一組針對同一時刻的系統(tǒng)特性的測量值，例如觀測量可以包括GNSS系統(tǒng)的位置測量值，或者INS與GNSS位置結(jié)果的差值。1個算法：卡爾曼濾波算法使用觀測向量、觀測模型和系統(tǒng)模型來獲得狀態(tài)向量的最優(yōu)估計，分為系統(tǒng)傳遞和測量更新兩個部分。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法741組觀測向量目錄10目錄1.5卡爾曼濾波的導(dǎo)航應(yīng)用慣性導(dǎo)航系統(tǒng)（INS）的精對準(zhǔn)和標(biāo)定單一導(dǎo)航（GNSS,無線電、水聲學(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)方程和量測方程分別為：

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

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

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

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

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

目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法802.離散卡爾曼濾波方程狀態(tài)一步預(yù)測方程狀態(tài)估值計算方程濾波時間更新方程量測修正方程目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法81時間更新量測修正目錄17濾波計算回路增益計算回路目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法82濾波計算回路增益計算回路目錄183.卡爾曼濾波示例目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法有一個質(zhì)點，沿X軸正方向運動，質(zhì)點從X=0開始勻速直線運動,速度為V=10m/s，則每一時刻質(zhì)點的真實位置（參考真值）為：X=X0+V*t；實際上，我們每隔0.1s可以測量一次質(zhì)點的位置，但位置測量值存在誤差（假設(shè)是均值為0的白噪聲序列）根據(jù)我們對質(zhì)點的位置觀測量，用卡爾曼濾波方法計算每一時刻質(zhì)點的位置和速度Xv=10m/sXi-1XiXi+1位置觀測值833.卡爾曼濾波示例目錄有一個質(zhì)點，沿X軸正方向運目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法狀態(tài)量x=[X,V]，即以質(zhì)點的位置和速度作為卡爾曼濾波狀態(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ì)點運動模型，因此可認(rèn)為模型的不確定度為0，即Q=[00;00]觀測矩陣H：由于只觀測了質(zhì)點位置，未觀測速度，因此觀測矩陣H=[10];觀測噪聲矩陣R：位置觀測量的方差為m2，即R=1觀測量向量Z：在真實狀態(tài)（真實位置）加上均值為零，方差為m2的白噪聲;卡爾曼濾波初始狀態(tài)：X0=0,V0=5m/s，初始狀態(tài)誤差協(xié)方差矩陣P=[10;01]設(shè)計卡爾曼濾波84目錄狀態(tài)量x=[X,V]，即以質(zhì)點的位置和目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波位置估計85目錄卡爾曼濾波位置估計21目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法卡爾曼濾波速度估計86目錄卡爾曼濾波速度估計222.2閉環(huán)卡爾曼濾波1.全狀態(tài)濾波和誤差狀態(tài)濾波根據(jù)卡爾曼濾波狀態(tài)向量的選取不同，卡爾曼濾波可分為:全狀態(tài)卡爾曼濾波（TotalStateImplementation）和誤差狀態(tài)卡爾曼濾波（ErrorStateImplementation）組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進行估計的主要對象導(dǎo)航參數(shù)

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

位置λ，L

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

的估值

反饋到慣導(dǎo)系統(tǒng)內(nèi)，對誤差狀態(tài)進行校正。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法閉環(huán)（反饋校正）的卡爾曼濾波器2022/12/30903.閉環(huán)卡爾曼濾波慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-采用開環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩種校正方法的性質(zhì)是一樣的，具有同樣的精度。閉環(huán)濾波的反饋校正使得卡爾曼濾波狀態(tài)值為小量；開環(huán)因無反饋，狀態(tài)值會隨時間不斷變大。狀態(tài)方程都是經(jīng)過一階近似的線性方程，狀態(tài)的數(shù)值越小，則近似的準(zhǔn)確性越高，因此，利用狀態(tài)反饋校正的系統(tǒng)狀態(tài)方程，更能接近真實地反映系統(tǒng)誤差狀態(tài)的動態(tài)過程?？柭鼮V波算法中，反饋狀態(tài)估計的最佳時機是在測量更新后立即進行。卡爾曼濾波的閉環(huán)和開環(huán)可以混合使用，即一些狀態(tài)估計作為校正值被反饋，而另外一些不反饋。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法4.開環(huán)與閉環(huán)卡爾曼濾波對比2022/12/3091開環(huán)濾波僅校正系統(tǒng)輸出量，閉環(huán)濾波則是校正系統(tǒng)內(nèi)部的狀態(tài)。兩5.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：位置、速度和姿態(tài)只做開環(huán)修正；而IMU誤差，如陀螺和加速度計零偏，比例因子誤差進行反饋，修正IMU的原始觀測值。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法GINS軟件松組合算法架構(gòu)示意圖（混合濾波）2022/12/30925.混合卡爾曼濾波示例在松組合算法中，21維向量，其中：目2.3濾波特性及濾波實現(xiàn)中的問題1.濾波收斂特性初始不確定度平衡不確定度時間狀態(tài)不確定度收斂過程中的卡爾曼濾波狀態(tài)不確定度*注：狀態(tài)不確定度是誤差協(xié)方差矩陣P對角元素的平方根當(dāng)卡爾曼濾波狀態(tài)不確定度接近平衡點，每次測量更新后狀態(tài)不確定度的降低量與系統(tǒng)噪聲造成的不確定度的增加量是匹配的；在平衡點，不確定度所反映出的估計置信度水平基本固定.目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法932.3濾波特性及濾波實現(xiàn)中的問題1.濾波收斂特性初始不確

狀態(tài)估計的收斂速度基本上取決于該狀態(tài)的可觀測性。如果觀測矩陣隨時間變化或者狀態(tài)之間通過狀態(tài)轉(zhuǎn)移矩陣存在時間依存關(guān)系，那么隨著迭代次數(shù)的增加，更多的狀態(tài)量變得可觀測。例：導(dǎo)航中用位置的變化率來確定速度許多參數(shù)的可觀測性依賴于系統(tǒng)的動態(tài)性例：姿態(tài)不變時，INS姿態(tài)誤差和加速度計偏差不是獨立可觀測的；陀螺儀誤差則需要載體有更高的動態(tài)性，方可觀測如果兩個狀態(tài)對觀測量有同樣的影響，以相同方式隨時間變化，并且具有相同的動態(tài)特性，則它們非獨立可觀量，在濾波設(shè)計時，應(yīng)將其組合在一起，以免浪費計算資源目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法94狀態(tài)估計的收斂速度基本上取決于該狀態(tài)的可觀測性。目2.濾波參數(shù)調(diào)整狀態(tài)估計誤差P/R太小狀態(tài)估計誤差狀態(tài)估計誤差P/R適中P/R太大時間時間時間目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法不同P/R比率下的卡爾曼濾波誤差傳遞（PaulGroves）為克服卡爾曼濾波模型的限制，噪聲建模必須足以囊括實際系統(tǒng)的行為，形象地說，要將真實世界的“方形楔子”放到卡爾曼濾波模型的“圓洞”中，因此這個洞要被擴寬。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2022/1目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法Schmidt卡爾曼率濾波對應(yīng)的誤差協(xié)方差系統(tǒng)傳遞式為:將誤差協(xié)方差P,相關(guān)噪聲協(xié)方差W,相關(guān)矩陣U分解為不同傳遞方程:2022/12/30112目錄Schmidt卡爾曼率濾波對應(yīng)的誤差協(xié)方差系目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法類似于觀測矩陣H,定義J為待估計參數(shù)到觀測向量的耦合矩陣,則卡爾曼濾波增益變?yōu)?Schmidt卡爾曼濾波誤差協(xié)方差,相關(guān)噪聲和相關(guān)矩陣更新為:2022/12/30113目錄類似于觀測矩陣H,定義J為待估計參數(shù)到觀測目錄概述標(biāo)準(zhǔn)KF擴展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擴展KFSchmidtKF自適應(yīng)KF平滑算法1.問題描述多數(shù)應(yīng)用中，Kalman濾波中的系統(tǒng)噪聲協(xié)方差陣Q和觀測噪聲協(xié)方差陣R可事先通過系統(tǒng)測量、仿真和實驗等方法確定。例：在INS/GNSS組合導(dǎo)航中，Q陣常由IMU的性能參數(shù)如VRW,ARW，零偏的一階高斯馬爾可夫過程參數(shù)確定GNSS噪聲由位置的點位中誤差或誤差建模問題：有些情況上述參數(shù)是無法獲取的，例如：1）MEMSIMU在出廠時未經(jīng)過嚴(yán)格標(biāo)定；2）如果沒有飛機武器掛倉的先驗振動環(huán)境信息，則無法獲得觀測噪聲協(xié)方差陣；解決辦法Kalman濾波自行估計矩陣Q或/和R，即自適應(yīng)卡爾曼濾波2022/12/301165.自適應(yīng)卡爾曼濾波目錄1.問題描述多數(shù)應(yīng)用中基于新息的自適應(yīng)估計（InnovationBasedAdaptiveEstimation,IAE），要從測量新息統(tǒng)計中計算Q、R。計算最后n個測量信息的協(xié)方差上述協(xié)方差矩陣C用于計算R和/或Q當(dāng)處理第一組觀測新息統(tǒng)計值時，必須提供R和Q初始值，初始值的選定須謹(jǐn)慎目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法2.基于新息的自適應(yīng)估計2022/12/30117基于新息的自適應(yīng)估計（InnovationBasedAd多模型自適應(yīng)估計（MultipleModelAdaptiveEstimation,MMAE）利用一組并行的卡爾曼濾波器進行計算，每一個濾波器對應(yīng)于不同的系統(tǒng)噪聲和/或觀測噪聲協(xié)方差矩陣Q和R。第i個卡爾曼濾波器被分配的概率為隨時間推移，最優(yōu)濾波假設(shè)的概率會接近1，而其它的接近0；為充分利用計算處理能力，可剔除弱的濾波假設(shè)，并周期性地細(xì)分最強的假設(shè)用于精化濾波器參數(shù)。目錄概述標(biāo)準(zhǔn)KF擴展KFSchmidtKF自適應(yīng)KF平滑算法3.多模型自適應(yīng)估計2022/12/30118多模型自適應(yīng)估計（MultipleModelAdapti完整的狀態(tài)向量估計和誤差協(xié)方差計算公式如下：

IAE（新息）

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