濾波與組合導(dǎo)航原理

無(wú)人系統(tǒng)導(dǎo)航定位技術(shù)

----卡爾曼濾波與組合導(dǎo)航技術(shù)

主講：申強(qiáng)機(jī)電工程與控制國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室412

主要學(xué)習(xí)內(nèi)容

最優(yōu)估計(jì)與卡爾曼濾波組合導(dǎo)航基本原理和方法學(xué)習(xí)參考資料1.秦永元.卡爾曼濾波與組合導(dǎo)航原理.西北工業(yè)大學(xué)出版社2.付夢(mèng)印等.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用3.王志賢編著.最優(yōu)狀態(tài)估計(jì)與系統(tǒng)辨識(shí).西北工業(yè)大學(xué)出版社.卡爾曼魯?shù)婪颉た柭≧udolfEmilKalman），匈牙利裔美國(guó)數(shù)學(xué)家，1930年出生于匈牙利首都布達(dá)佩斯。1953年于麻省理工學(xué)院獲得電機(jī)工程學(xué)士，翌年碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。1964年至1971年任職斯坦福大學(xué)。1971年至1992年任佛羅里達(dá)大學(xué)數(shù)學(xué)系統(tǒng)理論中心（CenterforMathematicalSystemTheory）主任。1972起任瑞士蘇黎世聯(lián)邦理工學(xué)院數(shù)學(xué)系統(tǒng)理論中心主任直至退休。先居住于蘇黎世和佛羅里達(dá)。2009年獲美國(guó)國(guó)家科學(xué)獎(jiǎng)?wù)??？柭鼮V波的一個(gè)典型實(shí)例是從一組有限的，包含噪聲的，對(duì)物體位置的觀察序列（可能有偏差）預(yù)測(cè)出物體的位置的坐標(biāo)及速度。在很多工程應(yīng)用(如雷達(dá)、計(jì)算機(jī)視覺(jué))中都可以找到它的身影。同時(shí)，卡爾曼濾波也是控制理論以及控制系統(tǒng)工程中的一個(gè)重要課題。當(dāng)輸入為帶有高斯白噪聲的信號(hào)時(shí)，使期望輸出和實(shí)際輸出之間的均方根誤差達(dá)到最小的線性系統(tǒng)，這種濾波方法以它的發(fā)明者魯?shù)婪?E.卡爾曼的名字命名為卡爾曼濾波。1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念估計(jì)的概念待求系統(tǒng)狀態(tài)測(cè)量得出的數(shù)據(jù)測(cè)量噪聲X(t)的估計(jì)是Z(t)的函數(shù),若為線性函數(shù),則稱作X(t)的線性估計(jì)解算1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念預(yù)測(cè)和平滑tt0t1當(dāng)t=t1時(shí),稱為X(t)的估計(jì)；設(shè)在[t0，t1]時(shí)間段內(nèi)量測(cè)為Z，待求狀態(tài)為當(dāng)t>t1時(shí),稱為X(t)的預(yù)測(cè)；Z計(jì)算1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

某一指標(biāo)函數(shù)最小達(dá)到若以量測(cè)估計(jì)的偏差的平方和達(dá)到最小為指標(biāo)則所得估計(jì)為最小二乘估計(jì)！1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念最小二乘估計(jì)

該方法由高斯(KarlGauss)在1795年測(cè)定行星軌道而提出的參數(shù)估計(jì)算法。該算法特點(diǎn)是簡(jiǎn)單，不必知道被估計(jì)量及量測(cè)值相關(guān)的任何統(tǒng)計(jì)信息。原理：誤差平方和最小。隨機(jī)量測(cè)噪聲量測(cè)矩陣量測(cè)向量被估計(jì)向量1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

指標(biāo)函數(shù)：

最小二乘估計(jì)

1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念優(yōu)點(diǎn)：算法簡(jiǎn)單，不必知道量測(cè)誤差的統(tǒng)計(jì)信息；局限性：（1）只能估計(jì)確定性的常值向量，無(wú)法估計(jì)隨機(jī)向量的時(shí)間過(guò)程；（2）最優(yōu)指標(biāo)只保證了量測(cè)的估計(jì)均方誤差之和最小，而并未確保被估計(jì)量的估計(jì)誤差達(dá)到最佳，所以估計(jì)精度不高。

最小二乘估計(jì)的特點(diǎn)：

1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念原理：被估計(jì)量估計(jì)誤差方差最小。設(shè)為隨機(jī)向量，為的量測(cè)向量，即，求的估計(jì)就是根據(jù)解算出，顯然是的函數(shù)，由于是隨機(jī)誤差，所以無(wú)法從的函數(shù)關(guān)系式中直接求取，而必須按統(tǒng)計(jì)意義的最優(yōu)標(biāo)準(zhǔn)求取。

最小方差估計(jì)

最小方差估計(jì)等于量測(cè)為某一具體實(shí)現(xiàn)條件下的條件均值：定理11最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

最小方差估計(jì)

最小方差估計(jì)是的無(wú)偏估計(jì)。定理2定理3若被估計(jì)向量和量測(cè)向量都服從正態(tài)分布，且則的最小方差估計(jì)為：1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

