機(jī)器人智能感知基礎(chǔ) 課件 第6-9章 估計(jì)理論基礎(chǔ) -基于多傳感器信息融合的自動(dòng)駕駛汽車位姿估計(jì)

1第六章估計(jì)理論基礎(chǔ)目錄CONTENT123矩陣微分法最小二乘估計(jì)基礎(chǔ)線性最小方差估計(jì)456最小方差估計(jì)遞推最小二乘估計(jì)卡爾曼濾波理論基礎(chǔ)矩陣微分法6.1定義2對(duì)于

的矩陣函數(shù)

相對(duì)于數(shù)量的微分法定義1對(duì)于

維向量函數(shù)

，定義它對(duì)變量

的導(dǎo)數(shù)為（6.1）（6.2）相對(duì)于數(shù)量的微分法

顯然上述兩個(gè)定義是一致的。當(dāng)矩陣

退化為向量

時(shí)，定義2變?yōu)槎x1。換言之，矩陣（包括向量）函數(shù)對(duì)數(shù)量變量的導(dǎo)數(shù)等于它的各個(gè)元素的導(dǎo)數(shù)，結(jié)果是一個(gè)同階矩陣。根據(jù)上述定義，以下不加證明的直接給出導(dǎo)出的運(yùn)算公式。在下列公式中，

和

都是變量

的矩陣函數(shù)，但它們也可以代表向量函數(shù)（當(dāng)其列數(shù)為1時(shí)），或數(shù)量函數(shù)（當(dāng)其行數(shù)列數(shù)都為1時(shí)），其中

是變量

的數(shù)量函數(shù)。（6.3）（6.4）（6.5）設(shè)函數(shù)

是以向量

為自變量的數(shù)量函數(shù)，即

以個(gè)變量為自變量的數(shù)量函數(shù)。定義3將列向量

稱為數(shù)量函數(shù)

對(duì)列向量的導(dǎo)數(shù)，記作相對(duì)于向量的微分法1、數(shù)量函數(shù)的導(dǎo)數(shù)（6.6）該導(dǎo)數(shù)習(xí)慣上稱為函數(shù)

的梯度，它是三維空間中梯度概念的推廣，可記作

或

。此外，還可以將函數(shù)

對(duì)行向量

的導(dǎo)數(shù)定義為相對(duì)于向量的微分法根據(jù)定義3還可得到以下運(yùn)算公式：（6.7）（6.8）2、向量函數(shù)的導(dǎo)數(shù)設(shè)函數(shù)

是

的

維列向量函數(shù)。定義4

階矩陣函數(shù)稱為

維行向量函數(shù)

對(duì)維列向量

的導(dǎo)數(shù)。相對(duì)于向量的微分法（6.9）稱為

維列向量函數(shù)

對(duì)

維行向量

的導(dǎo)數(shù)。分別記作

和

。

階矩陣函數(shù)根據(jù)定義4顯然可知，導(dǎo)數(shù)矩陣的行數(shù)等于不加轉(zhuǎn)置的向量的行數(shù)，列數(shù)等于加以轉(zhuǎn)置的向量的列數(shù)。據(jù)此則有

。相對(duì)于向量的微分法（6.10）相對(duì)于向量的微分法根據(jù)定義還可以推得以下運(yùn)算公式。以下公式中

和是

維列向量函數(shù)，

是數(shù)量函數(shù)。根據(jù)定義還可以直接驗(yàn)證兩個(gè)有用的公式：（6.11）（6.12）（6.13）和（6.14）其中

是

單位矩陣。定義5

的矩陣函數(shù)設(shè)函數(shù)

是

矩陣函數(shù)，即其中的每個(gè)元素都是

的函數(shù)。稱為

矩陣函數(shù)

對(duì)列向量

的導(dǎo)數(shù)。

的矩陣函數(shù)

稱為

矩陣函數(shù)

對(duì)行向量

的導(dǎo)數(shù)。3、矩陣函數(shù)的導(dǎo)數(shù)相對(duì)于向量的微分法

其中的每個(gè)分塊矩陣是矩陣函數(shù)

對(duì)變量

的導(dǎo)數(shù)矩陣，仍然是一個(gè)

矩陣：相對(duì)于向量的微分法將以上兩種導(dǎo)數(shù)分別記為

和

。一般這兩種導(dǎo)數(shù)不存在互為轉(zhuǎn)置的關(guān)系，即

，但有

成立。不難看出，當(dāng)

退化為向量時(shí)，定義5就與定義4相同了。進(jìn)一步，若

和

都是

維矩陣，

是

維矩陣，

是

的數(shù)量函數(shù)，則根據(jù)定義5以下運(yùn)算等式也成立：相對(duì)于向量的微分法（6.15）（6.16）（6.17）其中

之中的每個(gè)分塊都是

矩陣，所以其本身是一個(gè)

矩陣；

之中每個(gè)分塊都是

矩陣，所以其本身是一個(gè)

矩陣。設(shè)函數(shù)

是以

矩陣

的

個(gè)元素

為自變量的數(shù)量函數(shù)，簡稱以矩陣

為自變量的數(shù)量函數(shù)。例如，函數(shù)

就是以

為自變量的函數(shù)。相對(duì)于矩陣的微分法1、數(shù)量函數(shù)的導(dǎo)數(shù)定義6

維矩陣（6.18）稱為數(shù)量函數(shù)

對(duì)矩陣

的導(dǎo)數(shù)，記作

。設(shè)函數(shù)

是以矩陣

為自變量的維列向量函數(shù)。相對(duì)于矩陣的微分法2、向量函數(shù)的導(dǎo)數(shù)定義7

維矩陣（6.19）稱為列向量函數(shù)

對(duì)

矩陣

的導(dǎo)數(shù)，其中每個(gè)分塊矩陣是個(gè)

矩陣：（6.20）同樣還可以定義行向量函數(shù)

對(duì)

的導(dǎo)數(shù)，它是個(gè)

矩陣，分別記為

和

。設(shè)函數(shù)

是以

矩陣

為自變量的

矩陣函數(shù)。3、矩陣函數(shù)的導(dǎo)數(shù)相對(duì)于矩陣的微分法定義8

矩陣（6.21）稱為矩陣

對(duì)矩陣

的導(dǎo)數(shù)其中每個(gè)分塊矩陣是個(gè)

矩陣：3、矩陣函數(shù)的導(dǎo)數(shù)相對(duì)于矩陣的微分法相對(duì)于矩陣求導(dǎo)的運(yùn)算公式比較復(fù)雜，應(yīng)該根據(jù)具體情況處理。若

是

維列向量，

是

維列向量，

是

矩陣，則根據(jù)定義8以下公式成立：（6.22）該導(dǎo)數(shù)可記作

。（6.23）3、矩陣函數(shù)的導(dǎo)數(shù)相對(duì)于矩陣的微分法（6.24），（6.25）（6.26）（6.27）（6.28）1、數(shù)量函數(shù)的公式復(fù)合函數(shù)微分法以下不加證明直接給出一些最常用的估計(jì)理論相關(guān)的基本公式。用

表示數(shù)量函數(shù)，

表示

維列向量函數(shù)，

表示

維列向量函數(shù)，

表示

維列向量函數(shù)，

表示數(shù)量變量。（1）設(shè)

，

，則（2）設(shè)

，

，則（3）設(shè)

，

，則，，2、向量函數(shù)的公式復(fù)合函數(shù)微分法（1）設(shè)

，

，則（2）設(shè)

，

，則

，（3）設(shè)

，

，則

