基于統(tǒng)計的融合

基于統(tǒng)計的融合第1頁/共115頁24.1點估計理論基礎應用舉例（狀態(tài)最優(yōu)估計融合算法在伺服系統(tǒng)中的應用）

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

4.4應用——濾波及仿真第4章基于統(tǒng)計的融合--統(tǒng)計推斷與估計理論第2頁/共115頁3第4章基于統(tǒng)計的融合--統(tǒng)計推斷與估計理論估計：根據(jù)測量值z(t)，z(t)=h[x(t)]+w(t)，解算出x(t)的計算值(t)。隨機向量v(t)為量測誤差，稱為x的估計，z稱為x的量測。設在[t0,t1]時間內的量測為z，相應的估計為，則當t=t1時，(t)稱為x(t)的估計；當t>t1時，(t)稱為x(t)的預測；當t<t1時，x(t)稱為x(t)的平滑；最優(yōu)估計是指某一指標值達到最值時的估計。第3頁/共115頁4第4章基于統(tǒng)計的融合--統(tǒng)計推斷與估計理論以量測估計Z的偏差的平方和達到最小為指標，即(Z-Z)T(Z-Z)=min

則所得的估計為最小二乘估計；以狀態(tài)估計的均方誤差集平均達到最小為指標，E[(x-)T(x-)]=min

則所得的估計為最小方差估計；若又是x的線性估計，則為x的線性最小方差估計。以估計值出現(xiàn)的概率作為估計指標，有極大驗后估計、貝葉斯估計和極大似然估計。第4頁/共115頁5點估計：又稱定值估計，就是用實際樣本指標數(shù)值作為總體參數(shù)的估計值，實際上它就是總體未知參數(shù)的近似值。由樣本x1,x2,…xn確定一個統(tǒng)計量用它來估計總體的未知參數(shù)，稱為總體參數(shù)的估計量。當具體的樣本抽出后，可求出樣本參數(shù)的值。衡量標準有：無偏性(unbiasedness)一致性（consistency）有效性4.1點估計的理論基礎第5頁/共115頁64.1點估計的理論基礎本小節(jié)的主要內容有：一般概念Bayes點估計理論最佳線性無偏（BLUE）估計加權最小二乘（WLS）估計極大似然（ML）估計遞推最小二乘（RLS）估計第6頁/共115頁74.1點估計的理論基礎——一般概念

一般概念

定義4.1.1設是一個未知參數(shù)向量，量測y是一個m維的隨機向量，而y的一組容量為N的樣本是設對它的統(tǒng)計量為(4.1.1)

稱其為對x的一個估計量，其中稱為統(tǒng)計規(guī)則或估計算法。

利用樣本對參數(shù)的估計量本質上是隨機的，而當樣本值給定時所得到的參數(shù)估計值一般與真值并不相同，因而需要用某些準則進行評價。第7頁/共115頁84.1點估計的理論基礎——一般概念

定義4.1.2對于式(4.1.1)，所得估計量如果滿足(4.1.2)則稱是對參數(shù)x的一個無偏估計；如果滿足(4.1.3)則稱是對參數(shù)x的一個漸近無偏估什。定義4.1.3對于式(4.1.1)所得估計量，如果依概率收斂于真值，即(4.1.4)

則稱是對參數(shù)x的一個一致估計量。第8頁/共115頁94.1點估計的理論基礎——一般概念

定理4.1.1（Cramer-Rao不等式）設是參數(shù)x的一個正規(guī)無偏估計，則其估計誤差的協(xié)方差陣滿足Cramer-Rao不等式(4.1.5)其中是估計誤差，而是Fisher信息矩陣（注意標量對向量求導取行向量），定義為(4.1.6)

其中是給定x時y的條件概率密度函數(shù)。第9頁/共115頁104.1點估計的理論基礎——Bayes點估計理論Bayes點估計理論設x也是一個n維隨機向量，仍設是y的一組容量為N的樣本。設表示量測信息，則x與z的聯(lián)合概率密度函數(shù)是

(4.1.7)

假定表示由量測信息z得到的一個估計，而估計誤差定義為

(4.1.8)第10頁/共115頁114.1點估計的理論基礎——Bayes點估計理論定義4.1.4估計誤差的標量函數(shù)稱為一個損失函數(shù)，如果（1）零誤差的損失為零，即，則；（2）按分量的絕對值單調增，即和的第i個分量滿足，其余分量相等，則；

（3）是對稱的，即，有。定義4.1.5設，估計誤差的損失函數(shù)是，則風險函數(shù)定義為(4.1.9)

