現代控制理論最優線性預測與濾波的基本方程第1頁/共192頁第一節維納濾波

12.1.1、維納濾波問題的提法12.1.2、維納-霍夫積分方程第2頁/共192頁維納濾波問題的提法設系統的觀測方程為式中，為有用信號為觀測信號為觀測誤差。設、和都是均值為零并具有各態歷經性的平穩隨機過程（附錄四）。根據觀測值估計，使估值接近于。維納濾波的任務就是設計出一個線性定常系統L，如圖12-1所示，使得系統的輸出與具有最小方差，即

(12-3)這樣就作為的估值。第3頁/共192頁根據問題的性質，維納濾波有下列三個條件：⑴信號與噪聲都是均值為零并具有各態歷經性的平穩隨機過程；⑵濾波器是一個物理可實現的線性定常系統。當時，；⑶最優準則是濾波的方差為最小。這些條件使維納濾波受到很大限制。如果系統的脈沖過渡函數為，則

(12-4)

是系統L根據輸入信號在上的全部過去值所給出的實際輸出，如圖12-2所示。是的線性函數。第4頁/共192頁維納-霍夫積分方程維納-霍夫積分方程是確定最優濾波器脈沖過渡函數的一個方程式。根據正交定理（附錄五），估計誤差應與觀測值正交，即(12-5)把上面兩式代入式(12-5)，可得第5頁/共192頁維納濾波在隨機控制領域中是一個很大的突破，但很少被應用，這主要有如下兩方面原因：⑴維納－霍夫積分方程很難解，即使求出了最優濾波器的脈沖過渡函數，在工程上往往很難實現；⑵維納理論要求所有的隨機過程都平穩的，這與工程實際問題往往不相符合。(12-6)這就是維納-霍夫積分方程，解此方程可得最優濾波器的脈沖過渡函數。把上面兩式代入式(12-5)，可得第6頁/共192頁卡爾曼在1960年提出了另一種適合于數字計算機計算的遞推濾波法，即所謂的卡爾曼濾波。這種濾波方法不需要求解積分方程，既適用于平穩隨機過程，也適用于非平穩隨機過程，是一種有廣泛應用價值的工程方法。卡爾曼維納第7頁/共192頁第二節卡爾曼濾波問題的提法

在許多實際控制過程中，系統往往受到隨機干擾作用，例如飛行中的飛機、導彈受到陣風的擾動。在這種情況下，線性連續系統的控制過程可用下式表示：

(12-7)

第8頁/共192頁式中，是控制系統的n維狀態向量，是r維控制向量，假定是均值為零的p維白噪聲向量，是n*n矩陣，是n*r矩陣，是n*p矩陣。對于實際控制系統，最優控制律或自適應控制律的形成需要系統的狀態變量，而狀態變量往往不能直接獲得，需要通過測量裝置進行觀測，根據觀測得到的信號來確定狀態變量。但測量裝置中一般都存在隨機干擾。因此在觀測得到的信號中夾雜有隨機噪聲。第9頁/共192頁式中，是m維觀測向量,是m*n矩陣，稱為觀測矩陣。假定是均值為零的m維白噪聲，和相互獨立，它們的協方差陣分別為要從夾雜有隨機噪聲的觀測信號中準確地分離出狀態變量是不可能的，只有根據觀測信號來估計這些狀態變量。通常，觀測系統的觀測方程為（12-8）（12-9）（12-10)第10頁/共192頁式中的是狄拉克(Dirac)函數，當時，；時；且。當和不隨時間而變化時，Q和R都是白噪聲的譜密度矩陣。為對稱的非負定矩陣，為正定的對稱矩陣。正定的物理意義是觀測向量的各分量都附加有隨機噪聲。和都可對t微分。的初始狀態是一個隨機向量。第11頁/共192頁

現在的任務是從觀測信號中估計出狀態變量，希望估計出來的值與實際的值愈接近愈好，因此提出最優估計問題。一般都采用線性最小方差估計。假定的數學期望，方差矩陣都為已知。第12頁/共192頁

線性最小方差估計問題可闡述如下：假定線性控制過程如式(12-7)所示，觀測方程如式(12-8)所示。從時間開始得到觀測值，在區間內已給出觀測值。要求找出的最優線性估計。這里記號“”表示利用時刻t以前的觀測值來估計時刻的值。所謂最優線性估計包含以下三點意義：⑴估值是的線性函數。第13頁/共192頁⑶要求估值誤差的方差為最小，即要求⑵估值是無偏的，即第14頁/共192頁

比較起來預測問題稍為簡單一些，平滑問題最復雜。通常講的卡爾曼濾波指的是預測和濾波。第一類：>稱為預測（或外推）問題根據和的大小關系，連續系統估計問題可分為三類：第二類：=，稱為濾波問題；第三類：<稱為平滑（或內插）問題。第15頁/共192頁設離散系統的差分方程和觀測方程分別為（12-11）（12-12）式中，是n維狀態向量，是r維控制向量，是m維觀測向量，是n*n轉移矩陣，是n*r矩陣，是n*p矩陣，是m*n矩陣。下面討論離散系統的卡爾曼濾波問題。第16頁/共192頁（12-13）式中，為克羅尼克(Kroneker)函數，其特性是