最小方差估計(jì)

估計(jì)的均方誤差為：對(duì)于線性關(guān)系：和互不相關(guān)，則:1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

最小方差估計(jì)

還可寫成：例：設(shè)為服從正態(tài)分布的隨機(jī)量，均值為方差為，對(duì)用臺(tái)儀器同時(shí)直接測(cè)量，測(cè)量誤差都是服從正態(tài)分布的隨機(jī)變量，均值為零，方差為，求的最小方差估計(jì)和估計(jì)的均方差。1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

最小方差估計(jì)

根據(jù)題意，量測(cè)方程為：根據(jù)公式有：1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

極大驗(yàn)后估計(jì)

設(shè)為隨機(jī)向量，為的量測(cè)，為條件下的條件概率密度（亦稱的驗(yàn)后概率密度）。如果估計(jì)值使下列指標(biāo)滿足則稱為的極大驗(yàn)后估計(jì)。定理4

如果和都服從正態(tài)分布，則的極大驗(yàn)后估計(jì)與最小方差估計(jì)相等。1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

貝葉斯估計(jì)

設(shè)為被估計(jì)量，是的量測(cè)量，是根據(jù)給出的對(duì)的估計(jì)，為估計(jì)誤差，如果標(biāo)量函數(shù)具有性質(zhì)（1）當(dāng)時(shí)，（2）當(dāng)時(shí)，（3）則稱為對(duì)被估計(jì)量的損失函數(shù)，也稱代價(jià)函數(shù)，并稱其期望值為的貝葉斯風(fēng)險(xiǎn)。使貝葉斯風(fēng)險(xiǎn)達(dá)到最小的估計(jì)稱為貝葉斯估計(jì)，記為1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

極大似然估計(jì)

設(shè)為被估計(jì)量，為的量測(cè)，為條件下的條件概率密度，稱為的似然函數(shù)。使似然函數(shù)最大的估計(jì)量為最大似然估計(jì)，記為。1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

線性最小方差估計(jì)

如果將估值規(guī)定為量測(cè)矢量的線性函數(shù)，即式中A和b分別是（n×m）階和n維的矩陣和矢量。這樣的估計(jì)方法稱為線性最小方差估計(jì)?？勺C明，這種估計(jì)只需要被估計(jì)值X和量測(cè)值Z的一、二階統(tǒng)計(jì)特性，所以，它比最小方差估計(jì)較為實(shí)用。

1最優(yōu)估計(jì)與卡爾曼濾波1.1最優(yōu)估計(jì)的基本概念

各種最優(yōu)估計(jì)的比較估計(jì)方法特點(diǎn)最小二乘法對(duì)象：常值向量或隨機(jī)向量；好處：不需要統(tǒng)計(jì)信息，算法簡(jiǎn)單缺點(diǎn)：精度不高。最小方差估計(jì)是所有估計(jì)中的最佳。求取條件均值需求條件概率密度，難！

極大驗(yàn)后、貝葉斯、極大似然除正態(tài)分布，計(jì)算概率密度十分困難。常用于故障檢測(cè)和識(shí)別的算法中。

線性最小方差估計(jì)所有線性估計(jì)中最優(yōu)，只需求取被估計(jì)量和量測(cè)量的一階和二階矩。1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

卡爾曼濾波特點(diǎn)

線性最小方差估計(jì)的問(wèn)題：平穩(wěn)過(guò)程——簡(jiǎn)單，因?yàn)槠湟浑A、二階矩皆為常值。非平穩(wěn)過(guò)程---復(fù)雜，因?yàn)槠湟浑A、二階矩隨時(shí)間變化，難以適用！

1960年由卡爾曼（R.E.Kalman）首次提出，是一種線性最小方差估計(jì)，其特點(diǎn)：（1）算法是遞推的，且使用狀態(tài)空間法在時(shí)域內(nèi)設(shè)計(jì)濾波器，所以卡爾曼濾波適用于對(duì)多維隨機(jī)過(guò)程的估計(jì)。1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波在k時(shí)刻以前估值的基礎(chǔ)上，根據(jù)k時(shí)刻的量測(cè)值Zk,遞推得到k時(shí)刻的狀態(tài)估計(jì)值

：根據(jù)k-1時(shí)刻以前所有的量測(cè)值得到X（k）也可以說(shuō)是綜合利用k時(shí)刻以前的所有量測(cè)值得到的一次僅處理一個(gè)量測(cè)量計(jì)算量大大減小

主要適用于線性動(dòng)態(tài)系統(tǒng)！1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

離散卡爾曼濾波數(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è)噪聲1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波Qk和Rk分別稱為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，在卡爾曼濾波中要求它們分別是已知值的非負(fù)定陣和正定陣；δkj是Kroneckerδ函數(shù)，即：卡爾曼濾波要求{Wk}和{Vk}是互不相關(guān)的零均值的白噪聲序列，有：1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波Var{·}為對(duì){·}求方差的符號(hào)卡爾曼濾波要求mx0和Cx0為已知量，初始狀態(tài)的一、二階統(tǒng)計(jì)特性為：且要求X0與{Wk}和{Vk}都不相關(guān)1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