其中估計方法；Bayes風險定義為

(4.1.10)

其中，分別表示按分布或條件分布求期望；第11頁/共115頁124.1點估計的理論基礎——Bayes點估計理論最小Bayes風險估計定義為(4.1.11)

定義4.1.6利用Bayes公式Bayes風險可以改寫為(4.1.12)

其中就是損失函數(shù)的后驗期望。而最小后驗用望損失估計定義為

(4.1.13)

其中就是損失函數(shù)的后驗期望。而最小后驗用望損失估計定義為

(4.1.13)第12頁/共115頁134.1點估計的理論基礎——Bayes點估計理論定理4.1.2設參數(shù)x和量測信息z是聯(lián)合Guass分布的，其均值和協(xié)方差陣分別為

(4.1.14)

并假定R和非奇異，那么，給定z時，x也是條件Gauss的，而且對估計誤差的任意容許損失函數(shù)，最小后驗期望損失估計公式為(4.1.15)

估計誤差的協(xié)方差陣是(4.1.16)第13頁/共115頁144.1點估計的理論基礎——BLUE估計

BLUE估計定義4.1.7設，，對參數(shù)x的估計表示為量測信息z的線性函數(shù)(4.1.17)

則稱為線性估計；進而如果估計誤差的均方值達到最小，則稱之為線性最小方差估計；如估計還是無偏的，則稱為線性無偏最小方差估計。這種線性無偏最小方差估計在多源信息融合領域一般稱為最佳線性無偏估計（BestLinearUnbiasedEstimation，BLUE）。第14頁/共115頁154.1點估計的理論基礎——BLUE估計定理4.13設參數(shù)x和量測信息z是任意分布，z的協(xié)方差陣非奇異，則利用量測信息z對參數(shù)x的BLUE估計惟一地表示為

(4.1.18)

此處只是一個記號，不表示條件期望；而估計誤差的協(xié)方差陣是

(4.1.19)第15頁/共115頁164.1點估計的理論基礎——WLS估計WLS估計定義4.1.8假定量測信息z可以表示為參數(shù)x的線性函數(shù)，即(4.1.20)

其中，是一個零均值的隨機向量設，為對稱陣，則如下估計(4.1.21)

稱為加權最小二乘（weightedleastsquare，WLS）估計；如果，則稱為最小二乘（leastsquare，LS）估計。定理4.1.4設可逆，則基于量測信息z和加權矩陣W對參數(shù)x的WLS估計為

(4.1.22)第16頁/共115頁174.1點估計的理論基礎——ML估計ML估計定義4.1.9給定參數(shù)x時量測信息z的似然函數(shù)表示為，極大似然（maximumIikelihood，ML）參數(shù)估計問題可以描述為

(4.1.23)

考慮如下參數(shù)估計問題

(4.1.24)

其中是時間指標，分別是回歸向量和未知參數(shù)向量，是k時刻的量測量，而是量測誤差。第17頁/共115頁184.1點估計的理論基礎——RLS估計RLS估計

假定是一個零均值的隨機過程，對于k時刻的量測總量、回歸總量和誤差總量，，，可有總量關系

(4.1.25)

所以有最小二乘估計

(4.1.26)第18頁/共115頁194.1點估計的理論基礎——RLS估計引理（矩陣求逆引理）設，均可逆，，則有

(4.1.27)定理4.1.7

設獲得第k+1時刻的量測和回歸向量之后，令

(4.1.28)

則有遞推最小二乘（recursiveleastsquares，RLS）估計是

(4.1.29)第19頁/共115頁204.1點估計的理論基礎——RLS估計遞推最小二乘（recursiveleastsquares，RLS）估計其中和分別是Kalman增益矩陣和一步預測誤差或參數(shù)估計新息（innovation），分別計算為

(4.1.30)

而的遞推計算式為

(4.1.31)第20頁/共115頁214.1點估計的理論基礎——應用應用舉例（狀態(tài)最優(yōu)估計融合算法在伺服系統(tǒng)中的應用）

第21頁/共115頁224.1點估計理論基礎應用舉例（狀態(tài)最優(yōu)估計融合算法在伺服系統(tǒng)中的應用）

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

4.4應用——濾波及仿真第4章基于統(tǒng)計的融合第22頁/共115頁234.2線性動態(tài)系統(tǒng)的濾波理論與算法主要內容有：離散時間線性系統(tǒng)狀態(tài)估計問題的一般描述基本Kalman濾波器Kalman濾波基本方程的推導過程第23頁/共115頁244.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述