，最小二乘估計(jì)基礎(chǔ)6.2估計(jì)問題是指根據(jù)觀測(cè)數(shù)據(jù)對(duì)隨機(jī)量進(jìn)行定量推斷的問題。例如，對(duì)飛行器飛行姿態(tài)的估計(jì)、對(duì)機(jī)械臂關(guān)節(jié)轉(zhuǎn)角的估計(jì)等。估計(jì)問題的最優(yōu)解一般是難以得到的，但如果進(jìn)行簡單簡化，假設(shè)被估計(jì)量是不隨時(shí)間變化的參數(shù)，就可視為靜態(tài)估計(jì)問題，進(jìn)一步假設(shè)“最優(yōu)性”是在均方誤差意義下的，則可得到一些理論結(jié)果。通過傳感器進(jìn)行量測(cè)，必然會(huì)有誤差或噪聲，所以觀測(cè)數(shù)據(jù)實(shí)際上是隨機(jī)量，若視被估計(jì)量與觀測(cè)數(shù)據(jù)的統(tǒng)計(jì)性質(zhì)不同，則有各種各樣的估計(jì)方法。最小二乘估計(jì)是二百多年前由高斯提出的一種經(jīng)典的估計(jì)方法。最小二乘估計(jì)量最初是針對(duì)標(biāo)量未知數(shù)進(jìn)行估計(jì)的。為了說明這個(gè)問題，以下給出一個(gè)例子。數(shù)量的最小二乘估計(jì)數(shù)量的最小二乘估計(jì)

例14測(cè)量兩次水下深度，測(cè)量結(jié)果分別為220米和210米，試估計(jì)水下深度的真實(shí)值。

解最直接的方法是取測(cè)量的平均值，即

米作為水下深度的估計(jì)值。該例子使用的估計(jì)方法就是最小二乘估計(jì)方法。以下給出最小二乘估計(jì)法的明確定義。為了對(duì)未知數(shù)量

進(jìn)行了

次測(cè)量，測(cè)量值是（6.29）其中

是已知量，

是第

次的測(cè)量誤差。假設(shè)所求的估計(jì)值記為

，則第

次測(cè)量值與相應(yīng)的估計(jì)值之間的誤差可表示為（6.30）將次誤差的平方和記為（6.31）能使

取極小值的估計(jì)值

就稱為未知數(shù)量

的最小二乘估計(jì)，記作

。使得

取極小值的準(zhǔn)則就稱為最小二乘準(zhǔn)則。根據(jù)最小二乘準(zhǔn)則求估計(jì)值的方法就稱為最小二乘法。數(shù)量的最小二乘估計(jì)現(xiàn)在驗(yàn)證水下深度測(cè)量的例子中得到的結(jié)果是最小二乘估計(jì)。這時(shí)由于

，而

為

，對(duì)其求導(dǎo)數(shù)并令導(dǎo)數(shù)為零，則有

，解得

米。顯然，這是

的極小值點(diǎn)。

由于對(duì)未知數(shù)量

測(cè)量了

次，則可根據(jù)

次測(cè)量結(jié)果求

的最小二乘估計(jì)

。為此，采用向量矩陣形式，記，

，數(shù)量的最小二乘估計(jì)則可把方程（6.29）和（6.31）寫為進(jìn)一步求

的極小值，可得令上式等于零就得到最小二乘估計(jì)（6.32）（6.33）（6.34）（6.35）由于

，所以

，這表明

是最小二乘估計(jì)。數(shù)量的最小二乘估計(jì)例15對(duì)某一未知量進(jìn)行次測(cè)量，測(cè)量值分別為

、…、

，求的最小二乘估計(jì)。

解這時(shí)的測(cè)量方程的向量形式為因此

，則由公式（6.35）可得

，這是

次測(cè)量值的算數(shù)平均值，也是常用的估計(jì)值。以下計(jì)算估計(jì)誤差，由方程（6.35）有數(shù)量的最小二乘估計(jì)（6.36）若假設(shè)測(cè)量誤差

的均值為0，方差矩陣為

，即

，

，則可以算出估計(jì)誤差的均值為（6.37）因此估計(jì)誤差的均值為零，即可稱為無偏估計(jì)。換言之，最小二乘估計(jì)滿足測(cè)量誤差均值為零的條件時(shí)屬于無偏估計(jì)。進(jìn)一步，利用方程（6.36）可以算出估計(jì)誤差的方差為（6.38）假設(shè)被估計(jì)量

是

維向量。在此情況下，關(guān)于最小二乘估計(jì)的定義與前面數(shù)量情形保持一致，需要修改的地方可以通過以下內(nèi)容看出來，在此不在重復(fù)贅述。向量的最小二乘估計(jì)若對(duì)

進(jìn)行了

次測(cè)量，測(cè)量方程分別為（6.39）其中第

次測(cè)量

與同次的測(cè)量誤差

有相同的維數(shù)，但每個(gè)

的維數(shù)不一定是相同的，記其維數(shù)為

，則第

次的測(cè)量矩陣

為一個(gè)

矩陣。向量的最小二乘估計(jì)將全部

次測(cè)量向量復(fù)合而成一個(gè)維數(shù)為

的向量為

，并相應(yīng)地定義

矩陣

和

維向量

分別為因此可將全部測(cè)量方程（6.39）寫為一個(gè)方程（6.40）而這時(shí)按照最小二乘準(zhǔn)則的誤差平方和為，（6.41）向量的最小二乘估計(jì)將方程（6.40）和（6.41）和方程（6.32）和（6.33）比較看出，除了向量的維數(shù)不同以外，其余部分完全一致。因此，可以用類似的推導(dǎo)方法得到最小二乘估計(jì)向量為估計(jì)誤差向量為（6.42）（6.43）假設(shè)

的均值和方差矩陣分別為

和

，則得估計(jì)誤差的均值為（6.44）所以這時(shí)的

也是無偏估計(jì)，得到估計(jì)誤差的方差矩陣為（6.45）向量的最小二乘估計(jì)將方程（6.42）、（6.43）和（6.45）與方程（6.35）、（6.36）和（6.38）進(jìn)行比較可知，除了向量矩陣的維數(shù)不同之外，其余完全一致。但有一點(diǎn)需要注意：在被估計(jì)量是數(shù)量時(shí)，

是一個(gè)大于零的數(shù)量，其逆必然存在；但被估計(jì)量為向量時(shí)，

是一個(gè)

維方陣，其逆不一定存在。在上述推到過程中都假設(shè)

一定存在，這相當(dāng)于假設(shè)矩陣

的秩為

，而

往往是大于

的。加權(quán)最小二乘估計(jì)在以上敘述中，所采用的估計(jì)準(zhǔn)則（6.33）和（6.41）是對(duì)每次測(cè)量結(jié)果都同等看待的。這自然產(chǎn)生一個(gè)問題，因?yàn)槊看蔚挠^測(cè)誤差不一定相同，采用如此的準(zhǔn)則是否合理？例如例3中第一次觀測(cè)誤差的方差是

，第二次的觀測(cè)誤差的方差是

，顯然第二次的觀測(cè)更加準(zhǔn)確，理應(yīng)更重視第二次觀測(cè)結(jié)果。實(shí)際上，如果第二次觀測(cè)誤差的方差是零，那么只需要利用第二次觀測(cè)數(shù)據(jù)足以估計(jì)出

，根本不需要再進(jìn)行計(jì)算了。以上采用的平等看待每次測(cè)量的準(zhǔn)則顯然與此不符，必須加以改進(jìn)。事實(shí)上，觀測(cè)誤差

的方差越小，在準(zhǔn)則（6.33）和（6.41）中的相應(yīng)項(xiàng)

或者

所乘的加權(quán)因子就應(yīng)該越大。加權(quán)最小二乘估計(jì)利用矩陣微分方法，對(duì)方程（6.46）求導(dǎo)可得綜上所述，以下來研究使得加權(quán)之后的最小二乘準(zhǔn)則（6.46）達(dá)到最小值的最小二乘估計(jì)，其中

是一個(gè)適當(dāng)選取的對(duì)稱正定加權(quán)矩陣。當(dāng)

時(shí)，最小二乘準(zhǔn)則

就化為前述的準(zhǔn)則