離散卡爾曼濾波方程或狀態(tài)一步預(yù)測(cè)方程狀態(tài)估值計(jì)算方程濾波增益方程一步預(yù)測(cè)均方差方程估計(jì)均方差方程1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

離散卡爾曼濾波方程時(shí)間修正方程量測(cè)修正方程1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

離散卡爾曼濾波方程物理意義（1）狀態(tài)一步預(yù)測(cè)方程Xk-1的卡爾曼濾波估值利用Xk-1計(jì)算得到的一步預(yù)測(cè)

也可以認(rèn)為是利用k-1時(shí)刻和以前時(shí)刻的量測(cè)值得到的Xk的一步預(yù)測(cè)

1最優(yōu)估計(jì)與卡爾曼濾波

上式就是通過(guò)計(jì)算新息，把估計(jì)出來(lái)，并左乘一個(gè)系數(shù)矩陣加到中，從而得到估值和，稱為濾波增益矩陣（2）狀態(tài)估值計(jì)算方程

計(jì)算估值Xk的方程。它是在一步預(yù)測(cè)Xk/k-1的基礎(chǔ)上，根據(jù)量測(cè)值Zk計(jì)算出來(lái)的一步預(yù)測(cè)誤差若把看作是量測(cè)的一步預(yù)測(cè)，則就是量測(cè)的一步預(yù)測(cè)誤差由兩部分組成：和，正是在基礎(chǔ)上估計(jì)所需信息，因此又稱為新息1最優(yōu)估計(jì)與卡爾曼濾波（3）濾波增益方程Kk選取的標(biāo)準(zhǔn)就是卡爾曼濾波的估計(jì)準(zhǔn)則，也就是使得

均方誤差陣最?。河捎谝簿哂袩o(wú)偏性，即的均值為零，所以也稱為一步預(yù)測(cè)誤差方差陣。上式中的和分別就是新息中的兩部分內(nèi)容一步預(yù)測(cè)均方差陣，即：如果Rk大，Kk就小Rk小，Kk就大由于也具有無(wú)偏性，即的均值為零，所以也稱為一步預(yù)測(cè)誤差方差陣。上式中的和分別就是新息中的兩部分內(nèi)容一步預(yù)測(cè)均方差陣，即：如果Rk大，Kk就小Rk小，Kk就大1最優(yōu)估計(jì)與卡爾曼濾波（4）一步預(yù)測(cè)均方誤差方程從下式可以看出，求Kk必須先求出Pk/k-1式中，為

的估計(jì)誤差，可以看出一步預(yù)測(cè)均方誤差陣Pk/k-1是從估計(jì)均方誤差陣Pk-1轉(zhuǎn)移過(guò)來(lái)的，并且再加上系統(tǒng)噪聲方差的影響。

的均方誤差陣，即：1最優(yōu)估計(jì)與卡爾曼濾波（5）估計(jì)均方誤差方程或計(jì)算量小，但在計(jì)算機(jī)有舍入誤差的條件下，不能始終保證算出的Pk是對(duì)稱的

1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

離散卡爾曼濾波計(jì)算流程濾波計(jì)算回路增益計(jì)算回路1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

離散卡爾曼濾波初值確定在濾波開始時(shí)，必須有初始值

和

才能進(jìn)行為了保證估值的無(wú)偏性，應(yīng)選擇：這樣才能保證估計(jì)均方差陣Pk始終最小。1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化線性時(shí)變系統(tǒng)離散系統(tǒng)其中，系統(tǒng)的驅(qū)動(dòng)源為白噪聲過(guò)程，即

為的方差強(qiáng)度陣

為系統(tǒng)噪聲方差陣

為的方差強(qiáng)度陣

為的方差強(qiáng)度陣1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化根據(jù)連續(xù)系統(tǒng)的系統(tǒng)矩陣

F(t)計(jì)算出離散系統(tǒng)的轉(zhuǎn)移矩陣

ΦK/K-1根據(jù)連續(xù)系統(tǒng)的系統(tǒng)噪聲方差強(qiáng)度陣q(t)計(jì)算出離散系統(tǒng)噪聲方差陣

Qk1）ΦK/K-1

的計(jì)算

根據(jù)線性系統(tǒng)理論，線性時(shí)變連續(xù)系統(tǒng)的解為：1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化

因此，其離散形式可以寫成：

為狀態(tài)轉(zhuǎn)移矩陣，具有如下性質(zhì)：

特別的：線性定常系統(tǒng)，其狀態(tài)轉(zhuǎn)移矩陣為：1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化式中：為濾波器的計(jì)算周期

如果計(jì)算周期T遠(yuǎn)小于系統(tǒng)陣F(t)發(fā)生明顯變化所需要的時(shí)間，則ΦK/K-1可以利用定常系統(tǒng)的計(jì)算方法，即

1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化

另外，如果F(t)在計(jì)算周期T內(nèi)變化比較劇烈，則將計(jì)算周期分為個(gè)連續(xù)的子周期。在內(nèi)每隔就能得到系統(tǒng)矩陣的采樣值：

則一步轉(zhuǎn)移矩陣1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化

則：2）Qk的計(jì)算

在內(nèi)，取并記：1最優(yōu)估計(jì)與卡爾曼濾波1.2離散卡爾曼濾波