離散時間線性系統(tǒng)狀態(tài)估計問題的一般描述定義4.2.1考慮離散時間線性隨機動態(tài)系統(tǒng)(4.2.1)(4.2.2)

其中是時間指標，是k時刻的系統(tǒng)狀態(tài)向量，是系統(tǒng)狀態(tài)轉移矩陣，而是過程演化噪聲，是噪聲矩陣，是k時刻對系統(tǒng)狀態(tài)的量測向量，是量測矩陣，而是量測噪聲。假定直到k時刻所有的量測信息是

(4.2.3)

馬爾可夫序列大多數(shù)實際物理系統(tǒng)成立第24頁/共115頁25

基于量測信息：對的估計問題，稱為狀態(tài)濾波問題；對的估計問題，稱為狀態(tài)預測問題；對的估計問題，稱為狀態(tài)平滑問題。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述第25頁/共115頁264.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述定義4.2.2仍考慮式(4.2.1)和式(4.2.2)描述的離散時間線性隨機動態(tài)系統(tǒng)，假定所有隨機變量都是Gauss的情況下，考慮對于量測的一步提前預測

(4.2.4)

而預測誤差序列

(4.2.5)

稱為新息（innovation）序列。隨機過程{X(t),t∈D}，其中的向量X(t1),…,X(tm)兩兩不相關第26頁/共115頁27

如果假定隨機變量是非Gauss的情況下，仍考慮對于量測的一步提前預測

(4.2.6)其中估計采用BLUE準則，而預測誤差序列

(4.2.7)稱為偽新息序列。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述第27頁/共115頁284.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述定理4.2.1Gauss序列所產(chǎn)生的新息序列是一個零均值的獨立過程，它與原量測序列之間存在因果性線性運算，而且包含了原序列的所有信息；同時原量測序列、一步提前預測序列和新息序列構成一個一步提前預測器，這個預測器是一個具有單位反饋的線性系統(tǒng)，如下圖所示。第28頁/共115頁294.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述推論：非Gauss序列所產(chǎn)生的偽新息序列也是一個零均值的獨立過程，它與原量測序列之間也存在因果性線性運算，而且包含了原序列的所有信息；同時原量測序列、一步提前預測序列和偽新息序列構成一個一步提前預測器，與Gauss情況相同。定理4.2.2設是由Gauss序列產(chǎn)生的新息序列，假定x是與聯(lián)合Gauss分布的，且，則

(4.2.8)第29頁/共115頁304.2線性動態(tài)系統(tǒng)的濾波理論與算法

——狀態(tài)估計問題的一般描述推論如果是由非Gauss序列產(chǎn)生的偽新息序列，并假定x是與具有任意形式的聯(lián)合分布，且，則

(4.2.9)第30頁/共115頁314.2線性動態(tài)系統(tǒng)的濾波理論與算法

——基本Kalman濾波基本Kalman濾波的主要內容有：卡爾曼濾波所要解決的問題卡爾曼濾波的基本方程卡爾曼濾波基本方程的直觀推導卡爾曼濾波基本方程的正交投影推導第31頁/共115頁324.2線性動態(tài)系統(tǒng)的濾波理論與算法

——基本Kalman濾波解決的問題濾波所謂濾波就是從混合在一起的諸多信號中提取出所需要的信號。信號是傳遞和運載信息的時間或空間函數(shù)。有一類信號的變化規(guī)律是既定的，具有確定的頻譜，稱為確定性信號。另一類信號沒有既定的變化規(guī)律，沒有確定的頻譜，稱為隨機信號。第32頁/共115頁33濾波由于確定性信號具有確定的頻潛，所以可根據(jù)各信號頻帶的不同，設置具有相應頻率特性的濾波器，如低通、高通、帶通、帶阻濾波器，使有用信號無衰減地通過，使干擾信號受到抑制。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——基本Kalman濾波解決的問題這類濾波器可用物理方法實現(xiàn)，此即模擬濾波器，也可用計算機通過算法實現(xiàn)，此即數(shù)字濾波器。對確定性信號的濾波處理也稱常規(guī)濾波。隨機信號沒有確定的頻譜，無法用常規(guī)濾波提取或抑制信號。但隨機信號具有確定的功率譜，所以可根據(jù)有用信號和干擾信號的功率譜設計濾波器。第33頁/共115頁344.2線性動態(tài)系統(tǒng)的濾波理論與算法