了。由于數(shù)量情況和向量情況的推到完全一樣，此處不加區(qū)別敘述。（6.47）令上式等于零，則得到

的加權(quán)最小二乘估計(jì)（6.48）加權(quán)最小二乘估計(jì)由此可算出加權(quán)最小二乘估計(jì)的誤差（6.49）若測(cè)量誤差

的均值為零，即

，則有（6.50）所以這時(shí)的

是無偏估計(jì)。若設(shè)

的方差矩陣為

，則得估計(jì)誤差的方差矩陣為（6.51）加權(quán)最小二乘估計(jì)需要說明的是

是合理的，特別當(dāng)矩陣

為該對(duì)角矩陣時(shí)更明顯。它表示每一次加權(quán)因子與該次測(cè)量誤差的方差成正比，這正是人們所期望的。當(dāng)

時(shí)，誤差方差矩陣達(dá)到最小值，這時(shí)方程（6.48）和（6.51）變?yōu)?，?.52）注意到上式中矩陣

和

都是已知的，因此要問如何選取加權(quán)矩陣

才能使上式取最小值？以下不加證明直接給出這一問題的答案。線性最小方差估計(jì)6.3

在做最小二乘不估計(jì)時(shí)對(duì)被估計(jì)量

與測(cè)量

的統(tǒng)計(jì)性質(zhì)沒有任何要求，現(xiàn)在假定被估計(jì)量與測(cè)量的前兩階矩，即均值

和

，方差矩陣

和

及協(xié)方差矩陣

都已知了。在此情況下，充分利用這些統(tǒng)計(jì)知識(shí)，對(duì)統(tǒng)計(jì)量的函數(shù)形式適當(dāng)加以限制，就能得到很有意義的結(jié)果。我們限定估計(jì)量是測(cè)量

的線性函數(shù)，及設(shè)估計(jì)量為（6.53）這樣形式的估計(jì)稱為線性估計(jì)。它有便于計(jì)算的優(yōu)點(diǎn)。前面介紹的最小二乘估計(jì)就是線性估計(jì)。設(shè)被估計(jì)量

的維數(shù)為

，測(cè)量

的維數(shù)為

，則可以將問題明確如下。

線性最小方差估計(jì)，限定估計(jì)量是觀測(cè)

的線性函數(shù)，即方程（6.53），其中

是待定的

維向量，

是待定的

矩陣，而使均方誤差矩陣（6.54）達(dá)到最小值的線性估計(jì)（6.53）稱為線性最小方差估計(jì)，記為

。線性最小方差估計(jì)也可以使用下述準(zhǔn)則定義，即使均方誤差矩陣的跡，也就是各分量的均方誤差之和（6.55）達(dá)到最小值的線性估計(jì)（6.53）稱為線性最小方差估計(jì)。可以證明，按準(zhǔn)則（6.54）和同按準(zhǔn)則（6.55）求出的線性估計(jì)（6.53）是一致的。由于準(zhǔn)則（6.55）計(jì)算方便一些，所以以它作為依據(jù)。最小方差估計(jì)6.4

現(xiàn)在假設(shè)

和

的聯(lián)合概率密度

已知，這樣

的概率密度

，

的概率密度

，再給定

條件下

的條件概率密度

，在給定

條件下

的條件概率密度

也都是已知的。顯然，這時(shí)應(yīng)該能得到比線性最小方差估計(jì)更優(yōu)越的估計(jì)結(jié)果了。

已知被估計(jì)量

與測(cè)量

的聯(lián)合概率密度

，在此情況下使得各分量均方誤差之和（6.60）取最小值的估計(jì)

稱為最小方差估計(jì)，將其記為

。

同線性最小方差估計(jì)相比，需要的統(tǒng)計(jì)知識(shí)更多，但去掉了估計(jì)量必須是測(cè)量

的線性函數(shù)的限制。仍假設(shè)

的維數(shù)為

，

的維數(shù)為

，這時(shí)存在如下的定理。定理最小方差估計(jì)

等于

的條件均值（6.61）它是測(cè)量

的函數(shù)。證明將（6.60）寫成積分形式有（6.62）上式對(duì)任何估計(jì)量

都成立。當(dāng)前的任務(wù)是要選一個(gè)估計(jì)量

使得上式取最小值。事實(shí)上，對(duì)于給定的測(cè)量

而言，使上式取最小值等價(jià)于確定

使

取最小值，求該式對(duì)向量

的導(dǎo)數(shù)可得

，令該式等于零，對(duì)于給定的測(cè)量而言，

是

的確定估計(jì)量，可以提到積分號(hào)之外，并簡記為

，則得到

，根據(jù)該式和方程（6.62）可得

。證畢。遞推最小二乘估計(jì)6.5首先回顧一下最小二乘估計(jì)。假設(shè)對(duì)被估計(jì)量

進(jìn)行了

次測(cè)量，測(cè)量方程為（6.63）為了強(qiáng)調(diào)測(cè)量的次數(shù)為

，采用以下記號(hào)，，進(jìn)而將方程（6.63）寫為

。由6.2節(jié)可知

的最小二乘估計(jì)為（6.64）現(xiàn)在假定在

次測(cè)量之后又得到了另外一次測(cè)量：（6.65）若把此新的測(cè)量加到之前的測(cè)量向量中就得到復(fù)合測(cè)量方程：（6.66）其中，，同理，根據(jù)

次測(cè)量的新的估計(jì)量為（6.67）例23試根據(jù)前兩次測(cè)量

，

及第三次測(cè)量

，分別求未知二維向量

的最小二乘估計(jì)。解在本例中顯然有

，

，代入公式（6.64）可得從例23中可以看出，加一次新的測(cè)量后，若直接利用公式（6.67）計(jì)算，則還需要重新求一個(gè)矩陣

的逆，這是很麻煩的。此外，這種算法必須將全部數(shù)據(jù)保存下來，如果測(cè)量維數(shù)高數(shù)據(jù)多，那么就要占用計(jì)算機(jī)的大量內(nèi)存單元。如何解決這個(gè)困難呢？比較例23中

和

可知，利用矩陣的分塊乘法是可以導(dǎo)出一組遞推公式的。為了方便敘述，定義

或

，則（6.64）式可寫為

，而由（6.67）式有

，其中利用矩陣求逆公式可得

，將其代入

可得（6.68）通常利用公式（6.68）可以節(jié)省計(jì)算量，被估計(jì)量

的維數(shù)為

，則

自然也包括

是一個(gè)

矩陣（假定測(cè)量次數(shù)

足夠大，

的行數(shù)不小于

），而利用方程（6.64）或（6.67）求估計(jì)量就需要計(jì)算一個(gè)

矩陣的逆矩陣。但是，矩陣

是個(gè)

矩陣，其中

是第

次測(cè)量

的維數(shù)，它往往小于

，計(jì)算低階的逆矩陣當(dāng)然簡單多了。特別是，當(dāng)

為數(shù)量時(shí)，矩陣

也是數(shù)量，求逆就成簡單的除法了。例24利用公式（6.68）求例23中的解由例23可知

，

，

，

，代入

這與例23結(jié)果是一致的。公式（6.68）可知從例24可以想到，有可能可以更好地利用第步的數(shù)據(jù)和第次測(cè)量來求第步的估計(jì)。由公式可得（6.69）上式右端第一項(xiàng)可由

得到

，由公式

可解出

，將其代入

可得將該式代入方程（6.69）并經(jīng)過整理可得（6.70）這就是所需要的遞推公式，而在計(jì)算

時(shí)可用公式（6.71）來計(jì)算。因此公式（6.70）和（6.71）共同組成了遞推最小二乘估計(jì)公式。卡爾曼濾波理論基礎(chǔ)6.6預(yù)備知識(shí)