假定是均值為零的p維白噪聲向量序列，是均值為零的m維的白噪聲向量序列，和相互獨立，在采樣間隔內和都為常值，其統計特性如下：第17頁/共192頁（1）(12-14)（2）(12-15)

為非負定矩陣，為正定矩陣。和都是方差陣，而和不是方差陣。當和不隨時間而變時，都是譜密度矩陣。可以證明，、與、之間存在下列關系：第18頁/共192頁

在的極限條件下，離散噪聲序列和趨向于持續時間為零、幅值為無窮大的脈沖序列。而“脈沖”自相關函數與橫軸所圍的面積和分別等于連續白噪聲脈沖自相關函數與橫軸所圍的面積和。第19頁/共192頁

也就是要求各狀態變量估計誤差的方差為最小。同時要求是的線性函數，并且估計是無偏的，即根據j和k的大小關系，離散系統估計問題也可分成三類；第一類：j>k，稱為預測（或外推）問題；第二類：j=k，稱為濾波問題；第三類：j<k稱為平滑（或內插）問題。本章只討論連續系統和離散系統的最優預測和最優濾波問題。第20頁/共192頁第三節離散系統卡爾曼最優預測基本方程的推導

在推導卡爾曼預測基本方程時，為了簡便起見，先不考慮控制信號的作用，這樣，離散系統的差分方程(12-11)變成(12-16)觀測方程仍為式(12-12)：式中，和都均值為零的白噪聲序列，和相互獨立第21頁/共192頁在采樣間隔內和為常值，其統計特性如式(12-13)所示，即狀態向量的初值，其統計特性是給定的，即第22頁/共192頁要求估值是的線性函數，并且要求估計是無偏的，即給出觀測序列，要求找出的線性最優估計，使得估計誤差的方差為最小，即第23頁/共192頁

下面推導卡爾曼預測基本公式。推導的方法有幾種，比較簡易的方法是利用正交定理，用數學歸納法推導卡爾曼估計的基本遞推估計公式。

當獲得觀測值之后，假定已經得到狀態向量的一個最優線性預測估計。當還未獲得時刻的新觀測值時，根據已有的觀測值，可得時刻的系統狀態向量的兩步預測估值：(12-17)第24頁/共192頁

由于是的一步最優線性估計，也是的最優線性預測估計，這可用正交定理來證明。由式(12-16)減式(12-17)，可得(12-18)式中，第25頁/共192頁

因為是的最優線性預測估值，根據正交定理，估計誤差必須正交于，所以的線性變換也必須正交于。式(12-18)中的是均值為零的白噪聲序列，與相互獨立，因此正交于。所以在沒有獲得之前，是的最優兩步線性預測。

在新觀測值獲取之后，可通過修正兩步估值來得到的一步預測估值。第26頁/共192頁設的預測估值由式(12-12)減式(12-19)，得的預測估計誤差為(12-19)第27頁/共192頁

造成的原因有兩個：⑴對時刻狀態向量的預測估計有誤差；⑵附加了時刻的觀測噪聲干擾。顯然包含修正的新信息。這樣，在獲得之后，在兩步估值的基礎上，用去修正，便可得到時刻狀態向量的一步預測估計，即或(12-20)式中是待定矩陣，稱為最優增益矩陣或加權矩陣。第28頁/共192頁式(12-20)可改寫成

時刻系統狀態方程為由式(12-22)減式(12-21)得的估計誤差為(12-21)(12-22)(12-23)第29頁/共192頁

觀察式(12-23)右邊，、、均分別正交于，因此，正交于。

若與正交，則就是的一步最優線性預測估值。因此，利用與的正交條件：來確定最優增益矩陣。(12-24)第30頁/共192頁把式(12-23)和式(12-12)代入式(12-24)，得考慮到、、和相互間都是正交的，因此上式可簡化為即第31頁/共192頁設，又有，代入上式后可得最優增益矩陣為(12-25)現在需確定估計誤差方差陣的遞推關系式。根據估計誤差方差陣的定義，有第32頁/共192頁將式(12-23)代入上式得考慮到、和相互間都正交，可得：式中第33頁/共192頁將上式展開，經整理后得上式右邊第四項與第五項之和為(12-26)第34頁/共192頁

顯然，式(12-26)右邊第四項與第五項之和在數值上等于第三項，但符號相反。這樣，式(12-25)右邊第三、第四和第五項之和為零，所以把式(12-25)的代入上式，可得估計誤差方差陣的遞推關系式為(12-27)第35頁/共192頁方程(12-20)、(12-25)和(12-27)構成一組完整的最優線性估計方程，現綜合如下：⑴最優預測估計方程為式(12-20)，即⑵最優增益矩陣方程為(12-25)，即第36頁/共192頁⑶估計誤差方差陣的遞推方程為式(12-27)，即從式(12-27)可看出，估計誤差方差陣與有關，而與觀測值無關。因此，可事先估計誤差方差陣，同時也可算出最優增益矩陣。第37頁/共192頁按式(12-16)和式(12-12)作出系統模型方塊圖，如圖12-3所示。第38頁/共192頁圖12-4表示由方程(12-21)所描述的卡爾曼最優預測估計方塊圖。第39頁/共192頁