——基本Kalman濾波解決的問題卡爾曼濾波所要解決的問題卡爾曼濾波從與被提取信號有關的量測量中通過算法估計出所需信號。被估計信號是由白嗓聲激勵引起的隨機響應，激勵源與響應之問的傳遞結構（系統(tǒng)方程）已知，量測量與被估計量之間的函數(shù)關系（量測方程）也已知。估汁過程中利用了如下信息：系統(tǒng)方程、量測方程、白噪聲激勵的統(tǒng)計特性、量測誤差的統(tǒng)計特性。由于所用信息都是時域內的量，所以卡爾曼濾波器是在時域內設計的，且適用于多維情況。第34頁/共115頁354.2線性動態(tài)系統(tǒng)的濾波理論與算法

——基本Kalman濾波解決的問題從以上簡述中可看出卡爾曼濾波有如下特點：（1）卡爾曼濾波處理的對象是隨機信號；（2）被處理信號無有用和干擾之分，濾波的目的是要估計出所有被處理信號；（3）系統(tǒng)的白噪聲激勵和量測噪聲并不是需要濾除的對象，它們的統(tǒng)計特性正是估計過程中需要利用的信息。第35頁/共115頁36

卡爾曼濾波的基本方程設時刻的被估計狀態(tài)受系統(tǒng)噪聲序列驅動，驅動機理由下述狀態(tài)方程描述

(4.2.10)4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程

對的量測滿足線性關系，量測方程為

(4.2.11)

式中：為時刻至時刻的一步轉移陣；為系統(tǒng)噪聲驅動陣；為量測陣；為量測噪聲序列；為系統(tǒng)激勵噪聲序列。第36頁/共115頁37

同時，和滿足

(4.2.12)

式中：若為系統(tǒng)噪聲序列的方差陣，假設為非負定陣；為量測噪聲序列的方差陣，假設為正定陣。

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程定理4.3.2

如果被估計狀態(tài)滿足式(4.2.10)，對的量測量滿足式(4.2.11)，系統(tǒng)噪聲和量測噪聲滿足式(4.2.12)，系統(tǒng)噪聲方差陣非負定，量測噪聲方差陣正定，時刻的量測為，則估計按下述方程求解：第37頁/共115頁38狀態(tài)一步預測

(4.2.13a)狀態(tài)估計

(4.2.13b)濾波增益

(4.2.13c)或

(4.2.13c')一步預測均方誤差(4.2.13d)4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程第38頁/共115頁39估計均方誤差

(4.2.13e)或(4.2.13e′)或

(4.2.13e〞)

式(4.2.13)即為離散型卡爾曼濾波基本方程。只要給定初值和，根據(jù)時刻的量測，就可遞推計算得時刻的狀態(tài)估計。

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程第39頁/共115頁404.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程第40頁/共115頁414.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程式(4.2.13)所示算法可用圖來表示?？柭鼮V波具有兩個計算回路：增益計算回路和濾波計算回路。增益計算回路是獨立計算回路濾波計算回路依賴于增益計算回路。式(4.2.13)所示算法可用圖來表示?？柭鼮V波具有兩個計算回路：增益計算回路和濾波計算回路。增益計算回路是獨立計算回路濾波計算回路依賴于增益計算回路。第41頁/共115頁424.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程在一個濾波周期內：系統(tǒng)信息和量測信息卡爾曼濾波具有兩個明顯的信息更新過程，時間更新過程和量測更新過程。根據(jù)k-1時刻的狀態(tài)估計，預測k時刻狀態(tài)估計的方法對這種預測的質量優(yōu)劣作了定量描述兩式的計算中僅使用了與系統(tǒng)動態(tài)特性有關的信息，如一步轉移陣、噪聲驅動陣、驅動噪聲的方差陣。從時間的推移過程來看，該兩式將時間從時刻推進到時刻。所以該兩式描述了卡爾曼濾波的時間更新過程。第42頁/共115頁434.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程其余諸式用來計算對時間更新值的修正量，該修正量由時間更新的質量優(yōu)劣()、量測信息的質量優(yōu)劣()、量測與狀態(tài)的關系()以及具體的量測值所確定，所有這些方程圍繞一個目的，即正確合理地利用量測。這一過程描述了卡爾曼濾波的量測更新過程。第43頁/共115頁444.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導卡爾曼濾波基本方程的直觀推導

為了加深對離散型卡爾曼濾波基本方程的理解，此處根據(jù)直觀理解和物理概念推導出式(4.2.13)。

第44頁/共115頁454.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導初始值狀態(tài)預測值協(xié)方差矩陣預測值濾波增益矩陣狀態(tài)估計值協(xié)方差矩陣估計值第45頁/共115頁461.一步預測方程推導

一步預測是根據(jù)時刻的狀態(tài)估計預測時刻的狀態(tài)，即根據(jù)個量測，對作線性最小方差估計。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導

根據(jù)線性最小方差估計的性質，有由式(4.2.10)知，只影響，所以與不相關，且。

則有：因此采用BLUE估計（4.2.4）第46頁/共115頁472.狀態(tài)估計方程推導4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導用一步預測代替真實狀態(tài)引起的誤差為引起對量測的估計誤差為濾波理論中稱為殘差（也稱新息）。從上式可看出，殘差包含有一步預測誤差信息，對作適當?shù)募訖嗵幚砭湍軐⒎蛛x出來，用來修正即可得到狀態(tài)的估計式中為對殘差的加權陣，稱為濾波增益陣。第47頁/共115頁483.濾波增益陣和估計均方誤差陣的推導（1）增益陣的選取準則是使估計的均方誤差陣達到最小，其中為估計誤差。而

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導所以狀態(tài)估計第48頁/共115頁49由于是根據(jù)時刻前的量測對時刻的狀態(tài)所作的估計，而是時刻的量測噪聲。所以與不相關，并注意到，因此有4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導代入的表達式，得式中第49頁/共115頁50（2）下面根據(jù)極值原理從式(4.2.13e)推導出濾波增益陣4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導設是使估計的均方誤差陣達到最小的最佳增益陣，并設該最小均方誤差陣為。顯然，若濾波增益陣偏離最佳增益陣的偏離量為，則由式(4.2.13e)確定的估計的均方誤差將偏離最小值而達到，且為非負定陣，即。