動(dòng)力學(xué)方程為連續(xù)微分方程時(shí)的離散化1最優(yōu)估計(jì)與卡爾曼濾波1.3連續(xù)系統(tǒng)卡爾曼濾波系統(tǒng)矩陣系統(tǒng)噪聲矩陣系統(tǒng)噪聲向量量測(cè)矩陣噪聲矩陣

為的方差強(qiáng)度陣

為的方差強(qiáng)度陣1最優(yōu)估計(jì)與卡爾曼濾波1.3連續(xù)系統(tǒng)卡爾曼濾波連續(xù)系統(tǒng)卡爾曼濾波形式：

黎卡蒂（Riccati)方程1最優(yōu)估計(jì)與卡爾曼濾波1.4連續(xù)-離散卡爾曼濾波

實(shí)際被估計(jì)狀態(tài)的系統(tǒng)經(jīng)常是連續(xù)系統(tǒng)，而量測(cè)是間隔時(shí)間的，這種被估計(jì)對(duì)象常稱為連續(xù)—離散系統(tǒng)。則系統(tǒng)方程和量測(cè)方程分別：1最優(yōu)估計(jì)與卡爾曼濾波1.4連續(xù)-離散卡爾曼濾波濾波方程：量測(cè)修正方程時(shí)間修正方程:可以采用微分方程的數(shù)值解法來(lái)求解，也可以用離散化的方法求解1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

一般的非線性系統(tǒng)（連續(xù)）和離散系統(tǒng)的方程可由以下形式描述:或

如果或，或的概率分布是任意的，那么上述系統(tǒng)所描述的將是屬于非常一般地隨機(jī)非線性系統(tǒng)。這類系統(tǒng)的最優(yōu)估計(jì)問(wèn)題的求解非常困難。為了簡(jiǎn)化問(wèn)題分析，必須對(duì)噪聲的統(tǒng)計(jì)特性給以符合實(shí)際又便于處理的假定。1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

這里研究的非線性最優(yōu)估計(jì)問(wèn)題的隨機(jī)非線性系統(tǒng)的數(shù)學(xué)模型屬于如下類型:或

其中或，或的概率分布是彼此不相關(guān)的零均值白噪聲序列，且它們與初始狀態(tài)或也不相關(guān)。

目前解決此類問(wèn)題的主要方法是將非線系統(tǒng)線性化。1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

基本假設(shè)：非線性微分方程的理論解一定存在，而且這個(gè)理論解與實(shí)際解之間的差能夠用一個(gè)線性微分方程表示，稱為“線性干擾方程”，“小偏差方程”，“攝動(dòng)方程”。1.5.1圍繞標(biāo)稱狀態(tài)的線性化當(dāng)系統(tǒng)噪聲和量測(cè)噪聲恒為0時(shí)，上述系統(tǒng)模型的解稱為非線性方程的理論解，又稱“標(biāo)稱軌跡”或標(biāo)稱狀態(tài)。通常記為或，和或，則有1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

非線性系統(tǒng)的真軌跡運(yùn)動(dòng)與標(biāo)稱軌跡運(yùn)動(dòng)的偏差為：或

如果這些偏差足夠小，那么，可以圍繞標(biāo)稱狀態(tài)把和展開成泰勒(Taylor)級(jí)數(shù)，并且可取一次近似值。連續(xù)系統(tǒng)線性化1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

則有：或：

稱為雅克比矩陣1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

推導(dǎo)離散系統(tǒng)線性化卡爾曼濾波方程有兩條途徑：1）先進(jìn)非線性連續(xù)系統(tǒng)的離散化，再進(jìn)行線性化；（麻煩）2）先線性化，后離散化。（方便）1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波線性化:離散化:1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波基于標(biāo)稱軌跡線性化的狀態(tài)量偏差離散型卡爾曼濾波方程：1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波EKF是為了解決按標(biāo)稱軌跡線性化存在的以下問(wèn)題：（1）標(biāo)稱解難解；（2）真軌跡與標(biāo)稱解之間偏差不能確保其足夠小。1.5.2按最優(yōu)狀態(tài)估計(jì)線性化的卡爾曼濾波方程

——廣義（推廣、擴(kuò)展）卡爾曼濾波（EKF,ExtendedKalmanfiltering)

為此，改用另一種近似方法，即采用圍繞最優(yōu)化狀態(tài)估計(jì)

或

的線性化方法，現(xiàn)定義真軌跡與標(biāo)稱軌跡間的偏差為：1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波EKF是為了解決按標(biāo)稱軌跡線性化存在的以下問(wèn)題：（1）標(biāo)稱解難解；（2）真軌跡與標(biāo)稱解之間偏差不能確保其足夠小。1.5.2按最優(yōu)狀態(tài)估計(jì)線性化的卡爾曼濾波方程

——廣義（推廣、擴(kuò)展）卡爾曼濾波（EKF,ExtendedKalmanfiltering)

為此，改用另一種近似方法，即采用圍繞最優(yōu)化狀態(tài)估計(jì)

或

的線性化方法，現(xiàn)定義真軌跡與標(biāo)稱軌跡間的偏差為：1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波或