所謂濾波一般是指把污染信號(hào)里的噪聲盡可能地消除，從中分離出所需要的信號(hào)。本節(jié)中所介紹的濾波是由帶有噪聲的狀態(tài)方程和觀測(cè)方程去推斷系統(tǒng)的狀態(tài)信號(hào)，所以本質(zhì)上也是一種估計(jì)。但是與前介紹的估計(jì)方法不同的是，在本節(jié)中的被估計(jì)量，即系統(tǒng)的狀態(tài)量，是隨著時(shí)間變化的。相對(duì)應(yīng)的，前面解決的估計(jì)問題可稱為參數(shù)估計(jì)，而這一節(jié)則稱為狀態(tài)估計(jì)。

由于軍事上的原因，在第二次世界大戰(zhàn)期間，維納濾波得到了廣泛應(yīng)用，但其不足之處在于：必須用到全部的歷史觀測(cè)數(shù)據(jù)，存儲(chǔ)量和計(jì)算量都非常大；當(dāng)獲得新的觀測(cè)數(shù)據(jù)時(shí)，沒有合適的遞推算法；很難用于解決非平穩(wěn)過程的濾波問題。預(yù)備知識(shí)

現(xiàn)在要對(duì)一個(gè)變量

進(jìn)行估計(jì)。假設(shè)已經(jīng)得到了這個(gè)變量的兩個(gè)獨(dú)立無偏估計(jì)

和

，它們的準(zhǔn)確度不必相同，即

和

的均方誤差可以有大有小。我們的目標(biāo)是要根據(jù)這兩個(gè)獨(dú)立的估計(jì)，構(gòu)成一個(gè)新的加權(quán)估計(jì)

，使得

有最小的均方誤差。以下先介紹單維的情況，再介紹多維的情況。為了克服上述缺點(diǎn)，在上世紀(jì)六十年代初，卡爾曼和布西等人發(fā)展了一種遞推濾波方法，即現(xiàn)在的卡爾曼濾波。不足的是，這種濾波方法設(shè)計(jì)的數(shù)學(xué)較深，難以被工程人員理解。鑒于此，我們將遵循諾頓和巴哈謨等人的思路，采用較為初等的方法得出卡爾曼濾波方程組，雖然推導(dǎo)步驟冗長，但對(duì)理解問題的本質(zhì)具有一定的幫助。1、單維情況預(yù)備知識(shí)設(shè)

和

是變量

的兩個(gè)獨(dú)立無偏估計(jì)，誤差的方差分別為

和

。現(xiàn)在利用這兩個(gè)估計(jì)構(gòu)成一個(gè)新的估計(jì)，但限定為線性估計(jì)，即（6.72）其中

是加權(quán)因子。取

表示只相信估計(jì)

；取

表示對(duì)兩個(gè)估計(jì)同等看待；取

表示只相信估計(jì)

?，F(xiàn)在的問題就是要確定

的值，使得估計(jì)誤差有最小的方差。換言之，最小方差仍然是估計(jì)的準(zhǔn)則。需要指出的是，由于所討論的估計(jì)都是無偏估計(jì)，均方誤差同誤差的方差含義一樣，可以不加區(qū)別。下面來求估計(jì)誤差

的方差，記為

，則由（6.72）式可得（6.73）預(yù)備知識(shí)因?yàn)?/p>

和

是獨(dú)立估計(jì)，自然

與

無關(guān)，又因?yàn)楣烙?jì)都是無偏的，所以（6.74）將其代入（6.73）后，方程（6.73）變?yōu)椋?.75）為了求

的極值，取它對(duì)

的導(dǎo)數(shù)（6.76）

或令其等于零，可解出加權(quán)因子的最優(yōu)值（6.77）由此可知，各個(gè)估計(jì)的加權(quán)因子與各估計(jì)的方差成反比。由此可知，各個(gè)估計(jì)

的加權(quán)因子與各估計(jì)的方差

成反比。預(yù)備知識(shí)將方程（6.76）代入（6.72）得出線性最小方差估計(jì)（6.78）代入方程（6.75）可給出

的方差（6.79）以上兩個(gè)方程很容易變?yōu)橐韵滦问剑?.80）（6.81）這種遞推的形式含義是很明顯的，設(shè)

是未知變量

原有的估計(jì)，在得到另一估計(jì)

后，就應(yīng)根據(jù)差值

來改正原有的估計(jì)。為了要得到最小方差估計(jì)，所以乘以按公式（6.76）確定的加權(quán)因子。從方程（6.81）可以看出，改正后的估計(jì)

比原有估計(jì)

其誤差的方差是減小了。2、多維情況

設(shè)

和

是未知向量

的兩個(gè)獨(dú)立無偏估計(jì)，它們都是

維的，其估計(jì)誤差的方差矩陣分別記作

和

，即預(yù)備知識(shí)（6.82）（6.83）我們僅考慮線性估計(jì)，則基于估計(jì)

和

的最優(yōu)估計(jì)，假設(shè)取下列形式（6.83）以下需要確定加權(quán)矩陣

使得估計(jì)誤差

的方差陣最小，或者其各分量的方差之和最小。由方程（6.83）可得

，因此有（6.84）

其中利用了

和

是獨(dú)立無偏估計(jì)的性質(zhì)，因而

。將方程（6.84）的左端簡記為

，右端展開得到預(yù)備知識(shí)（6.85）下面要做的就是選擇加權(quán)矩陣

使得估計(jì)誤差

的各分量的方差之和取最小值，即使（6.86）取最小值。采用矩陣微分法，利用矩陣微分公式并考慮到矩陣

和

都是對(duì)稱矩陣，因而

，便得到預(yù)備知識(shí)（6.87）令上式等于零可得

。由此便得到使

為線性最小方差估計(jì)的加權(quán)矩陣（6.88）將其代入（6.83）可得線性最小方差估計(jì)（6.89）將其代入方程（6.85）可得估計(jì)誤差

的最小方差陣（6.90）設(shè)

和

是

維未知向量

的兩個(gè)獨(dú)立無偏估計(jì)。

與

的維數(shù)相同，誤差方差陣為

；

的維數(shù)是

，誤差方差陣為

?，F(xiàn)在要根據(jù)估計(jì)

和

確定的線性最小方差估計(jì)。這時(shí)，可以假設(shè)3、維數(shù)不同的情況（6.91）其中

是一個(gè)確定的常值矩陣，而

可以認(rèn)為是

的一個(gè)同維估計(jì)。以下會(huì)看到這正是我們要討論的實(shí)際情況。將方程（6.83）中的加權(quán)矩陣

相應(yīng)于方程（6.91）寫成（6.92）則得到（6.93）其中

是待定的矩陣，目的是使

為線性最小方差估計(jì)。因?yàn)?/p>

，則將其與（6.93）相減可得預(yù)備知識(shí)（6.94）進(jìn)一步根據(jù)方程（6.91）和（6.94）可得誤差方差陣為（6.95）簡記上式左端的方差陣為

，并將右端展開可得進(jìn)一步可知預(yù)備知識(shí)令上式等于零可得（6.96）這就是所求的矩陣。將它代入方程（6.93）就得到線性最小方差估計(jì)（6.97）這一結(jié)果和方程（6.80）的含義和形式一致。現(xiàn)在把方程（6.96）代入（6.95），求誤差方差陣

，為此需要用到下列矩陣等式（6.98）該式可分為兩步來證明。首先，根據(jù)矩陣乘法，容易驗(yàn)證

，再用矩陣

左乘該式兩端，然后再用

右乘該式兩端，就得到等式（6.98）。預(yù)備知識(shí)利用等式（6.98）可將方程（6.96）改寫為（6.99）將該式代入方程（6.95）得（6.100）由于，利用該結(jié)果則方程（6.100）變?yōu)椋?.101）進(jìn)一步，利用矩陣求逆公式和方程（6.96）又可將（6.101）寫成預(yù)備知識(shí)（6.102）現(xiàn)在將上述結(jié)果概述如下：設(shè)

和

是未知向量