和滿足的方程：第50頁/共115頁51由，可得

式中4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導

若取即則第51頁/共115頁52

由于為正定陣，至少為非負定陣，所以至少為非負定陣。若為非零陣，則至少為非負定陣，即。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導

這說明，若按式（4.2.13c）確定，則對于相對增益陣的任何偏離，估計的均方誤差將產(chǎn)生非負的偏差，因此是使估計的均方誤差達到最小的最佳增益陣。（4.2.13c）第52頁/共115頁534．一步預側均方誤差陣的推導

一步預測產(chǎn)生的誤差為

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導

所以一步預測均方誤差陣為由于,只影響，不影響，所以與不相關，又因，因此

第53頁/共115頁544.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導第54頁/共115頁55初始值狀態(tài)預測值協(xié)方差矩陣預測值濾波增益矩陣狀態(tài)估計值協(xié)方差矩陣估計值4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導第55頁/共115頁56計算步驟4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的直觀推導時間Hk-1,Rk-1Xk-1/k-2Pk-1/k-2Pk-1Xk-1tk-1Hk,RkXk/k-1Pk/k-1PkXktkΦk/k-1Qk-1Φk+1/kQk第56頁/共115頁574.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程卡爾曼濾波應稱作最優(yōu)估計理論，此處稱謂的濾波與常規(guī)濾波具有完全不同的概念和含意。就實現(xiàn)形式而言，卡爾曼濾波器實質上是一套由數(shù)字計算機實現(xiàn)的遞推算法，每個遞推周期中包含對被估計量的時間更新和量測更新兩個過程。時間更新由上一步的量測更新結果和設計卡爾曼濾波器時的先驗信息確定，量測更新則在時間更新的基礎上根據(jù)實時獲得的量測值確定。因此，量測量可看做卡爾曼濾波器的輸人，估計值可看做輸出，輸入與輸出之間由時間更新和量測更新算法聯(lián)系。第57頁/共115頁58濾波中的發(fā)散現(xiàn)象引起濾波器發(fā)散的主要原因有兩點：描述系統(tǒng)動力學特性的數(shù)學模型和噪聲的統(tǒng)計模型不準確，使模型與獲得的量測值不匹配?？柭鼮V波是遞推過程，隨著濾波步數(shù)的增加，舍入誤差逐漸積累。漸消kalman濾波kalman濾波4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程第58頁/共115頁59漸消因子4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波的基本方程第59頁/共115頁60卡爾曼濾波基本方程除利用物理概念經(jīng)直觀推導獲得外，還可用嚴密的數(shù)學概念推導獲得

4.2.1正交投影定理設和都是隨機向量，如果(4.2.14)

則稱與正交。應注意隨機向量間正交、不相關和獨立三者的區(qū)別。

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第60頁/共115頁61隨機向量間正交、不相關和獨立三者的區(qū)別。如果隨機向量和的協(xié)方差陣為零，即即(4.2.15)