或

為非線性化系統(tǒng)標(biāo)稱狀態(tài)微分方程：

當(dāng)初始值用初始狀態(tài)最優(yōu)估計(jì)

代入時(shí)的解。

或初始值用初始狀態(tài)最優(yōu)估計(jì)

對(duì)上式進(jìn)行數(shù)值求解所得的解。

其中

就是系統(tǒng)狀態(tài)

的一步預(yù)測(cè)值，即：1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波離散型EKF方程：采用先線性化后離散化的推導(dǎo)方法。離散化線性化1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波離散型EKF方程：

1最優(yōu)估計(jì)與卡爾曼濾波1.5非線性系統(tǒng)卡爾曼濾波

由于因此：所以上述離散卡爾曼濾波的最終形式為：EKF為次優(yōu)估計(jì)！1最優(yōu)估計(jì)與卡爾曼濾波1.6卡爾曼濾波應(yīng)用舉例例1：設(shè)運(yùn)動(dòng)體沿某一直線運(yùn)動(dòng)，時(shí)刻的位移、速度、加速度和加加速度分別為,,,,只對(duì)運(yùn)動(dòng)體的位置作測(cè)量，測(cè)量值為,若：量測(cè)量采樣周期為T，求對(duì)的估計(jì)。

1最優(yōu)估計(jì)與卡爾曼濾波1.6卡爾曼濾波應(yīng)用舉例例：設(shè)電離層探測(cè)器上裝有慣性導(dǎo)航系統(tǒng)，在飛行初始階段用無(wú)線電定位測(cè)量的方法來(lái)實(shí)現(xiàn)飛行中導(dǎo)航參數(shù)的校正。為分析方便，僅考慮單軸情況，且認(rèn)為慣導(dǎo)系統(tǒng)的誤差源主要是初始條件（位置、速度、加速度）的誤差。采用卡爾曼濾波信息融合的方法對(duì)慣導(dǎo)誤差進(jìn)行校正。1最優(yōu)估計(jì)與卡爾曼濾波1.6卡爾曼濾波應(yīng)用舉例例1：捷聯(lián)慣導(dǎo)速度匹配對(duì)準(zhǔn)關(guān)鍵：建立好系統(tǒng)動(dòng)力學(xué)方程和量測(cè)方程（a)建立姿態(tài)估計(jì)誤差和速度估計(jì)誤差傳播微分方程（b)用狀態(tài)方程式表示1最優(yōu)估計(jì)與卡爾曼濾波1.6卡爾曼濾波應(yīng)用舉例例3：捷聯(lián)慣導(dǎo)速度匹配對(duì)準(zhǔn)離散形式：1最優(yōu)估計(jì)與卡爾曼濾波1.6卡爾曼濾波應(yīng)用舉例例3：捷聯(lián)慣導(dǎo)速度匹配對(duì)準(zhǔn)飛機(jī)導(dǎo)航系統(tǒng)的北向和東向速度測(cè)量值為：導(dǎo)彈導(dǎo)航系統(tǒng)的北向和東向速度測(cè)量值估計(jì)值為：以二者差值作為卡爾曼濾波器測(cè)量更新值：量測(cè)方程為：1最優(yōu)估計(jì)與卡爾曼濾波1.7卡爾曼濾波在融合導(dǎo)航系統(tǒng)中的應(yīng)用方式1、輸出校正和反饋校正

（1）直接法估計(jì)和間接法估計(jì)

組合導(dǎo)航系統(tǒng)采用卡爾曼濾波進(jìn)行估計(jì)的主要對(duì)象導(dǎo)航參數(shù)

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

位置λ，L

速度VX,VY,VZ姿態(tài)ψ,θ,γ導(dǎo)航參數(shù)用X表示1最優(yōu)估計(jì)與卡爾曼濾波1.7卡爾曼濾波在融合導(dǎo)航系統(tǒng)中的應(yīng)用方式直接法間接法根據(jù)濾波器狀態(tài)選取的不同，估計(jì)方法分為

以各種導(dǎo)航參數(shù)X為主要狀態(tài)濾波器估值的主要部分即是導(dǎo)航參數(shù)的估值以某種導(dǎo)航系統(tǒng)輸出導(dǎo)航參數(shù)的誤差為主要狀態(tài)濾波器估值的主要部分即是導(dǎo)航參數(shù)誤差的估值模型可能是線性的，也可能是非線性的模型一般都是線性的根據(jù)間接法估計(jì)的狀態(tài)都是誤差狀態(tài)，即濾波方程矢量是上述導(dǎo)航參數(shù)誤差狀態(tài)△X和其他誤差狀態(tài)的集合（仍用△X表示）利用狀態(tài)估值

去對(duì)原系統(tǒng)進(jìn)行校正也有兩種方法，即輸出校正和反饋校正兩種。1最優(yōu)估計(jì)與卡爾曼濾波1.7卡爾曼濾波在融合導(dǎo)航系統(tǒng)中的應(yīng)用方式（2）輸出校正

慣性系統(tǒng)卡爾曼濾波器其他導(dǎo)航系統(tǒng)+-輸出校正的濾波器