的兩個(gè)獨(dú)立無偏估計(jì)，其誤差方差陣分別

為和

，則

根據(jù)

和

作出的線性最小方差估計(jì)是（6.103）其中矩陣（6.104）

這一結(jié)果是非常重要的。方程（6.103）和（6.104）實(shí)際上可以用作遞推公式：原有估計(jì)為

，當(dāng)?shù)玫搅硪还烙?jì)

時(shí)，就用方程（6.103）進(jìn)行校正，其中第二項(xiàng)代表校正項(xiàng)，它與兩個(gè)估計(jì)的差

成正比例，加權(quán)矩陣

與兩個(gè)估計(jì)的誤差方差矩陣

和

有關(guān)。這些都是易于想到的。此外，從方程（6.105）還能看到，經(jīng)過校正后誤差方差矩陣比起

時(shí)減小了。在以上討論中，要求兩個(gè)估計(jì)

和

是獨(dú)立的，其實(shí)只要它們的協(xié)方差矩陣滿足就能應(yīng)用本節(jié)的結(jié)論。預(yù)備知識(shí)

從牛頓力學(xué)的觀點(diǎn)來看，只要系統(tǒng)的初始狀態(tài)和運(yùn)動(dòng)方程一旦確定，那么系統(tǒng)在任何時(shí)刻的未來狀態(tài)都是能夠精確地加以預(yù)報(bào)的。事實(shí)遠(yuǎn)非如此，那不過是理想的情況而已。事實(shí)上，客觀世界總存在干擾或噪聲，因此任何一個(gè)實(shí)際系統(tǒng)總包括兩部分，一部分是確定的，另一部分是隨機(jī)的。換言之，一個(gè)物理系統(tǒng)的狀態(tài)無論是過去、現(xiàn)在或者未來都具有某種統(tǒng)計(jì)的性質(zhì)，是不能精確知道的。這自然出現(xiàn)一個(gè)問題，即如何根據(jù)已有數(shù)據(jù)來對(duì)系統(tǒng)的過去、現(xiàn)在或者未來的狀態(tài)作出滿意的估計(jì)。離散系統(tǒng)卡爾曼濾波假設(shè)對(duì)未知量

在各個(gè)離散時(shí)刻1、2、…、j進(jìn)行了j次觀測(cè)，觀測(cè)結(jié)果記為

、

、…

、

。為易于理解，可以把它們?cè)O(shè)想為在第1秒到第j秒的觀測(cè)，再有未知量

和觀測(cè)

不一定是同維的。通常

是的維數(shù)比

的低一些。利用這j個(gè)觀測(cè)就可以對(duì)未知量在時(shí)刻的狀態(tài)進(jìn)行估計(jì)。將這個(gè)估計(jì)值記為

，顯然它是上述j個(gè)觀測(cè)的某一函數(shù)：離散系統(tǒng)卡爾曼濾波（6.106）選擇不同的最優(yōu)準(zhǔn)則或最優(yōu)指函數(shù)，

將取不同的形式。這里共有三種情況：

預(yù)測(cè)問題當(dāng)

時(shí)這種估計(jì)問題稱為預(yù)測(cè)問題，即根據(jù)過去的觀測(cè)來預(yù)測(cè)未來的狀態(tài)。預(yù)測(cè)問題又稱為外推問題。

濾波問題當(dāng)

時(shí)稱為濾波問題，即根據(jù)過去直到現(xiàn)在的觀測(cè)來估計(jì)現(xiàn)在的狀態(tài)。

平滑問題當(dāng)

時(shí)稱為平滑問題，即根據(jù)過去和現(xiàn)在的觀測(cè)來估計(jì)過去的狀態(tài)。平滑問題又稱為內(nèi)插問題。以下僅討論濾波問題，相對(duì)而言在這三個(gè)問題中它不是較難的，而且用處較廣。最優(yōu)準(zhǔn)則仍然是使均方誤差陣取最小。由于在本節(jié)中得到的估計(jì)都是無偏估計(jì)，均方誤差陣就是誤差的方差陣。換言之，最優(yōu)準(zhǔn)則是使誤差的方差陣取最小值，符合這一準(zhǔn)則的估計(jì)稱為最優(yōu)估計(jì)。設(shè)系統(tǒng)的狀態(tài)方程為1、系統(tǒng)的數(shù)學(xué)模型離散系統(tǒng)卡爾曼濾波（6.107）,其中

是指系統(tǒng)在

時(shí)刻的

維狀態(tài)向量；

是指系統(tǒng)從時(shí)刻

到時(shí)刻

的

狀態(tài)轉(zhuǎn)移矩陣，或簡稱狀態(tài)轉(zhuǎn)移矩陣，它是可逆的；

表示

維隨機(jī)序列

中相應(yīng)于時(shí)刻

的

維隨機(jī)向量，統(tǒng)稱為輸入（或動(dòng)態(tài)、模型）噪聲。設(shè)系統(tǒng)的觀測(cè)方程為（6.108）其中

表示在時(shí)刻

獲得的

維觀測(cè)數(shù)據(jù)；

表示系統(tǒng)在時(shí)刻

的

維觀測(cè)矩陣，或簡稱觀測(cè)矩陣；

表示

維隨機(jī)向量

中相應(yīng)于時(shí)刻

的

維隨機(jī)向量，稱為觀測(cè)噪聲。容易看出，上述的

和

代表系統(tǒng)的隨機(jī)部分。若要得到有意義的結(jié)果，需要對(duì)

和

的統(tǒng)計(jì)性質(zhì)有相應(yīng)的了解。統(tǒng)計(jì)性質(zhì)不同，問題的難易程度及其答案就因之而異。關(guān)于

和

的統(tǒng)計(jì)性質(zhì)，最簡單而又最基本的模型是假定它們的為零均值的白噪聲序列，即離散系統(tǒng)卡爾曼濾波，，（6.108）（6.109）其中

（Kroneckerdelta）根據(jù)其定義是由此從方程（6.109）可知任意兩個(gè)不同時(shí)刻的噪聲都是不相關(guān)的，這就是“白噪聲”的含義。此外，還可以假定輸入噪聲

與觀測(cè)噪聲

互不相關(guān)，即對(duì)所有的k和j都滿足離散系統(tǒng)卡爾曼濾波（6.110）又因?yàn)橄到y(tǒng)的初始狀態(tài)

與

和

也互不相關(guān)，即，且其均值和方差矩陣分別為，綜上所述，我們研究的系統(tǒng)數(shù)學(xué)模型如下：n維狀態(tài)方程和m維觀測(cè)方程分別為，，（6.113）（6.114）輸入噪聲

和觀測(cè)噪聲

的統(tǒng)計(jì)特性為離散系統(tǒng)卡爾曼濾波，，（6.115）（6.116）（6.117）系統(tǒng)初始狀態(tài)的統(tǒng)計(jì)特性為，（6.118）（6.119）（6.120）結(jié)束以上準(zhǔn)備知識(shí)，可以證明下面的定理。2、卡爾曼濾波遞推公式離散系統(tǒng)卡爾曼濾波定理在上述方程（6.115）-（6.120）表明的假設(shè)條件下，系統(tǒng)（6.113）和（6.114）的狀態(tài)

的最優(yōu)線性濾波

可用下式遞推計(jì)算：，（6.121）其中（6.122）（6.123）（6.124）且，這個(gè)定理是在1960年首先由卡爾曼證明的。由方程（6.121）-（6.124）所描述的遞推濾波算法就是所謂的卡爾曼濾波算法，其中方程（6.121）被稱為濾波方程；（6.122）被稱為增益方程；（6.123）被稱為預(yù)報(bào)誤差（先驗(yàn)）方差方程；（6.124）被稱為濾波誤差（后驗(yàn)）方差方程。離散系統(tǒng)卡爾曼濾波證明（1）已知估計(jì)：我們假設(shè)系統(tǒng)在時(shí)刻