式中和分別為和的均值，則稱和不相關。

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導

如果和的聯(lián)合分布密度(4.2.16)

式中和分別為和的分布密度，則稱和互相獨立。第61頁/共115頁62從上述定義可得出如下結論：（1）若X與Z獨立，則X與Z一定不相關。但逆命題一般并不成立，只有當X和Z都服從正態(tài)分布時才成立。（2）如果X和Z的數(shù)學期望至少有一個為零，則不相關與正交等價。（3）如果X和Z都服從正態(tài)分布。且至少有一個數(shù)學期望為零，則不相關、正交和獨立三者等價。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第62頁/共115頁63

如果存在某矩陣和某向量，對任意矩陣和任意向量b都能使下式成立

(4.2.17)

則稱為在上的正交投影。式(4.2.17)也可改寫成如下形式由于為任意矩陣。為任意向量，要使上式恒能成立，須有

(4.2.18)式(4.2.18)是正交投影定義的另一種形式。從該式可看出：4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第63頁/共115頁64（1）正交投影是量測量和常值向量的線性組合，位于張成的空間內，該空間也稱量測空間。（2）若用正交投影作為的估計。則估計誤差與量測空間正交。（3）正交投影是的無偏估計。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第64頁/共115頁65定理4.2.4

在上的正交投影即為在上的線性最小方差估計。反之亦然。即

(4.2.19)

證明（1）設是在上的線性最小方差估計，有，，則有：由于同理4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第65頁/共115頁66

所以此外，由于線性最小方差估計是無偏估計，即根據(jù)式(4.2.18)所給定義知是在上的正交投影。（2）設為在上的正交投影，則根據(jù)式(4.2.18)，有

(1)(2)

由(1)證明了線性最小方差估計是在上的正交投影，即有

(3)(4)4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第66頁/共115頁67

式(1)—式(3)，得由于，因此

(5)

式(2)—式(4)，得

(6)

將式(6)代入式(5)，得在估計過程中，為了充分利用量測信息，的各分量必須是獨立的，即有，4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第67頁/共115頁68

即滿秩，因此有將此關系式代入(6)式，得從而有這就證明了定理4.2.44.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第68頁/共115頁692.更新信息定理定理4.2.5（更新信息定理）設為隨機向量，為第次量測，和分別為前次量測和前次量測，則

(4.2.20)

即

(4.2.21)

式中，4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第69頁/共115頁70

更新信息定理說明只需用對作修正即可獲得，所以是卡爾曼濾波中的一個重要信息源。由于是前次量測已提供的關于的信息，而含有第次量測新提供的信息，所以常將稱為新息。這一定理也由此而得名。

證明：而(1)

式中即是關于的線性函數(shù)，由式(1)可推知，可由和線性表示，所以可由和線性表示

(2)4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第70頁/共115頁71由于，由定理4.2.4知與正交，即

(3)且

(4)根據(jù)線性最小方差估計性質及式(3)、式(4)，有4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第71頁/共115頁72注意到是確定性向量，所以4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第72頁/共115頁733．基本方程的正交投影推導下面根據(jù)更新信息定理推導卡爾曼濾波的狀態(tài)估計方程和最佳增益陣。由于并注意到時刻的量測噪聲不會影響時刻前的估計，所以4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第73頁/共115頁74

將上述關系式代人式(4.2.21)，得令則一步預測方程、一步預測的均方誤差陣方程和估計的均方誤差陣方程的推導與直觀推導方法中所述的一樣，此處不再重復。從上述推導過程可看出，是量測空間上的正交投影，根據(jù)定理4.2.4，正交投影與線性最小方差估計兩者等價，所以卡爾曼濾波是在上的線性最小方差估。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第74頁/共115頁75

利用該時刻及該時刻以前的所有量測信息，離散型卡爾曼濾波基本方程有如下優(yōu)點：（1）由于采用了遞推算法，不同時刻的量測值不必儲存起來，而是經(jīng)實時處理提煉成被估汁狀態(tài)的信息，隨著濾波步數(shù)的增加，提取出的信息濃度逐漸增加。（2）不必了解被估計量和量測量在不同時刻的一、二階矩，而只須知道驅動被估計量的驅動噪聲的統(tǒng)計特性、描述這種驅動作用的系統(tǒng)狀態(tài)方程及量測噪聲的統(tǒng)計特性。驅動噪聲和量測噪聲都是白噪聲，是平穩(wěn)過程，統(tǒng)計特性不隨時間而變，系統(tǒng)的狀態(tài)方程又是準確已知的，所以卡爾曼濾波能對非平穩(wěn)的被估計量作估計。4.2線性動態(tài)系統(tǒng)的濾波理論與算法