間接法的綜合導(dǎo)航卡爾曼濾波器：將慣性系統(tǒng)和其他的導(dǎo)航系統(tǒng)各自計(jì)算的某些導(dǎo)航參數(shù)（分別用XI和XN表示）進(jìn)行比較，其差值就包含了慣導(dǎo)某些導(dǎo)航參數(shù)誤差，即：

濾波器將這種差值作為量測(cè)值，經(jīng)過(guò)濾波計(jì)算，得到濾波器的狀態(tài)估值。1最優(yōu)估計(jì)與卡爾曼濾波1.7卡爾曼濾波在融合導(dǎo)航系統(tǒng)中的應(yīng)用方式

就是用導(dǎo)航參數(shù)誤差的估值

去校正系統(tǒng)輸出的導(dǎo)航參數(shù)，得到綜合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估值（即經(jīng)過(guò)校正后系統(tǒng)導(dǎo)航參數(shù)值）即：

定義

的估計(jì)誤差

為：

即組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)

的誤差，就是慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)誤差估值的估計(jì)誤差。1最優(yōu)估計(jì)與卡爾曼濾波1.7卡爾曼濾波在融合導(dǎo)航系統(tǒng)中的應(yīng)用方式（3）反饋校正

慣性系統(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)行校正。1最優(yōu)估計(jì)與卡爾曼濾波1.7卡爾曼濾波在融合導(dǎo)航系統(tǒng)中的應(yīng)用方式輸出校正與反饋校正總結(jié)

從形式上看，輸出校正僅僅校正系統(tǒng)得輸出量，而反饋校正則是校正系統(tǒng)內(nèi)部的狀態(tài)?？梢宰C明，兩種校正方法的性質(zhì)是一樣的，具有同樣的精度。但是，輸出校正的濾波器所估計(jì)的狀態(tài)是未經(jīng)校正的導(dǎo)航參數(shù)誤差，而反饋校正的濾波器所估計(jì)的狀態(tài)誤差是經(jīng)過(guò)校正的導(dǎo)航參數(shù)誤差。前者數(shù)值大，后者數(shù)值小，而狀態(tài)方程都是經(jīng)過(guò)一階近似的線性方程，狀態(tài)的數(shù)值越小，則近似的準(zhǔn)確性越高，因此，利用狀態(tài)反饋校正的系統(tǒng)狀態(tài)方程，更能接近真實(shí)地反映系統(tǒng)誤差狀態(tài)的動(dòng)態(tài)過(guò)程。故：對(duì)實(shí)際系統(tǒng)來(lái)講，只要狀態(tài)能夠具體實(shí)施反饋校正，綜合導(dǎo)航系統(tǒng)就可盡量采用反饋校正的濾波方案。2組合導(dǎo)航基本原理和方法2.1組合導(dǎo)航中的多傳感器融合方法

集中式濾波分散化濾波

利用一個(gè)卡爾曼濾波器來(lái)集中地處理所有導(dǎo)航子系統(tǒng)的信息1971年P(guān)earson提出分散濾波的概念利用子濾波器處理子系統(tǒng)的信息通過(guò)全局濾波器實(shí)現(xiàn)所有子系統(tǒng)的信息融合。1988年Carlson提出了聯(lián)邦濾波器(FederatedFilter)2多傳感器融合導(dǎo)航基本原理和方法

濾波器子系統(tǒng)1參考系統(tǒng)時(shí)間更新最優(yōu)融合子系統(tǒng)2子系統(tǒng)21）集中式濾波2.1組合導(dǎo)航中的多傳感器融合方法2多傳感器融合導(dǎo)航基本原理和方法

集中式濾波器優(yōu)點(diǎn)(1)可以給出最優(yōu)的誤差估計(jì)；(2)只有一個(gè)濾波器，結(jié)構(gòu)簡(jiǎn)單，工程中容易實(shí)現(xiàn)。(3)目前國(guó)內(nèi)采用的仍然主要是集中式濾波。2.2組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法

集中式濾波器缺點(diǎn)：

集中式濾波理論上可以給出誤差估計(jì)的最優(yōu)估計(jì)，但存在著如下缺點(diǎn)：

(1)狀態(tài)維數(shù)高，計(jì)算負(fù)擔(dān)重，不利于濾波實(shí)時(shí)運(yùn)行，狀態(tài)維數(shù)高會(huì)帶來(lái)“維數(shù)災(zāi)難”；

(2)

容錯(cuò)性能差，不利于故障診斷，信息污染問(wèn)題。2.2組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法

2）分散化濾波LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法

2）分散化濾波優(yōu)點(diǎn)

(1)全局濾波仍然較復(fù)雜；(2)算法基于各測(cè)量值是不相關(guān)的假設(shè)。缺點(diǎn)計(jì)算量小，容錯(cuò)性好。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2）分散化濾波---一種簡(jiǎn)單的分散化濾波假定系統(tǒng)的狀態(tài)方程和量測(cè)方程為

其中的協(xié)方差陣為

,的協(xié)方差陣為

2.2組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2）分散化濾波---一種簡(jiǎn)單的分散化濾波子系統(tǒng)的狀態(tài)方程和量測(cè)方程為其中的協(xié)方差陣為