的狀態(tài)

的估計(jì)

是已知的，誤差方差矩陣記為（6.125）例如當(dāng)

時(shí)，由給定條件可知（6.126）（2）一步預(yù)報(bào)：有了第

步的估計(jì)

，加上狀態(tài)方程（6.113），就可以對(duì)狀態(tài)

作出初步估計(jì)。將

代入方程（6.113）可得離散系統(tǒng)卡爾曼濾波（6.127）（6.128），由前面的知識(shí)知道隨機(jī)向量在無其他測(cè)量數(shù)據(jù)時(shí)，其最小方差估計(jì)就是它的均值，則求上式均值可得因?yàn)?/p>

是確定的估計(jì)值，而根據(jù)假定

的均值為零可得，

需要強(qiáng)調(diào)此時(shí)的估計(jì)只是根據(jù)已有估計(jì)

和狀態(tài)方程（6.113）而得到的，因此將其記為

，即（6.129）并稱為最優(yōu)一步預(yù)報(bào)估計(jì)，它還不是待求得對(duì)

得最后估計(jì)?，F(xiàn)在還需要計(jì)算該估計(jì)得誤差方差矩陣，記估計(jì)誤差為離散系統(tǒng)卡爾曼濾波（6.130）并將方程（6.113）和（6.129）兩邊相減可得

，因此（6.131）根據(jù)給定條件（6.115）和（6.120），并利用狀態(tài)方程的遞推解法可以證明

與

是互不相關(guān)的，即

。據(jù)此并利用條件（6.115）和（6.125），則方程（6.131）可化為

，將該式左端記為

，則有（6.132）（3）觀測(cè)估計(jì)：通過觀測(cè)得到觀測(cè)方程（6.114），即離散系統(tǒng)卡爾曼濾波（6.133）這時(shí)可以把

視為對(duì)狀態(tài)

的另一個(gè)估計(jì)（6.91），其誤差方差矩陣根據(jù)給定條件（6.116）應(yīng)為（6.134）總之，我們從估計(jì)

出發(fā)分別通過系統(tǒng)的狀態(tài)方程和估測(cè)方程得出關(guān)于狀態(tài)

的兩個(gè)估計(jì)：

和

，及其誤差方差矩陣

和

。下一步就是要根據(jù)這兩個(gè)估計(jì)來構(gòu)造

的最優(yōu)線性估計(jì)。根據(jù)給定條件（6.120）可以證明即

的兩個(gè)估計(jì)

和

滿足要求。因此只要注意到下列的對(duì)應(yīng)關(guān)系

、

、

、

、

，再利用公式（6.97）就得到最優(yōu)線性濾波（6.121），其中初始值是給定的；利用公式（6.96）就得到（6.122）；利用公式（6.95）就得到（6.124），其中初始值是給定的；再加上公式（6.132）就完成了對(duì)定理的證明。至于

和

的另外兩種形式，根據(jù)上一節(jié)的結(jié)果都是易于證明的。離散系統(tǒng)卡爾曼濾波（1）方程（6.121）表示從第

步出發(fā)，根據(jù)系統(tǒng)的狀態(tài)方程和第

步的觀測(cè)方程，就可以作出第

步狀態(tài)

的最優(yōu)線性濾波

。既然開始的估計(jì)值

是已知的，那就可以依次作出以后各步的估計(jì)，即

、

、…、

、…。換言之，卡爾曼濾波具有遞推性質(zhì)。3、關(guān)于卡爾曼濾波的幾點(diǎn)說明（2）利用方程（6.129），方程（6.121）又可以寫為，（6.135）其中右端第一項(xiàng)

是

的一個(gè)初步估計(jì)，即最優(yōu)一步預(yù)報(bào)估計(jì)；第二項(xiàng)括號(hào)內(nèi)的表達(dá)式表示第k次觀測(cè)

與其預(yù)報(bào)值

之差，將其記作

，并稱為第k次觀測(cè)中的新息。如果它等于零，那么這次觀測(cè)就沒有帶來任何新的信息，一步預(yù)報(bào)

就是

的最優(yōu)估計(jì)。如果它不等于零，那就表示第k次觀測(cè)

中還包含有由

表現(xiàn)出來的前k-1次觀測(cè)中所沒有的新信息，因?yàn)?/p>

是根據(jù)前k-1次觀測(cè)利用遞推關(guān)系得到的。（3）從公式（6.135）可以看出，最優(yōu)線性濾波具有“反饋校正”的性質(zhì)，其中前一項(xiàng)

是原有的一步預(yù)報(bào)值；第二項(xiàng)是與第k次觀測(cè)的新息成比例的校正項(xiàng)。比例矩陣

稱為卡爾曼濾波增益矩陣。從直觀上可以推斷，如果一步預(yù)報(bào)

很準(zhǔn)確，或者

很小，那么觀測(cè)

的意義就很小，因此增益矩陣

就不會(huì)太大。這從方程（6.122）中可以直接看出。極端地說，若

，則

就是

的準(zhǔn)確值，自然應(yīng)該有

，即根本無需校正。另一方面，如果觀測(cè)

很準(zhǔn)確，即

很小，那么觀測(cè)

就很重要，因此增益矩陣就要大一些。這從方程（6.122）就可以直接看出。3、關(guān)于卡爾曼濾波的幾點(diǎn)說明離散系統(tǒng)卡爾曼濾波極端地說，如果

趨近于零，以至于

和

相比可以忽略不計(jì)時(shí)，則由方程（6.122）可得

，將其代入方程（6.121）可得

，這和加權(quán)最小二乘估計(jì)結(jié)果完全一致，原因在于

表明觀測(cè)

沒有誤差，只要根據(jù)觀測(cè)方程

求出

的加權(quán)最小二乘估計(jì)就行了。這時(shí)加權(quán)最小二乘估計(jì)與線性最小方差估計(jì)的含義是相同的。綜合以上討論可知，

是與

成正比而與

成反比的。由于

的大小隨

而增減，所以可以更直觀地說，輸入噪聲越強(qiáng)，增益矩陣

越大，這表示應(yīng)該看重觀測(cè)；觀測(cè)噪聲越強(qiáng)，增益矩陣越小，這表示應(yīng)該看重原有估計(jì)。（4）最優(yōu)線性濾波是無偏估計(jì)，因?yàn)榫€性最小方差估計(jì)必定是無偏估計(jì)。3、關(guān)于卡爾曼濾波的幾點(diǎn)說明離散系統(tǒng)卡爾曼濾波考慮如下的狀態(tài)方程和觀測(cè)方程所描述的系統(tǒng)，其中

是外加的

維控制，并非隨機(jī)向量，

是

常數(shù)矩陣；

是l維輸入噪聲，

是常

矩陣；其余各量的含義與前一節(jié)相同。一般線性離散系統(tǒng)的濾波（6.136）（6.137）以下講討論的情況中，假定：（1）噪聲是零均值的，即

，

；（2）噪聲都是白色的，即

，

；（3）上述兩種噪聲在同一時(shí)刻之間可以是相關(guān)的，其協(xié)方差陣為

；（4）系統(tǒng)的初始狀態(tài)與上述噪聲是不相關(guān)的，即

，

，且其均值和方差陣為

，

?？梢宰C明，在以上假設(shè)條件下，對(duì)系統(tǒng)（6.136）和（6.137）而言存在下述定理。定理在滿足假定條件（1）~（4）的情況下，系統(tǒng)（6.136）和（6.137）的最優(yōu)線性濾波方程為一般線性離散系統(tǒng)的濾波最優(yōu)一步預(yù)報(bào)方程為其中初始濾波為

，且

；增益方程為預(yù)報(bào)誤差方差方程為濾波誤差方差方程為其初始值為

。該定理顯然是前一節(jié)定理的推廣，它的證明比較復(fù)雜，在此不再引述，但又以下幾點(diǎn)需要說明：（1）作為特例，若有

，