——Kalman濾波基本方程的正交投影推導第75頁/共115頁764.1點估計理論基礎應用舉例（狀態(tài)最優(yōu)估計融合算法在伺服系統(tǒng)中的應用）

4.2線性動態(tài)系統(tǒng)的濾波理論與算法

4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

4.4應用——濾波及仿真第4章基于統(tǒng)計的融合第76頁/共115頁774.3非線性動態(tài)系統(tǒng)的濾波理論與算法本小節(jié)的主要內容有：擴展Kalman濾波（EKF）UKF濾波(無跡（Unscented）卡爾曼濾波

)第77頁/共115頁784.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）

擴展Kalman濾波器（EKF）考慮離散時間非線性動態(tài)系統(tǒng)：(4.4.1)

(4.4.2)

其中是時間指標，是時刻的系統(tǒng)狀態(tài)向量，

是系統(tǒng)狀態(tài)演化映射，而是n維過程演化噪聲，

是k時刻對系統(tǒng)狀態(tài)的量測向量，是量測映射，而是m維量測噪聲。

假定和對其變元連續(xù)可微（當用二階擴展Kalman濾波時假定二階連續(xù)可微），同時假定初始狀態(tài)為任意分布，具有均值和協(xié)方差矩陣分別為(4.4.3)

第78頁/共115頁794.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）

過程噪聲是一個零均值的獨立過程，分布任意，具有協(xié)方差矩陣為（4.44）量測噪聲也是一個零均值的獨立過程，分布任意，具有協(xié)方差矩陣為（4.45）過程噪聲、量測噪聲以及初始狀態(tài)之間都相互獨立。擴展Kalman濾波算法實質上是一種在線線性的算法，即按名義軌線進行線性化處理，再利用Kalman濾波公式進行計算。這種算法已經(jīng)不再是按某個指標進行優(yōu)化的最優(yōu)化算法，其性能取決于非線性系統(tǒng)的復雜度以及算法的優(yōu)劣等。第79頁/共115頁804.2卡爾曼濾波——正交投影推導一階EKF算法的步驟描述如下：（1）在時刻，假定已經(jīng)獲得時刻的狀態(tài)估計值和估計誤差的協(xié)方差陣(4.4.6)而此時對演化方程的線性化方程為(4.4.7)其中(4.4.8)(4.4.9)4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）第80頁/共115頁814.2卡爾曼濾波——正交投影推導4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）（2）對時刻狀態(tài)的一步提前預測(4.4.10)狀態(tài)預測誤差是

(4.4.11)狀態(tài)預測誤差的協(xié)方差陣是(4.4.12)（3）對時刻量測的線性化量測方程(4.4.13)其中，(4.4.14)第81頁/共115頁824.2卡爾曼濾波——正交投影推導4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）(4.4.15)（4）對時刻量測的一步提前預測為(4.4.16)量測預測誤差是

(4.4.17)量測預測誤差的協(xié)方差陣是

(4.4.18)狀態(tài)預測誤差與量測預測誤差的協(xié)方差陣是(4.4.19)第82頁/共115頁834.2卡爾曼濾波——正交投影推導在時刻

k得到新的量測狀態(tài)濾波的更新公式

(4.4.20)預測誤差的協(xié)方差陣是(4.4.21)而k時刻Kalman增益陣為(4.4.22)二階EKF算法的步驟描述如下：(1)在時刻假定已經(jīng)獲得時刻的狀態(tài)估計值和估計誤差的協(xié)方差陣(4.4.23)4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）第83頁/共115頁844.2卡爾曼濾波——正交投影推導4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）而此時對演化方程的線性化方程為(4.4.24)其中是第個標準基向量，是第分量，而(4.4.25)(4.4.26)(4.4.27)第84頁/共115頁854.2卡爾曼濾波——正交投影推導4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）（2）對時刻

k狀態(tài)的一步提前預測

(4.4.28)其中表示矩陣求跡，狀態(tài)預測誤差是(4.4.29)

狀態(tài)預測誤差的協(xié)方差陣是(4.4.30)（3）對k時刻量測的線性化量測方程(4.4.31)第85頁/共115頁864.2卡爾曼濾波——正交投影推導4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）其中是第個標準基向量，是