從圖12-4可看出，最優預測估計由三部分組成：⑴估值為觀測值的線性函數；⑵最優增益矩；⑶單位負反饋回路。

現在需要驗證第二節時最優估計提出的三項要求：⑴估值為觀測值的線性函數；⑵估計值的方差為最小；⑶估值是無偏的，即第40頁/共192頁

在上面推導預測估計方程時，是按照⑴和⑵兩項要求推導的，現需要說明估值是無偏的問題。對式(12-16)的兩端取數學期望，考慮到，可得再對式(12-21)的兩端取數學期望，考慮到，可得(12-28)(12-29)第41頁/共192頁將式(12-28)減式(12-29)，得到如果初始條件為或則(12-30)第42頁/共192頁根據式(12-30)的遞推關系，可得因此

所以，只要初始估計選為，所得估計是無偏的。第43頁/共192頁卡爾曼預測估計遞推方程的計算步驟如下：⑴在時刻給定初值：估值誤差方差陣初值：⑵根據公式(12-25)計算時刻最優增益陣：第44頁/共192頁⑶根據公式(12-20)計算的最優估值：⑷根據公式(12-27)計算：⑸根據已知的計算時刻的。⑹根據計算的估值。重復上述遞推計算步驟，可得第45頁/共192頁例12-1設系統狀態方程和觀測方程為

和都是均值為零的白噪聲序列，且不相關，其統計特性如下：初值觀測值試求的最優預測估值。第46頁/共192頁解:

與前面的有關方程對照，可得最優預測估值方程為最優增益矩陣為估值誤差方差陣遞推方程為第47頁/共192頁下面計算：第48頁/共192頁取的初值第49頁/共192頁第四節離散系統卡爾曼最優濾波基本方程的推導

系統狀態方程、觀測方程和噪聲特性如式(12-16)、式(12-12)和式(12-13)所示。最優濾波問題簡述如下：給出觀測序列，要求找出的最優線性估計，使得估計誤差的方差為最小，并且要求估計是無偏的。采用與上節類似的推導方法――數學歸納法和正交定理，導出最優濾波估計方程，即離散系統卡爾曼濾波方程。第50頁/共192頁推導的具體步驟如下：⑴假定由觀測值估計得到狀態向量的一步最優預測估值和觀測向量的預測估值為:⑵當獲得之后，求得與的誤差，即⑶以去修正，得到的濾波估值為或(12-31)式中為待定的最優增益矩陣。第51頁/共192頁⑷求估計誤差。式(12-31)可改寫成則濾波估計誤差為第52頁/共192頁⑸利用與正交求。

由于是的最優估值，的估計誤差必須正交于。即

把和的表示式代入上式，并考慮到，，之間正交，可得第53頁/共192頁由上式直接得到最優增益矩陣：(12-32)第54頁/共192頁⑹按照估值誤差方差陣定義推導的遞推計算公式：整理并簡化上式，可得濾波估值誤差方差陣計算公式如下：(12-33)第55頁/共192頁⑺為了得到與之間的遞推關系，式(12-31)中的可由下式計算得到：(12-34)⑻為了得到與的遞推關系式，應將式(12-33)中的表示成的關系式。由式(12-34)求得的最優預測估值誤差為第56頁/共192頁而于是(11_35)第57頁/共192頁

綜上所述，方程(12-31)、(12-32)、(12-33)、(12-34)和(12-35)構成卡爾曼最優濾波基本方程組，與綜合如下：第58頁/共192頁若給定初始統計特性及，要得到無偏估計，應取初值離散系統卡爾曼最優濾波的方塊圖如圖12-5所示。第59頁/共192頁

從式(12-32)可分析和對的影響。當增大時，觀測噪聲大，觀測值可靠度低，于是加權陣應取得小一些，以減弱觀測噪聲的影響。所以式(12-32)中隨的增大而減小。當增大時，意味著第步轉移的隨機誤差大，對狀態預測修正應加強，于是應增大。從式(12-35)可知，當增大時，增大，也增大，表示對狀態預測修正加強。第60頁/共192頁

在討論卡爾曼濾波的特殊問題時，常需用到和的另一些表達式。根據式(12-32)可得以下公式：(12-36)(12-37)(12-38)(12-39)第61頁/共192頁根據式(12-33)可得(12-40)(12-41)將式(12-38)代入(12-40)得的另一表達式：(12-42)式(12-42)在形式上雖比式(12-41)簡單，但當計算過程具有舍入誤差時，容易失去對稱性和非負定性，而式(12-41)具有較強的保持對稱性和非負定性的能力。第62頁/共192頁

將式(12-42)展開，應用矩陣求逆引理（附錄二），又可得的另一種形式：再由式(12-39)和式(12-42)可得(12-43)(12-44)(12-45)需要指出，從式(12-36)至式(12-45)都是對濾波情況而言。第63頁/共192頁