,的協(xié)方差陣為

上式表示總系統(tǒng)利用了所有子系統(tǒng)的量測(cè)信息。

2.1組合導(dǎo)航中的多傳感器融合方法2多傳感器融合導(dǎo)航基本原理和方法2）分散化濾波---一種簡(jiǎn)單的分散化濾波假設(shè)各子系統(tǒng)的量測(cè)相互獨(dú)立，且是的一部分

2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2）分散化濾波---一種簡(jiǎn)單的分散化濾波2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2）分散化濾波---一種簡(jiǎn)單的分散化濾波全局濾波的量測(cè)更新可用局部濾波表示；時(shí)間更新仍需用全局濾波方程；子濾波和全局濾波都是最優(yōu)的。缺點(diǎn)：全局濾波的合成算法比較復(fù)雜，不僅用到了子濾波器的濾波值和協(xié)方差，還用到了它們的預(yù)報(bào)值；算法基于各量測(cè)值是不相關(guān)的。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2）分散化濾波---聯(lián)邦濾波LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合2.2組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2）分散化濾波---聯(lián)邦濾波信息分配原則（1）狀態(tài)運(yùn)動(dòng)方程的信息狀態(tài)方程的信息量是與系統(tǒng)噪聲的方差成反比的，因此可用Q-1表示狀態(tài)方程信息量；狀態(tài)初值的信息也是狀態(tài)方程的信息，可用初值估計(jì)的協(xié)方差陣的逆P-1(0)表示。（2）量測(cè)方程的信息量測(cè)方程的信息可用量測(cè)噪聲協(xié)方差的逆R-1表示。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法

假設(shè)將系統(tǒng)噪聲總的信息量Q-1分配到各局部濾波器和主濾波器中去,即

故:根據(jù)信息守恒狀態(tài)估計(jì)初值P-1(0)也可按上述方法分配,可得2.2組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法（1）將子濾波器和主濾波器的初始估計(jì)協(xié)方差設(shè)置為組合系統(tǒng)初始值的倍,滿足信息守恒原則；（2）將子濾波器和主濾波器的系統(tǒng)噪聲協(xié)方差設(shè)置為組合系統(tǒng)系統(tǒng)噪聲協(xié)方差的倍,滿足信息守恒原則；（3）各子濾波器處理自己的量測(cè)信息,獲得局部最優(yōu)估計(jì)；（4）得到局部估計(jì)和主濾波器的估計(jì)后按下式最優(yōu)合成：（5）用全局濾波解來(lái)重置各子濾波器和主濾波器的濾波值和協(xié)方差陣。2.2組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法聯(lián)邦濾波器的結(jié)構(gòu)與性能分析LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合（1）第一類（零化式重置）2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法優(yōu)、缺點(diǎn)主濾波器分配到全部信息，故障檢測(cè)和隔離能力強(qiáng);子濾波器狀態(tài)信息只被重置到零，協(xié)方差趨于無(wú)窮，故障檢測(cè)和隔離能力很差;減少了數(shù)據(jù)通訊量，計(jì)算簡(jiǎn)單。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法優(yōu)、缺點(diǎn)主濾波器分配到全部信息，故障檢測(cè)和隔離能力強(qiáng);子濾波器狀態(tài)信息只被重置到零，協(xié)方差趨于無(wú)窮，故障檢測(cè)和隔離能力很差;減少了數(shù)據(jù)通訊量，計(jì)算簡(jiǎn)單。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合（2）第二類（有重置）2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法優(yōu)、缺點(diǎn)主濾波器與子濾波器之間平均分配信息；融合后全局濾波精度高，局部因?yàn)橛腥譃V波反饋，精度也提高了；子濾波器故障檢測(cè)與隔離性能好；主濾波器的故障檢測(cè)與隔離性能中等；一個(gè)子系統(tǒng)故障后，主濾波器受污染，隔離后必須重新初始化主濾波器。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合（3）第三類（有重置）2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法優(yōu)、缺點(diǎn)主濾波器狀態(tài)方程無(wú)信息分配，主濾波器不需要進(jìn)行濾波，所以主濾波器的估計(jì)值取為全局估計(jì)，即主濾波器的故障檢測(cè)與隔離能力差；子濾波器的故障檢測(cè)與隔離能力與第二類結(jié)構(gòu)一樣。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法（4）第四類（無(wú)重置）LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法優(yōu)、缺點(diǎn)各局部濾波器獨(dú)立濾波，沒(méi)有反饋重置帶來(lái)的相互影響，提高了容錯(cuò)性能；由于沒(méi)有全局估計(jì)，局部估計(jì)精度不高。2.1組合導(dǎo)航中的多傳感器融合方法2組合導(dǎo)航基本原理和方法2.1組合導(dǎo)航中的多傳感器融合方法（5）第五類（有重置）LF1主濾波器子系統(tǒng)1子系統(tǒng)2參考系統(tǒng)時(shí)間更新最優(yōu)融合最優(yōu)量測(cè)更新2組合導(dǎo)航基本原理和方法2.1組合導(dǎo)航中的多傳感器融合方法優(yōu)、缺點(diǎn)主濾波器包含所有狀態(tài)信息；子濾波器的信息陣每次融合后重置為零；然后子濾波器再重新啟動(dòng)，即零化啟動(dòng)；子濾波器起到數(shù)據(jù)壓縮作用，故障檢測(cè)與隔離能力差；主濾波器的故障檢測(cè)與隔離能力強(qiáng)，但是故障恢復(fù)能力差，因此要重新初始化。2組合導(dǎo)航基本原理和方法2.1組合導(dǎo)航中的多傳感器融合方法LF1主濾波器子系統(tǒng)1子系統(tǒng)2參考系統(tǒng)時(shí)間更新最優(yōu)融合最優(yōu)量測(cè)更新（6）第六類（有重置）2組合導(dǎo)航基本原理和方法2.1組合導(dǎo)航中的多傳感器融合方法優(yōu)、缺點(diǎn)每次信息融合后，子濾波器信息被重置到全局信息一半，協(xié)方差重置為全局估計(jì)協(xié)方差的一倍；主、子濾波器的故障檢測(cè)與隔離能力中等；主濾波器的故障恢復(fù)能力差。是故障恢復(fù)能力差，因此要重新初始化。2組合導(dǎo)航基本原理和方法2.1組合導(dǎo)航中的多傳感器融合方法結(jié)論：利用融合后的全局狀態(tài)和協(xié)方差去反饋重置子濾波器，提高子濾波器精度，但是主濾波器也因此容易受到故障子濾波器的影響；如果不將融合后的全局狀態(tài)和協(xié)方差去反饋重置子濾波器，那么就不會(huì)產(chǎn)生交叉污染，容錯(cuò)性能大大提高。2組合導(dǎo)航基本原理和方法2.2GNSS/INS組合導(dǎo)航2.2.1組合導(dǎo)航的基本體系結(jié)構(gòu)（1）松組合法2組合導(dǎo)航基本原理和方法2.2GNSS/INS組合導(dǎo)航GNSS接收機(jī)通常通過(guò)自己的Kalman濾波輸出其速度和位置，這種組合導(dǎo)致濾波器的串聯(lián)，使組合導(dǎo)航觀測(cè)噪聲時(shí)間相關(guān)（有色噪聲），不滿足EKF觀測(cè)噪聲為白噪聲的基本要求，嚴(yán)重時(shí)可能使濾波器不穩(wěn)定。幾乎無(wú)冗余信息，不利于異常診斷，不利于進(jìn)行隨機(jī)模型改化…。