的第分量，而(4.4.32)(4.4.33)(4.4.34)（4）對時刻k量測的一步提前預測(4.4.35)量測預測誤差是

第86頁/共115頁874.2卡爾曼濾波——正交投影推導4.3非線性動態(tài)系統(tǒng)的濾波理論與算法

——擴展Kalman濾波器（EKF）(4.4.36)量測預測誤差的協(xié)方差陣是(4.4.37)狀態(tài)預測誤差與量測預測誤差的協(xié)方差陣是(4.4.38)（5）在時刻k得到新的量測，狀態(tài)濾波的更新公式(4.4.39)預測誤差的協(xié)方差陣是(4.4.40)而時刻k的Kalman增益陣為(4.4.41)第87頁/共115頁88884.3非線性動態(tài)系統(tǒng)的濾波理論與算法—UKF濾波UKF濾波(無跡（Unscented）卡爾曼濾波)雖然把擴展Kalman濾波應用于非線性系統(tǒng)狀態(tài)估計已經(jīng)得到學術界和工程界的認可，但這種應用卻存在著明顯的缺陷。因為EKF為了求取估計誤差協(xié)方差的傳播，將動力學模型在當前狀態(tài)估值處進行Taylor展開線性化，并將測量模型在狀態(tài)一步預測處進行Taylor展開線性化，而基于Taylor級數(shù)展開的方法存在函數(shù)的整體特性（它的平均值）被局部特性（其導數(shù)）所代替的缺點，而且噪聲的存在使之進一步惡化。因此，按Taylor級數(shù)進行高階展開的可用性也值得懷疑。為提高近似迫近的精度。為了改善對非線性問題進行濾波的效果，Julier等人提出了采用基于unscented變換的unscentedKalmanfilter（UKF）方法。該方法在處理狀態(tài)方程時，首先進行第88頁/共115頁894.3非線性動態(tài)系統(tǒng)的濾波理論與算法—UKF濾波無跡（Unscented）卡爾曼濾波

Julier和Uhlman等利用無跡變換（UT，UnscentedTransform）變換方法，用一組確定的取樣點來近似后驗概率，提出了Unscented卡爾曼濾波（UnscentedKalmanFilter）。

UKF不線性化非線性狀態(tài)方程和觀測方程，直接利用非線性狀態(tài)方程來估算狀態(tài)向量的概率密度函數(shù)（pdf），規(guī)定一組確定的取樣點，當狀態(tài)向量的概率密度函數(shù)是高斯型的，利用這組取樣點能獲取高斯密度函數(shù)的均值和協(xié)方差。當高斯型狀態(tài)向量經(jīng)由非線性系統(tǒng)進行傳遞時，對任何一種非線性系統(tǒng)，利用這組取樣點能獲取精確到三階矩的后驗均值和協(xié)方差。與EKF相比，UKF的誤差僅僅出現(xiàn)在三階以上的矩中，而EKF僅僅精確到一階矩。第89頁/共115頁90904.3非線性動態(tài)系統(tǒng)的濾波理論與算法—UKF濾波無跡（Unscented）變換unscented變換（簡稱U變換），然后使用U變換后的狀態(tài)變量進行濾波估計，以減小估計誤差。首先，考慮如下非線性模型(4.4.42)(4.4.43) 在進行濾波前先作如下假設：過程噪聲和測量噪聲為互不相關零均值白噪聲，且過程噪聲

具有協(xié)方差陣

，量測噪聲

具有協(xié)方差陣

，初始狀態(tài)

與所有噪聲獨立，其先驗均值和協(xié)方差陣是 ， (4.4.44)第90頁/共115頁91914.3非線性動態(tài)系統(tǒng)的濾波理論與算法—UKF濾波 定義2.4.1設n維隨機向量

，m維隨機向量 z為x的某一非線性函數(shù) 。

(4.4.45) x的統(tǒng)計特性是，通過非線性函數(shù)進行傳播得到z的統(tǒng)計特性。Unscented變換（UT）就是根據(jù)，設計一系列的點稱其為 點;對設定的點計算其經(jīng)過傳播所得的結果 然后基于，計算。 通常點的數(shù)量取為2n+1，即L=2n。第91頁/共115頁92924.3非線性動態(tài)系統(tǒng)的濾波理論與算法—UKF濾波 UT與線性化方法的比較可用圖2.4.1表示，UT的具體過程可描述如下： 計算2n+1個點及其權系數(shù)

(4.4.46) (4.4.47)第92頁/共115頁93934.3非線性動態(tài)系統(tǒng)的濾波理論與算法—UKF濾波