必須指出，在實際應用卡爾曼濾波算法時，每一步都要求和是對稱的。雖然式(12-36)和式(12-35)在理論上是對稱的，但是在運算過程中，有限字長和舍入誤差可能引起和不對稱，從而導致濾波系統的性能嚴重下降，甚至導致不穩定。這種情況當比較小時尤其明顯。第64頁/共192頁例12-2設二階系統模型和標量觀測模型為輸入噪聲是平穩的，，測量噪聲是非平穩的，

。換句話說，為偶數時的噪聲比為奇數時的噪聲大。假定初始狀態的方差陣，欲計算。第65頁/共192頁解:

運用方程式(12-35)、(12-32)和初始條件可算得再由式(12-42)計算出驗后方差陣為利用式(12-35)可計算下一步的驗前方差陣為第66頁/共192頁然后再計算等。直到計算到所要求的為止。讀者可按照上述方法繼續計算下去，從計算結果可以看出，當為奇數時，由于測量噪聲較小，所以較大；當為偶數時，則較小。在以后，增益就近似地達到周期性的穩態解。第67頁/共192頁第五節連續系統卡爾曼濾波基本方程的推導

在推導連續系統卡爾曼濾波基本方程時，也先不考慮控制信號的作用，這樣系統的狀態方程為式中，為n維狀態向量，為系統噪聲，其均值為零，為矩陣，為矩陣。(12-46)第68頁/共192頁觀測方程為(12-47)式中，為維觀測值向量，為矩陣，為維白噪聲，其均值為零。假設和相互獨立，它們的協方差陣為式中各符號的意義如本章第二節所述。第69頁/共192頁初始狀態向量是一個隨機向量，其均值和方差陣分別為在區間內已給出觀測值，要求找出的最優線性估計，使得估計誤差方差陣的跡為最小，即估計誤差各分量方差之和為最小：第70頁/共192頁

要求估值是觀測值的線性函數，并且要求估計是無偏的，即這里，只討論預測和濾波問題，即。第71頁/共192頁

關于連續系統卡爾曼濾波公式的推導，采用卡爾曼在1962年提出的方法：離散系統的采樣間隔，取離散型卡爾曼濾波方程的極限，連續型卡爾曼濾波方程，在離散型卡爾曼預測估計方程式(12-20)中，令采樣間隔為，，則式(12-20)變為(12-48)由式(12-25)有第72頁/共192頁考慮到則(12-49)轉移矩陣可用下面近似式表示(12-50)第73頁/共192頁把式(12-49)和式(12-50)代入到(12-48)，得到即第74頁/共192頁令，考慮到，對上式等號兩邊取極限，則得到連續系統的最優濾波方程：式中(12-51)(12-52)為最優增益矩陣，這里必須正定。第75頁/共192頁

根據估計誤差方差陣的遞推方程(12-27)，用同樣的方法，并考慮到可得第76頁/共192頁考慮到則第77頁/共192頁即令，對上式等號兩邊取極限可得連續系統誤差方差職的矩陣黎卡提微分方程：其初始條件為，解此方程可得。(12-53)第78頁/共192頁

估計誤差方差的矩陣微分方程與和的二階矩和有關，而與觀測值無關。因此可事先算出方差陣，同時也可算出。

方程(12-51)、(12-52)和(12-53)構成一組連續系統的卡爾曼濾波方程。第79頁/共192頁下面討論估計是否無偏的問題。如果初始條件選成則式(12-51)給出的估值是無偏的。由式(12-46)得由式(12-51)得第80頁/共192頁從方程(12-47)可得所以用減上式，可得第81頁/共192頁上式的解為

是轉移矩陣，它是下列微分方程的解：

如果，就有。這樣，估計是無偏的。第82頁/共192頁

根據式(12-46)和式(12-47)，可作出系統模型方塊圖，如圖12-6所示。第83頁/共192頁

根據式(12-51)可作出連續系統卡爾曼濾波方塊圖，如圖12-7所示。

從圖12-7可看出：連續系統的卡爾曼濾波器是由三部分組成:⑴系統模型；⑵最優增益矩陣；⑶單位負反饋回路。第84頁/共192頁例12-3設系統狀態方程和觀測方程如下：

和都是均值為零的白噪聲，其統計特性如下：設。試設計卡爾曼濾波器。第85頁/共192頁解與系統方程相比較，可得濾波方程為最優增益矩陣為估計誤差方差陣為第86頁/共192頁令，則用分離變量法，可得第87頁/共192頁對上式積分，可得當時，代入上式可得，則從上式求得當比較大時，趨近于穩態值1。第88頁/共192頁最優增益系數為當比較大時，趨近于穩態值。

穩態值值也可以按下列方式求得，當較大時，則黎卡提微分方程轉變為黎卡提代數方程：第89頁/共192頁因估計誤差方差必為正值，故穩態的值為1。卡爾曼濾波方塊圖如圖12-8所示。第90頁/共192頁

和隨時間的變化曲線如圖12-9所示。從該圖可看出，即使是定常系統，最優增益系數K也是變系數。當時間不斷增大，K趨近于某一穩態值。和的初始段與的初值()有很大關系，但和的穩態值與沒有關系。第91頁/共192頁