松組合的主要缺點(diǎn)系統(tǒng)結(jié)構(gòu)簡(jiǎn)單，易于實(shí)現(xiàn)，可以大幅度提高系統(tǒng)的導(dǎo)航精度，并使INS具有動(dòng)基座對(duì)準(zhǔn)能力。

松組合的主要優(yōu)點(diǎn)2組合導(dǎo)航基本原理和方法2.2GNSS/INS組合導(dǎo)航（2）緊組合法2組合導(dǎo)航基本原理和方法2.2GNSS/INS組合導(dǎo)航觀測(cè)量——根據(jù)GNSS接收機(jī)收到的星歷信息和INS輸出的位置和速度信息，計(jì)算相應(yīng)于INS位置的偽距和偽距率，GNSS接收機(jī)測(cè)量得到的偽距和偽距速率與INS計(jì)算值的差值。通過(guò)EKF對(duì)INS的誤差和GPS接收機(jī)的誤差進(jìn)行最優(yōu)估計(jì)，然后對(duì)INS進(jìn)行輸出或者反饋校正。由于不存在濾波器的級(jí)聯(lián)，并可對(duì)GNSS接收機(jī)的測(cè)距誤差進(jìn)行建模，因此這種偽距、偽距率組合方式比位置、速度組合具有更高的組合精度。而且在可見星的個(gè)數(shù)少于4顆時(shí)也可以使用。2組合導(dǎo)航基本原理和方法2.2GNSS/INS組合導(dǎo)航（3）深組合法

2組合導(dǎo)航基本原理和方法2.2GNSS/INS組合導(dǎo)航

深組合是使用慣性導(dǎo)航信息對(duì)GNSS接收機(jī)進(jìn)行輔助導(dǎo)航的組合方式。主要思想：既使用濾波技術(shù)對(duì)INS的誤差進(jìn)行最優(yōu)估計(jì)，同時(shí)使用校正后的INS速度信息對(duì)接收機(jī)的載波環(huán)、碼環(huán)進(jìn)行輔助跟蹤，從而減小環(huán)路的等效帶寬，增加GPS接收機(jī)在高動(dòng)態(tài)或強(qiáng)干擾環(huán)境下的跟蹤能力。嵌入式組合將INS和GNSS進(jìn)行一體化設(shè)計(jì)，通過(guò)共用電源、時(shí)鐘等進(jìn)一步減小體積、降低成本和減小非同步誤差的影響。2組合導(dǎo)航基本原理和方法2.3組合導(dǎo)航方法舉例

3導(dǎo)航技術(shù)發(fā)展趨勢(shì)

制導(dǎo)彈藥等常規(guī)武器制導(dǎo)化領(lǐng)域：20世紀(jì)90年代中期，美國(guó)DRAPER實(shí)