，

，則此定理變?yōu)榍耙还?jié)中的定理，這是很容易驗(yàn)證的結(jié)論；（2）若有

，

，則除了前一節(jié)定理中的

換成本定理中的

之外，兩個(gè)定理的其余部分完全一樣，證明也一樣；（3）若有

，則本定理的證明仍可仿照前一節(jié)定理的證明給出，不再贅述。一般線性離散系統(tǒng)的濾波這個(gè)問題比較困難，嚴(yán)格的闡述必須涉及隨機(jī)微分方程的理論，這就超出了本書的范圍。事實(shí)上，多數(shù)文獻(xiàn)也包括卡爾曼的原作對(duì)此問題的論證都是不夠嚴(yán)格的。下面采用的敘述方法，就是卡爾曼首先用以推導(dǎo)最優(yōu)濾波方程的方法。為便于理解，此處適當(dāng)作了簡化，因此只具有形式推證的作用。這種方法的實(shí)質(zhì)在于：利用極限法，從離散系統(tǒng)的結(jié)果推出連續(xù)系統(tǒng)的結(jié)果。連續(xù)系統(tǒng)的卡爾曼濾波1、系統(tǒng)的數(shù)學(xué)模型考慮系統(tǒng)由下列狀態(tài)方程和觀測(cè)方程：其中

和

是

維狀態(tài)向量和

維觀測(cè)向量；

和

分別是

維輸入噪聲和

維觀測(cè)噪聲向量；

和

分別是連續(xù)的

和

矩陣。（6.138）（6.139）以下僅討論下述簡單而典型的情況，假設(shè)：連續(xù)系統(tǒng)的卡爾曼濾波（1）隨機(jī)過程

和

都是零均值白噪聲過程，其協(xié)方差陣分別為（6.140）（6.141）其中

函數(shù)的定義是且

，常稱為狄拉克

函數(shù)；

矩陣

是連續(xù)非負(fù)定的；

矩陣

是連續(xù)正定的。（2）輸入噪聲隨機(jī)過程與觀測(cè)噪聲隨機(jī)過程是互不相關(guān)的，即（6.142）（3）系統(tǒng)初始狀態(tài)

的均值和方差矩陣分別為連續(xù)系統(tǒng)的卡爾曼濾波，（6.143）且和上述兩個(gè)隨機(jī)過程都不相關(guān)，即，（6.144）目前的問題是，當(dāng)觀測(cè)

給定時(shí)，確定系統(tǒng)（6.138）和（6.139）的最優(yōu)線性濾波。2、卡爾曼濾波公式對(duì)于系統(tǒng)（6.138）和（6.139）而論，當(dāng)滿足上述三個(gè)假定條件時(shí)，有如下定理。定理存在下列三個(gè)方程：（1）系統(tǒng)的最優(yōu)線性濾波方程為連續(xù)系統(tǒng)的卡爾曼濾波（6.145）其初始值為（6.146）（2）增益方程為的解，初始條件是

。方程（6.147）常稱為方差方程。其中增益矩陣

是個(gè)

矩陣；

是濾波誤差

的方差陣，是個(gè)

矩陣；（3）方差矩陣

是矩陣微分方程（6.147）3、預(yù)備知識(shí)連續(xù)系統(tǒng)的卡爾曼濾波（6.148）在給出定理的證明之前需要說明幾個(gè)問題。（1）連續(xù)方程與離散化方程之間的關(guān)系（6.149）這就是方程（6.148）的離散化方程。兩者之間的關(guān)系式已經(jīng)由所記的變量關(guān)系表示出來。顯然，這樣的離散化是近似的，

越小，離散化程度越高?，F(xiàn)在來求它的離散化方程。為便于敘述，在以下的討論中，高階無窮小項(xiàng)就不寫出來了。根據(jù)導(dǎo)數(shù)的定義，由方程（6.148）可得

或者如果記

，

，

，

，則方程（6.149）可化為

（6.150）假設(shè)有方程觀測(cè)方程根據(jù)類似推理可以離散化為

，即

，其中

，

，

。連續(xù)系統(tǒng)的卡爾曼濾波（2）隨機(jī)過程

與隨機(jī)序列

之間的關(guān)系為了獲得對(duì)零均值白噪聲過程的直觀理解，進(jìn)而說明

與

的關(guān)系，以下介紹一下布朗運(yùn)動(dòng)，即花粉在靜止的液面上的運(yùn)動(dòng)。液體分子的運(yùn)動(dòng)是毫無規(guī)則的，花粉受到液體分子的沖擊也將作毫無規(guī)則的運(yùn)動(dòng)。為簡便起見，我們只考慮花粉沿某一固定方向的無規(guī)則運(yùn)動(dòng)。換言之，即一維的布朗運(yùn)動(dòng)。將花粉理想化一個(gè)質(zhì)點(diǎn)，設(shè)此質(zhì)點(diǎn)的初始位置在坐標(biāo)原點(diǎn)，即

，那么根據(jù)運(yùn)動(dòng)的完全無規(guī)則性，質(zhì)點(diǎn)在任一時(shí)刻的位置

是個(gè)隨機(jī)變量，而且有

。該式的含義是：質(zhì)點(diǎn)在任何時(shí)刻向左與向右運(yùn)動(dòng)的機(jī)會(huì)是均等的，因此位移的均值為零。布朗運(yùn)動(dòng)有兩個(gè)性質(zhì)與定理的證明有關(guān)：首先，容易想到，對(duì)于任何

的時(shí)刻而言，位移

和

都是均值為零的隨機(jī)變量，而且兩者是獨(dú)立的。自然有

。根據(jù)以上敘述可知，一維布朗運(yùn)動(dòng)，質(zhì)點(diǎn)在各相繼時(shí)間區(qū)間上的位移構(gòu)成一個(gè)白噪聲隨機(jī)序列。進(jìn)一步，質(zhì)點(diǎn)在各相繼時(shí)間區(qū)間上的平均速度

構(gòu)成一個(gè)白噪聲序列。極限地說，質(zhì)點(diǎn)在每一時(shí)刻t的速度就構(gòu)成一個(gè)白噪聲隨機(jī)過程，因此有

，

。其次，對(duì)于此運(yùn)動(dòng)而言，有

，其中C是一個(gè)正常數(shù)。該式的含義是，質(zhì)點(diǎn)在任何一段時(shí)間上的位移的方差必與該段時(shí)間成正比。再次，現(xiàn)在我們要以方程

為根據(jù)進(jìn)行工程化。為簡單起見，我們?nèi)〕?shù)C=1，把T這段時(shí)間上的位移的標(biāo)準(zhǔn)差（即方差的平方根）叫做T這段時(shí)間的位移，并記為

，這與電工學(xué)中常用的電壓的有效值來刻畫交變電壓是一回事。連續(xù)系統(tǒng)的卡爾曼濾波連續(xù)系統(tǒng)的卡爾曼濾波這樣，由方程

可得

或者

，這表示質(zhì)點(diǎn)在

時(shí)間內(nèi)的位移等于這段時(shí)間的平方根。例如，1秒鐘的位移為1，

秒的位移為

，

秒的位移為

等。眾所周知，若質(zhì)點(diǎn)的速度為常數(shù)，則在時(shí)間

的位移應(yīng)與

成正比，現(xiàn)與

成正比，表示質(zhì)點(diǎn)的速度是遞減的。將式子除以

可得

，該式左端的物理意義是平均速度，將它記為

，即

，

，該式表示在

這段時(shí)間內(nèi)的平均速度與這段時(shí)間的平方根成反比。由此可知，質(zhì)點(diǎn)的平均速度是由無窮大開始隨時(shí)間而遞減的。令

，則由方程

可知

，該式右端函數(shù)的定義是連續(xù)系統(tǒng)的卡爾曼濾波根據(jù)定義，它就是

函數(shù)的平方根，即

。由方程