當濾波達到穩態過程后，由于趨于某一常值，作為濾波器的輸出，為濾波器的輸入，則可得輸出與輸入之間的傳遞函數由傳遞函數可知，本例的卡爾曼濾波器為一慣性環節。第92頁/共192頁第六節系統噪聲與觀測噪聲相關的卡爾曼濾波離散系統連續系統第93頁/共192頁一離散系統

系統的狀態方程和觀測方程與式(12-16)和式(12-12)完全相同，即

和都均值為零的白噪聲序列，兩者相關，則在和相關的情況下，最優估計方程的推導與前面所述的推導方法和步驟基本相同。第94頁/共192頁下面不加推導，直接寫出結論。最優線性估計的卡爾曼預測方程為(12-54)(12-55)(12-56)第95頁/共192頁

將式(12-54)、(12-55)、(12-56)與第三節中的式(12-20)、(12-25)、(12-27)相比較，可得以下結論：⑴與相關情況下的最優預測估計方程(12-54)與兩者不相關情況下的最優預測估計方程(12-20)完全相同的。⑵和的相關性只影響最優增益矩陣和估計誤差方差陣。很明顯，若，式(12-55)和(12-56)分別變成式(12-25)和式(12-27)。第96頁/共192頁二連續系統系統的狀態方程和觀測方程與式(12-46)和式(12-47)完全相同，即

和是均值為零的白噪聲，且兩者相關，則第97頁/共192頁

采用第五節所用的推導方法，可得和相關情況下的連續系統最優濾波方程、最優增益矩陣、估計誤差方差陣。(12-57)(12-58 )(12-59)第98頁/共192頁

將式(12-57)、(12-58)、(12-59)與式(12-51)、(12-52)、(12-53)相比較，可得以下兩點結論：⑴與相關情況下的最優濾波方程(12-57)和兩者不相關情況下的最優濾波方程(12-51)完全相同，這說明與兩者的相關性不影響估值方程。⑵和的相關性只影響計算公式，若則式(12-58)和(12-59)分別變成式(12-52)和式(12-53)。第99頁/共192頁

在上面討論卡爾曼濾波基本方程時，為了簡便起見，假定作用于系統的控制信號等于零。實際上，系統總會受到控制信號的作用。下面討論具有輸入控制信號的卡爾曼濾波方程。第七節具有輸入信號的卡爾曼濾波一離散系統二連續系統第100頁/共192頁一離散系統系統的狀態方程和測量方程為

式(12-60)中的為已知的非隨機控制序列，式(12-61)中的為觀測系統的系統誤差項，也是已知的非隨機序列。在采樣間隔內，和都是常值。（12-60）（12-61）第101頁/共192頁

和的統計特性如下：

采用與前面相同的推導方法，可得到具有輸入控制信號的卡爾曼濾波方程。下面不加推導地直接寫出具有輸入控制信號的最優預測的卡爾曼濾波方程。第102頁/共192頁（12-62）（12-63）（12-64）第103頁/共192頁將式(12-62)、(12-63)、(12-64)與式(12-54)、(12-55)、(12-56)相比較，可得以下兩點結論：⑴當系統具有輸入控制信號和測量系統誤差時，它們只對的估值方程有影響，而對最優增益矩陣和估計誤差方差陣的計算公式無任何影響。⑵和的相關性不影響的估值方程，只影響和的計算公式。因此式(12-63)與式(12-55)完全相同，式(12-64)與式(12-56)完全相同。第104頁/共192頁

具有輸入控制信號的離散系統方塊圖和卡爾曼預測方塊圖如圖12-10和圖12-11所示。

具有輸入控制信號的離散系統，其系統噪聲和測量噪聲均是白噪聲，且兩者相關。對于這樣的系統，前面已討論過直接考慮與相關的卡爾曼濾波方程的推導。這里通過工程實例，再介紹另一種將噪聲相關問題轉化為不相關問題處理的卡爾曼濾波方程的推導。第105頁/共192頁

例12-4在進行導彈容錯控制設計時，需要利用導彈俯仰加速度回路和狀態信息進行故障診斷，以判斷回路中的加速度計是否發生故障。設某地空導彈俯仰通道加速度回路的數學模型為其中，狀態變量，與彈體俯仰角速度率成比例，與彈體俯仰加速度率成比例，與彈體俯仰加速度的導數成比例。控制變量俯仰制導指令。輸出變量為加速度計測得的彈體加速度。試設計卡爾曼濾波器，估計出狀態變量。(12-65)(12-66)第106頁/共192頁

要求仿真計算1000步，初始條件：是周期為400步、幅值為1的方波信號。導彈俯仰通道加速度回路數學模型中的系數矩陣為第107頁/共192頁

和均為白噪聲序列，且兩者相關。其統計特性如下：第108頁/共192頁解⑴濾波方程的推導由式(12-66)可得將狀態方程(12-65)變為下式：或其中為待定矩陣(12-67)第109頁/共192頁設將和代入式(12-67)，得新狀態方程：式(12-68)中可視為新的控制項，可視為新的系統噪聲，仍是白噪聲。(12-68)第110頁/共192頁當，與不相關，必須即(12-69)第111頁/共192頁由此求得待定矩陣。將式(12-69)代入式(12-68)，可得

式(12-70)中，視為控制項。由式(12-70)和式(12-66)構成系統數學模型，直接利用前面系統噪聲和測量噪聲不相關的卡爾曼濾波基本方程組，即式(12-31)至式(12-35)，并考慮具有輸入控制信號的影響，可得到具有輸入控制信號，和相關情況下的卡爾曼濾波方程組。(12-70)第112頁/共192頁由式(12-31)寫出由式(12-34)并考慮輸入作用的影響，寫出與的轉移關系式，即(12-72)(12-71)第113頁/共192頁由式(12-32)和式(12-33)寫出根據的定義，寫出而(12-73)(12-74)第114頁/共192頁則即(12-75)第115頁/共192頁

至此，我們推導了具有輸入控制和噪聲相關情況的卡爾曼濾波方程組，下面就以此方程組計算例題的狀態估計問題。⑵狀態最優估值計算與數學仿真計算步驟：①令，組定狀態最優估值初值估計誤差方差陣初值。在本例計算中，選取=0，=diag[106，106，106]。②利用式(12-75)計算預測誤差方差陣。③利用式(12-73)計算增益矩陣。第116頁/共192頁④利用式(12-72)計算的最優預測估值。⑤利用式(12-71)計算的最優濾波估值。⑥利用式(12-74)計算估計誤差方差陣。⑦令增1，返回第2步，重復上述計算步驟。數字仿真流程圖如圖12-12所示。第117頁/共192頁⑶計算結果如圖12-13、圖12-14、圖12-15所示。圖中表示出輸入控制信號為方波情況下的狀態、和及其估值、和。圖12－13第118頁/共192頁圖12－14圖12－15第119頁/共192頁二、連續系統具有輸入控制信號的連續系統狀態方程和觀測方程如下：式中，為已知的非隨機函數，為觀測系統的系統誤差項，也是已知的非隨機函數，和都是均值為零的白噪聲，并且兩者是相關的。和的統計特性如下：(12-76)(12-77)第120頁/共192頁

在式(12-62)、式(12-63)和式(12-64)中，令采樣間隔

,并考慮到

然后求極限，可得具有輸入控制信號的連續系統的卡爾曼濾波方程：(12-78)第121頁/共192頁最優增益矩陣為估計誤差方差陣為(12-79)(12-80)第122頁/共192頁將式(12-78)、式(12-79)、式(12-80)與式(12-57)、式(12-58)、式(12-59)相比較，可得以下兩點結論：⑴當系統具有輸入控制信號和測量系統誤差時，它們只對的估值方程有影響，而對最優增益矩陣方程和估計方差陣的計算公式無任何影響。⑵和的相關性不影響的估值方程，只影響和的計算公式。因此式(12-79)與式(12-58)相同，式(12-80)與式(12-59)相同。第123頁/共192頁具有輸入控制信號的連續系統方塊圖和卡爾曼濾波方塊圖如圖12-16和圖12－17所示。圖12－16圖12－17第124頁/共192頁第八節有色噪聲情況下的卡爾曼濾波

前面推導卡爾曼濾波方程時，假定和都是白噪聲。實際上，和可能是有色噪聲。所謂白噪聲，就是不同時刻的噪聲都是互不相關的，而有色噪聲是在不同時刻的噪聲都是相關的。某些特定的有色噪聲可用白噪聲通過成形濾波器來表示。這樣就可直接應用上面的濾波方程。下面將先介紹成形濾波器，然后再通過實例說明如何把某些特定的有色噪聲用成形濾波器來處理的問題。第125頁/共192頁一連續系統的成形濾波器設是一平穩隨機過程，其相關函數為式中，為常數，為時間間隔。將噪聲的相關函數進行富氏變換，可得噪聲的頻譜密度為第126頁/共192頁可以將分解成由上式可直接寫出相應的傳遞函數：第127頁/共192頁

顯然，隨機過程相當于單位強度白噪聲（譜密度的白噪聲稱為單位強度白噪聲）長時間作用于傳遞函數為的線性系統的結果。該系統是一個慣性環節，其時間常數為，放大系數為，可用圖12-18表示。第128頁/共192頁

把單位強度白噪聲轉變為有色噪聲的線性系統稱為成形濾波器，上述的慣性環節即為成形濾波器。如果把上述成形濾波器的特性用微分方程表示，則有若令則成形濾波器方程為第129頁/共192頁式中為白噪聲，且有

因此，對于某些特定的有色噪聲，可用單位強度白噪聲通過成形濾波器來表示。通過以上分析可知，如果有色噪聲的相關函數是指數函數，則在譜密度中的最高次項為，則成形濾波器用一階微分方程表示。第130頁/共192頁對于多維的有色噪聲，如其相關函數為指數函數，則其成形濾波器方程可用一階向量微分方程表示：式中，為多維有色噪聲、為的系數矩陣，為均值等于零的白噪聲過程，其統計特性為第131頁/共192頁

對于一般的平穩隨機過程，如果譜密度為的有理函數，且能分解成

式中和是的多項式。和的根都在上半復數平面內，即和的根在復平面的左半部。則隨機過程相當于單位強度白噪聲長時間作用于頻率特性為的線性系統的結果，則成形濾波器的傳遞函數為第132頁/共192頁二離散系統的成形濾波器設為一平穩隨機序列，已知其相關函數為這里不加推導地寫出成形濾波器方程如下：式中，為成形濾波器的轉移矩陣：第133頁/共192頁