可知，質(zhì)點(diǎn)在t=0質(zhì)點(diǎn)每次受分子沖擊的瞬間都可看作t=0）的速度是無窮大。已經(jīng)說過

所表示的平均隨機(jī)速度在各相繼的時(shí)間區(qū)間上是兩兩獨(dú)立的。若取極限情況，可以認(rèn)為質(zhì)點(diǎn)在各時(shí)刻的隨機(jī)速度是相互獨(dú)立的，從而根據(jù)方程

有

或?qū)憺?/p>

。這表明一維布朗運(yùn)動(dòng)在極限情況下的隨機(jī)速度構(gòu)成一個(gè)白噪聲隨機(jī)過程。具體說就是（a）質(zhì)點(diǎn)受分子的碰撞而運(yùn)動(dòng)，向左運(yùn)動(dòng)與向右運(yùn)動(dòng)的機(jī)會(huì)是均等的，所以其位移和速度都是隨機(jī)量，且均值為零；（b）相繼兩次碰撞以至任何兩次碰撞之間而產(chǎn)生的運(yùn)動(dòng)，即質(zhì)點(diǎn)的位移與速度是相互無關(guān)的；（c）每次碰撞一發(fā)生，質(zhì)點(diǎn)便獲得無窮大的速度，然后隨時(shí)間按

的規(guī)律而遞減，盡管速度為無窮大，但在時(shí)間內(nèi)的位移仍因而與成正比；連續(xù)系統(tǒng)的卡爾曼濾波（d）設(shè)想每兩次碰撞的間隔時(shí)間為

，這就變?yōu)橘|(zhì)點(diǎn)在任何瞬間的速度都是無窮大，可正可負(fù)，任何兩個(gè)瞬間的速度都是不相關(guān)的，即縱然這一時(shí)刻的質(zhì)點(diǎn)速度是正的，但下一瞬間質(zhì)點(diǎn)的速度可正可負(fù)，且機(jī)會(huì)是均等的。

概括而言，質(zhì)點(diǎn)在任何瞬間都已無窮大的速度運(yùn)動(dòng)且任何瞬間都在毫無規(guī)則地改變運(yùn)動(dòng)的方向，這就是白噪聲隨機(jī)過程的一個(gè)模型。實(shí)際中當(dāng)然不是這樣，這只是一個(gè)極限情況而已。根據(jù)相同推理，可以把方程

一般化為

。將以上討論推廣到多維情形就不難理解，

應(yīng)該為

。這實(shí)際就是方程（6.140）。以上所述只有工程意義，并不嚴(yán)謹(jǐn)，但是得到的結(jié)論有助于我們給出定理的證明。連續(xù)系統(tǒng)的卡爾曼濾波4、定理的證明下面我們需要根據(jù)布朗運(yùn)動(dòng)的結(jié)論，找出上述隨機(jī)噪聲的方差陣之間的關(guān)系。從方程

可以看出，如果把

理解為隨機(jī)速度的話，那么就可以理解

為隨機(jī)位移，而其中

較準(zhǔn)確的寫法應(yīng)該是

，因此有

，進(jìn)一步利用方程

和

和給定條件，可得

。因?yàn)?/p>

是連續(xù)的，略去

的高階無窮小可將其寫為

。根據(jù)同樣推理，由方程

和給定條件可得

或

。以下證明的思想是：通過離散系統(tǒng)的濾波公式取極限得到連續(xù)系統(tǒng)的濾波公式。具體作法如下所述。連續(xù)系統(tǒng)的卡爾曼濾波（1）最優(yōu)線性濾波方程已知前面小節(jié)中的濾波方程（6.121）為

，利用關(guān)系式

、

、

，可將濾波方程（6.121）改寫為稍加整理得到取極限，令上式中的

并記

，則得定理中的最優(yōu)線性濾波方程為（6.151）（2）增益方程現(xiàn)在來求極限

。由前面小節(jié)中的增益方程（6.122）有連續(xù)系統(tǒng)的卡爾曼濾波根據(jù)離散化關(guān)系式可將上式寫作（6.152）而由方程（6.123）可知，其中（6.153）再借助方程

，在方程（6.153）兩邊令

，取極限可得（6.154）利用上式結(jié)果和等式

，由方程（6.152）可得（6.155）或（6.156）這就是定理中的增益方程（6.146）。連續(xù)系統(tǒng)的卡爾曼濾波（3）方差方程現(xiàn)在考慮誤差方差陣。由方程（6.124），即

，可寫出（6.157）利用方程（6.153），并稍加整理，略去無窮小項(xiàng)，可得并令

，借助方程

、（6.154）和（6.155）可得或簡寫為連續(xù)系統(tǒng)的卡爾曼濾波這就是定理中的方差方程（6.147）。再根據(jù)給定條件，方程（6.151）和（6.157）的初始條件顯然是，定理證畢。從開始就提到過，上述證明是形式上的。為了幫助理解濾波的意義。以下給出兩個(gè)例子。本章小結(jié)6.7本章第一部分主要介紹了估計(jì)理論中涉及到的矩陣微分相關(guān)數(shù)學(xué)基礎(chǔ)。第二部分以最小二乘估計(jì)為起點(diǎn)逐步闡述了參數(shù)估計(jì)理論的相關(guān)基礎(chǔ)知識(shí)，主要給出了線性最小方差估計(jì)算法、一般最小方差估計(jì)算法、遞推最小二乘估計(jì)算法。第三部分主要介紹了信息融合方面的重要技術(shù)基礎(chǔ)知識(shí)，即卡爾曼濾波技術(shù)，其中重點(diǎn)敘述了離散系統(tǒng)的卡爾曼濾波算法，還介紹了連續(xù)系統(tǒng)的卡爾曼濾波算法等方面知識(shí)。雖然這些知識(shí)非常經(jīng)典，但可幫助讀者掌握濾波基礎(chǔ)知識(shí)，為后續(xù)信息融合算法學(xué)習(xí)打下堅(jiān)實(shí)基礎(chǔ)。100第七章多傳感器信息融合技術(shù)基礎(chǔ)目錄CONTENT123多傳感器信息融合技術(shù)概述多傳感器信息融合的概率統(tǒng)計(jì)方法多傳感器信息融合的推理學(xué)習(xí)方法456基于知識(shí)模型的多傳感器信息融合方法多傳感器智能信息融合方法多源圖像融合多傳感器信息融合技術(shù)概述7.1多傳感器信息融合能有效地提高系統(tǒng)的性能，關(guān)鍵在于該技術(shù)融合了精確的和非精確的信息，特別是在信息具有不確定性和變化未知的情況下，多傳感器融合系統(tǒng)相較于單傳感器數(shù)據(jù)處理方式具有明顯優(yōu)勢(shì)。單傳感器信號(hào)處理是對(duì)傳感器信息的一種低水平模仿，不能像多傳感器系統(tǒng)那樣有效地利用多傳感器資源。多傳感器融合系統(tǒng)可以更大程度地獲得被探測(cè)目標(biāo)和環(huán)境的信息量。多傳感器融合系統(tǒng)與經(jīng)典信號(hào)處理方法之間存在本質(zhì)區(qū)別，其關(guān)鍵在于多傳感器信息具有更復(fù)雜的形式，而且可以在不同的信息層上出現(xiàn)。這些信息抽象層次包括像素層、特征層和決策層。對(duì)應(yīng)信息抽象的三個(gè)層次，多傳感器信息融合也分為三個(gè)層次，即像素級(jí)融合、特征級(jí)融合和決策級(jí)融合。多傳感器信息融合的層次1．像素級(jí)融合像素級(jí)融合原理如圖7-1所示，是直接在原始數(shù)據(jù)層上進(jìn)行的融合，在各種傳感器的原始測(cè)報(bào)預(yù)處理之前就進(jìn)行數(shù)據(jù)綜合和分析。像素級(jí)融合是最低層次的融合，如成像傳感器