為均值不零的白噪聲序列，其統計特性為第134頁/共192頁三系統中附加噪聲的幾種情況

上面討論了控制系統和觀測系統含有白噪聲的卡爾曼濾波。下面討論系統的附加噪聲為有色噪聲的卡爾曼濾波。有色噪聲的卡爾曼濾波有三種情況：⑴控制系統附加噪聲為有色噪聲，觀測系統噪聲為白噪聲；⑵控制附加噪聲為白噪聲，觀測系統噪聲為有色噪聲；⑶控制系統和觀測系統的附加噪聲均為有色噪聲。這里只討論情況⑴和⑵和卡爾曼濾波。關于情況⑶的卡爾曼濾波，讀者可把情況⑴和⑵的推導方法組合起來，進行情況⑶的卡爾曼濾波方程的推導，故在此不作討論。第135頁/共192頁

四

控制系統含有有色噪聲，觀測系統含白噪聲情況的卡爾曼濾波問題1.連續系統系統狀態方程和觀測方程如下：式中，是均值為零的有色噪聲，是均值為零的白噪聲。可用下列成形濾波器方程表示：(12-81)(12-82)(12-83)第136頁/共192頁設為擴大維數后的狀態向量，即設為擴大狀態變量維數后的系統附加噪聲，即顯然，中只包含白噪聲。第137頁/共192頁擴大狀態變量維數后的系統狀態方程和觀測系統方程為或式中(12-84)第138頁/共192頁或式中

式(12-84)和式(12-85)分別為擴大狀態變量維數后的控制系統方程和觀測系統方程。由于只包含白噪聲，為白噪聲，故可直接應用前面推導出的卡爾曼濾波方程。(12-85)第139頁/共192頁2.離散系統離散系統狀態方程和觀測系統方程如下：

式中，為有色白噪聲序列，是均值為零的白噪聲序列。可用下述成形濾波器方程來表示：式中為白噪聲序列。(12-86)(12-87)(12-88)第140頁/共192頁

把作為狀態變量的一部分，設為擴大維數后的狀態變量，即設為擴大狀態變量維數后的系統附加噪聲，即顯然，在中只包含白噪聲。第141頁/共192頁擴大狀態變量維數后的系統狀態方程和觀測系統方程為或式中(12-89)第142頁/共192頁或式中

式(12-89)和式(12-90)分別為擴大狀態變量維數后的控制系統方程和觀測系統方程。由于只包含白噪聲，為白噪聲，故可直接應用前面推導的離散系統卡爾曼濾波基本方程。(12-90)第143頁/共192頁

例12-5在進行導彈容錯控制系統設計時，需要利用導彈俯仰通道阻尼回路的狀態信息進行故障診斷，用來判斷回路中速率陀螺儀是否發生故障。設某地空導彈俯仰通道阻尼回路的數學模型為其中，狀態變量為與彈體俯仰角速率成比例的量，為與彈體俯仰角速率成比例的量。控制變量為俯仰制導指令，輸出變量為由速率陀螺測得的彈體俯仰角速率。試設計卡爾曼濾波器，估計出狀態變量。（注：與不相關）(12-91)(12-92)第144頁/共192頁要求仿真計算1000步，初始條件：，是周期為400步、幅值為1的方波信號。導彈俯仰通道阻尼回路數學模型中的轉移矩陣為其他系數矩陣為第145頁/共192頁為有色噪聲序列，其自相關函數為是均值為零的白噪聲序列，其統計特性為第146頁/共192頁解⑴成形濾波器設計由自相關函數的形式，直接寫出成形濾波器如下：

式中，，取采樣周期秒，因此取。為白噪聲，其統計特性為(12-93)第147頁/共192頁⑵構造擴大狀態變量維數的狀態方程和觀測方程擴展狀態變量擴展狀態方程和觀測方程為(12-94)(12-95)第148頁/共192頁式中

這樣，我們得到了系統噪聲與觀測噪聲均為白噪聲序列、且互不相關的擴大狀態變量維數的擴展系統，針對此擴展系統可直接利用前面所推導出的卡爾曼濾波方程。第149頁/共192頁⑶卡爾曼濾波方程組(12-96)(12-97)(12-98)(12-99)(12-100)第150頁/共192頁⑷計算步驟1.令，給定狀態最優估值初值和估值誤差方差陣初值P(0/0)。在本例計算中，選取。2.利用式(12-100)計算預測誤差方差陣。3.利用式(12-98)計算最優增益矩陣。4.利用式(12-97)計算擴展狀態的最優預測值。5.利用式(12-96)計算擴展狀態的最優預測值6.利用式(12-99)計算擴展狀態。7.令增1，返回第2步，重復上述計算步驟。第151頁/共192頁⑸數學仿真仿真程序流程圖如圖12-19所示。計算結果如圖12-20和圖12-21所示。第152頁/共192頁五、控制系統含白噪聲、觀測系統含有色噪聲情況的卡爾曼濾波問題1.連續系統系統狀態方程為觀測方程為式中，是均值為零的白噪聲，其統計特性為(12-101)(12-102)第153頁/共192頁

是均值為零的正態分布有色噪聲，可用成形濾皮器方程表示如下：式中，是均值為零的白噪聲，其統計特性為

與互不相關，即。(12-103)第154頁/共192頁

現設法改變觀測方程(12-102)，使等效觀測方程的附加噪聲為白噪聲。考察方程(12-103)可看出是白噪聲，從這一點出發，引入一個等效觀測值則也是白噪聲。(12-106)(12-105)第155頁/共192頁依據式(12-102)和式(12-103)，可將式(12-105)變為式中(12-106)第156頁/共192頁

這樣，把觀測方程(12-102)轉變成等效觀測方程(12-106)。式(12-106)的附加噪聲為白噪聲，因與都是均值為零的白噪聲，則也是均值為零的白噪聲。考慮到與的不相關性，的統計特性如下：式中第157頁/共192頁由于中包含，因此和是相關的，故式中由于等效觀測方程(12-106)的附加噪聲為白噪聲，因此完全可用前面推導出的系統噪聲和觀測噪聲相關的連續系統的卡爾曼濾波公式(12-57)、式(12-58)和式(12-59)。第158頁/共192頁根據式(12-57)最優濾波方程為把和代入上式，可得有色噪聲情況的最優濾波方程為(12-107)第159頁/共192頁根據式(12-58)，最優增益矩陣為

把和以及代入上式，可得有色噪聲情況的最優增益矩陣為(12-108)第160頁/共192頁根據式(12-59)，估計誤差方差陣為把和及代入上式，可得有色噪聲情況的估計誤差方差陣方程(12-109)第161頁/共192頁

從式(12-108)可看出，為了使濾波的存在，要求矩陣為正定矩陣。2.離散系統系統狀態方差為式中，是均值為零的白噪聲序列，其統計特性為(12-110)第162頁/共192頁觀測方程為式中是均值為零的正態分布有色噪聲序列，可用成形濾波器方程表示如下：式中是均值為零的白噪聲序列，其統計特性：(12-112)(12-111)第163頁/共192頁下面采用與處理連續系統的方法相同，設法改變觀測方程，使等效觀測方程的附加噪聲為白噪聲，然后直接應用前面已推導出的卡爾曼濾波方程。把式(12-112)代入式(12-111)，得再把式(12-111)改寫為以乘上式兩邊，得(12-114)(12-113)第164頁/共192頁

將式(12-113)減式(12-114)，并令，則把式(12-110)代入上式可得式(12-115)為等效觀測方程，是等效觀測值。(12-115)第165頁/共192頁等效觀測方程與原始觀測方程相比較，有兩個特點：

第一，等效觀測值只包含白噪聲，它與系統附加噪聲是相關的；

第二，在形式上看作時刻的觀測值，看起來是線性函數，實際上卻是的線性函數。

因此，針對系統狀態方程(12-110)和等效觀測方程(12-115)的濾波方程的估值和估計誤差方差陣，就是針對式(12-110)和式(12-111)的估值及。第166頁/共192頁

由于等效觀測方程的白噪聲與系統附加噪聲相關，因此可用濾波方程式(12-20)、式(12-25)和式(12-27)來處理。將式(12-115)與式(12-12)相比較可知：(12-117)(12-118)(12-119)第167頁/共192頁另外，考慮到等效觀測方程(12-115)的估值及是原觀測方程(12-111)的對應估值及。因為在中包含觀測值的信息，這樣可得有色噪聲情況下的離散系統濾波方程如下：(12-119)(12-120)(12-121)第168頁/共192頁式(12-119)中式(12-120)和式(12-121)中的，分別由式(12-116)、式(12-117)和式(12-118)表示。第169頁/共192頁

應當考慮初始值對估值的影響問題。把的初值對估值的影響歸結為的初始估值對的影響。若第一次測量時間，驗前估算為及，并且。根據線性最小方差估計，和的初值為最優濾波方塊圖如圖12-22所示。第170頁/共192頁以上用改變觀測方程的方法，避免了擴大狀態變量維數，這種方法在控制和導航等方面都可能應用。第171頁/共192頁第九節濾波的穩定性概念和濾波的發散問題一、濾波的穩定性概念二、濾波的發散問題第172頁/共192頁一、濾波的穩定性概念

前面比較詳細地討論了線性系統卡爾曼濾波基本方程的推導和遞推計算步驟。在估值計算時，需要利用一連串的觀測數據，按照濾波基本方程進步遞推計算，可得狀態向量的最優估值。這里，需要確切知道的初值和估計誤差方差陣的初值。但在許多實際問題中，往往不可能確切知道初值和，甚至根本不知道這些初值。為了進步濾波計算，只能假定初值和。這對濾波結果會造成什么影響呢？第173頁/共192頁

如果用正確的初值和，按濾波方程可得最優的濾波值及；如果選取不確切的初值和，可得非最優的和，在時間充分長之后，如果能收斂于，能收斂于，則初值不確切的影響可忽略不計。這種情況下，濾波是穩定的。反之，濾波是不穩定的。濾波是否穩定，與濾波系統的