基于矩陣分解的卡爾曼濾波技術分析及應用

1、基于矩陣分解的卡爾曼濾波技術分析及應用 【摘要】本文簡要介紹了卡爾曼濾波研究的發(fā)展歷程,重點對卡爾曼濾波及其在改善數(shù)值穩(wěn)定性，提高計算效率等數(shù)值方面的研究與發(fā)展進行了綜述，對Q-R分解，U-D分解，奇異值分解（SVD）等在卡爾曼濾波的應用進行了介紹。最后給出了一種基于Q-R矩陣分解的自適應濾波方法，仿真驗證了其有效性。1 引言1960年，美籍科學家卡爾曼(R. E. Kalman)在系統(tǒng)狀態(tài)空間模型的基礎上提出了著名的線性卡爾曼濾波器，它在線性的前提假設下是一個線性無偏、最小方差估計器，從而可以為線性濾波問題提供精確解析解。自該技術被提出以來，它已成為控制、信號處理與通信等領域最基本最重要的計

2、算方法和工具之一,并已成功地應用到航空、航天、電力系統(tǒng)及社會經(jīng)濟等不同領域。隨著微型計算機的普及應用,對卡爾曼濾波的數(shù)值穩(wěn)定性、計算效率、實用性和有效性的要求越來越高.為此,人們在如何改善卡爾曼濾波的計算復雜性和數(shù)值穩(wěn)定性方面作了大量的探索工作,各種基于平方根濾波與平滑,U-D分解濾波與平滑,奇異值分解濾波與平滑,狀態(tài)與偏差分離濾波以及并行與分散濾波等方法得到不斷發(fā)展.本文給出了矩陣分解的一些基礎知識，并著重從卡爾曼濾波數(shù)值計算方法入手,對現(xiàn)有的常規(guī)卡爾曼濾波、基于矩陣的因式分解濾波的數(shù)值計算方法進行了較系統(tǒng)的介紹和分析,并在第四章給出了一種基于Q-R矩陣分解的自適應濾波算法。2 常規(guī)卡爾曼濾

3、波2.1 協(xié)方差卡爾曼濾波考慮如下線性離散系統(tǒng) (2.1.1) (2.1.2)式中是狀態(tài)向量,是量測向量,是系統(tǒng)噪聲向量,是量測噪聲向量.假設系統(tǒng)噪聲和量測噪聲是互不相關的零均值高斯白噪聲,方差陣分別為，則協(xié)方差卡爾曼濾波方程為： (2.1.3) (2.1.4) (2.1.5) (2.1.6) (2.1.7)理論分析和實際應用均證明上述濾波公式是數(shù)值不穩(wěn)定的,其原因是由于計算機有限字長的限制,計算中舍入誤差和截斷誤差的累積、傳遞會使協(xié)方差陣失去對稱正定性,因此,Joseph提出一種所謂“穩(wěn)定化”卡爾曼濾波,其目的是減小濾波算法對計算舍入誤差的靈敏性,保證的對稱正定性,以提高濾波的數(shù)值穩(wěn)定性,防

4、止發(fā)散.其濾波陣公式，只是將(2.1.6)式改寫為如下形式即可: (2.1.8)但該算法由于所需計算量和存儲量較大,而且并不一定很奏效,因而應用并不廣泛.2.2 信息濾波為了解決在某些沒有有關初始狀態(tài)信息和先驗知識可供采用情況下的濾波,Fraser提出了信息濾波，即用協(xié)方差陣的逆來代替的遞推計算,這種算法對測量更新比較有效,但時間更新所需計算量較大.2.3 推廣卡爾曼濾波器推廣卡爾曼濾波(EKF)是一種應用最廣泛的非線性系統(tǒng)濾波方法。EKF與線性卡爾曼濾波公式完全類似,只是上述濾波公式中,和要在由非線性函數(shù)的偏導計算得到,不能象線性濾波那樣可事先離線計算增益和協(xié)方差陣,但EKF與常規(guī)卡爾曼濾波

5、一樣,數(shù)值穩(wěn)定性差,初值不易確定.為了改善上述常規(guī)濾波算法的數(shù)值穩(wěn)定性,并提高計算效率,自七十年代以來,人們提出了平方根濾波、U一D分解濾波、奇異值分解濾波等一系列數(shù)值魯棒的濾波算法. 3 基于矩陣因式分解的濾波方法3.1 預備知識定理3.1.1 設A是實正定對稱矩陣，則存在唯一正線下三角矩陣S，使得 （3.1.1）定理3.1.2 Householder變換 設，且，則 （3.1.2）稱為初等酉陣，或Householder變換。定理3.1.3 Cholesky分解 設是正定Hermite矩陣，用L表示單位下三角矩陣，D是對角矩陣，則有 （3.1.3）定理3.1.4 QR分解 設，則A可唯一地分

6、解為 （3.1.4）定理3.1.5 奇異值分解 設，是A的r個奇異值，則存在m階酉矩陣和n階酉矩陣V，使得 （3.1.5）其中，且。3.2 平方根協(xié)方差濾波（SRCF）首先提出平方根濾波思想的是Potter,他把按Cholesky方法分解為下三角陣,即令,在濾波遞推計算中用的傳遞計算代替的計算,由公式（3.1.1）可知，從而保證了的對稱正定性.Potter的算法經(jīng)美國阿波羅登月艙的實際應用,證明是很成功的.隨后,Potter的算法被推廣來解決存在著系統(tǒng)噪聲和量測量為向量的情形。Schmidt給出了向量量測既可以同時處理,也可以序列處理的一種處理過程噪聲的方法.為了提高平方根濾波的計算效率,Ca

7、rlson注意到傳遞陣通常是塊上三角陣的特點,給出了一種量測更新和時間更新均為上三角陣形式的快速平方根濾波,減少了計算量.上述平方根濾波均把時間更新和量測更新按常規(guī)分成兩個分離的過程,其算法的關鍵是通過利用正交變換獲得上三角陣的平方根矩陣.為了減小計算量,人們對如何構(gòu)造正交變換的問題給予了很大的注意,常用的正交變換方法是Householder變換，即公式（3.1.2）、修正的Gram-Sehmidt正交化法及Givens變換等。1975年,Morf一Kailath在總結(jié)上述平方根濾波基礎上,把時間更新和量測更新兩個過程結(jié)合起來,給出了一種量測和時間更新的聯(lián)合更新方程,從而僅需一個正交變換,即完

8、成濾波計算,且無需計算濾波增益陣.3.2 平方根信息濾波與平方根協(xié)方差濾波相對應,信息濾波的平方根濾波方法也得到人們的極大重視和研究。Dyer一MeReynolds基于Householder變換利用動態(tài)規(guī)劃理論研究出一種平方根信息濾波(SRIF),與SRCF類似,SRIF把信息矩陣定義為平方根陣形式,即定義由的遞推計算來代替的計算.Bierman利用“數(shù)據(jù)方程”法給出一種結(jié)構(gòu)較簡單的SRIF,并給出有色噪聲情況的濾波公式,該算法需要計算狀態(tài)轉(zhuǎn)移矩陣的逆,即要求狀態(tài)轉(zhuǎn)移矩陣是非奇異的,針對這一問題,給出一種對狀態(tài)轉(zhuǎn)移陣奇異仍適用的SRIF,Bierma在此基礎上,把SRIF應用于具有時間延遲系統(tǒng)

9、的濾波,并把SRIF推廣到大規(guī)模互聯(lián)系統(tǒng)的情形,大大減少了計算量和存儲量.正如SRCF那樣,將量測更新和時間更新結(jié)合起來,可以容易的求得聯(lián)合SRIF更新方程,Paige-Saunders基于把卡爾曼濾波轉(zhuǎn)換為最小二乘估計的思想,提出一種聯(lián)合量測更新和時間更新的SRIF方案。由于SRIF在某些情況下,如對于多量測量系統(tǒng),比常規(guī)卡爾曼濾波有更高的計算效率、更好的數(shù)值穩(wěn)定性和精度,因此,在軌道確定、飛行狀態(tài)估計和多傳感器跟蹤與辨識等方面得到應用。3.3 U-D分解濾波上述SRCF和SRIF,一般來講由于存在矩陣的求逆運算和平方根計算,所需計算量較常規(guī)卡爾曼濾波要大,因而限制了在工程中的應用。Bier

10、man在研究和應用SRIF及Carlson序列濾波的基礎上,于19751977年間,提出了一套計算效率高、數(shù)值穩(wěn)定的稱之為“U-D分解”濾波的算法。該算法把協(xié)方差陣分解為單位上三角陣U和對角陣D,即有，相當于協(xié)方差平方根陣S，即公式（3.1.3）。U-D分解濾波既具有平方根濾波的優(yōu)點,即始終能保證協(xié)方差陣的正定性,同時避免了Carlson等平方根濾波算法中平方根的計算,因而具有與常規(guī)卡爾曼濾波相當?shù)挠嬎懔?是上述濾波算法中效率最高的一種算法,并且在實際應用問題中,結(jié)合實際問題的特點,U-D分解算法計算效率還更高,因而,近年來在軌道確定、目標跟蹤和飛行狀態(tài)估計及神經(jīng)網(wǎng)絡學習算法等方面得到廣泛應用

11、和發(fā)展,但該算法由于量測更新采用序列處理,對于量測量較多的系統(tǒng),計算效率受到一定影響,對有色噪聲的處理不如SRIF,SRCF方便。3.4 基于奇異值分解（SVD）的濾波方法奇異值分解由于具有很強的數(shù)值魯棒性和可靠性,廣泛應用于最小二乘問題、病態(tài)方程組求解及廣義逆計算等場合,并在控制、通訊與信號處理等領域越來越受到人們的極大重視。在濾波問題中,也已得到應用。Oshman基于協(xié)方差陣的頻譜分解,以SVD為計算工具,提出稱之為v-Lambda濾波的方法,他把協(xié)方差陣分解成形式,其中V是矩陣P的特征向量矩陣,為對角元是P陣奇異值的對角矩陣。首先給出量測更新為信息濾波模式,時間更新為協(xié)方差濾波模式的v一

12、Lambda濾波方法。隨后,Oshman又給出量測更新和時間更新方程均為信息濾波模式的濾波公式,該算法不需計算濾波增益,由于利用SVD,使得狀態(tài)估計算法魯棒性較之平方根濾波、U-D分解濾波更好,但該算法由于進行一步濾波迭代計算,需一次正交變換,兩次奇異值分解,所以,其缺點是計算量較大,但其優(yōu)異的數(shù)值魯棒性以及隨著奇異值分解并行處理的實現(xiàn)而隨之帶來計算時間的減少,使得此算法將成為一種極富吸引力的濾波方法.另外利用協(xié)方差陣的對稱正定性,給出一種類似U-D分解形式、計算量較小的基于SVD的濾波算法,本文又給出一種基于SVD的推廣卡爾曼濾波算法,并應用于飛行狀態(tài)估計問題,隨后又提出一種基于SVD的遞推

13、最小二乘辨識新方法,與遞推最小二乘、基于U-D分解的遞推最小二乘法相比,不僅收斂速度快、數(shù)值穩(wěn)定性和辨識精度高,而且能得到系統(tǒng)參數(shù)的無偏估計。4 一種基于Q-R矩陣分解的自適應濾波算法4.1 機動目標加速度模型及自適應卡爾曼算法令狀態(tài)向量X為:,x1,分別表示目標的位置、速度和加速度。假設目標當前加速度服從非零均值的一階馬爾可夫過程,即: （4.1.1） （4.1.2）目標的時域狀態(tài)方程為： （4.1.3）式中：、為白噪聲序列。根據(jù)上述時域方程,經(jīng)過離散化處理后,得到系統(tǒng)的離散狀態(tài)方程和觀測方程為: （4.1.4） （4.1.5）式中：均為白噪聲序列?；跔顟B(tài)方程（4.1.4）和觀測方程（4.

14、1.5），依據(jù)標準的卡爾曼濾波方程，得到當前機動加速度統(tǒng)計模型的自適應卡爾曼濾波算法。1. 時間更新 (4.1.6) (4.1.7) (4.1.8) (4.1.9) (4.1.10)其中，表示最大可能的加速度。2. 測量更新 (4.1.11) (4.1.12)4.2 引入Q-R矩陣分解的自適應平方根濾波算法在機動“當前”統(tǒng)計自適應濾波算法中,在計算協(xié)方差陣時,存在矩陣相減的運算,由于計算或其他參數(shù)不匹配的影響,有可能導致協(xié)方差陣出現(xiàn)不對稱或負定的情況。協(xié)方差陣是一個對稱的非負定的矩陣,利用矩陣分解的技術,在基于平方根矩陣分解的基礎上,引入Q-R矩陣分解,構(gòu)造出協(xié)方差平方根自適應濾波算法。令其中

15、為下三角矩陣。又對進行Q-R分解得到： (4.2.1)因此有經(jīng)過推導,得到下述的基于Q-R分解的自適應平方根卡爾曼濾波算法。1. 時間更新 (4.2.2)進行Q-R分解得到 (4.2.3) (4.2.4) (4.2.5) (4.2.6) (4.2.7) (4.2.8) (4.2.9) (4.2.10) (4.2.11)2. 測量更新 (4.2.12) (4.2.13) (4.2.14)4.3 仿真與結(jié)論為了驗證算法的有效性,進行了一系列的仿真,并在一維的情況下,研究了基于Q-R矩陣分解的當前統(tǒng)計自適應濾波算法對常加速度目標運動的跟蹤特性。仿真中,假設觀測噪聲方差(距離觀測誤差)與目標距離的平方

16、成正比,即觀測噪聲為:式中為相對誤差系數(shù),c為固定觀測誤差,w(k)為均值為零,方差為1的標準正態(tài)分布函數(shù)。因此,觀測噪聲方差為:仿真中所選參數(shù)為:c=100m,=0.01,目標加速度=10m/s2,采樣間隔T=1.0s。圖1和圖2分別給出沒有自適應的濾波器和基于Q-R矩陣分解的當前統(tǒng)計自適應濾波器的位置、速度和加速度濾波誤差曲線。圖1非基于Q-R矩陣分解的加速度濾波器的濾波誤差曲線圖2基于Q-R矩陣分解的機動加速度“當前”統(tǒng)計模型自適應濾波器的濾波誤差曲線由以上圖可知,在目標作勻加速直線運動的情形下,基于Q-R矩陣分解的當前統(tǒng)計自適應濾波器的濾波效果好于非基于Q-R矩陣分解的濾波算法。圖中顯示的是濾波誤差曲線,其中(1 000m-12s)表示縱軸的一格代表1 000m,橫軸的一格代表12s。5 自我小結(jié)本文主要對常規(guī)的卡爾曼濾波、矩陣因式分解濾波等數(shù)值計算方法進行了綜述，給出了這些濾波方法的優(yōu)缺點及適用領域。同時重點介紹了一種基于Q-R矩陣分解的自適應濾波算法，該方法將協(xié)方差矩陣分解為兩個矩陣的乘積，來保證協(xié)方差矩陣的正定性，仿真結(jié)果表明，該算法可以較好地跟蹤機動目標，具有精度高、穩(wěn)定好、收斂快等特點。本文主要從矩陣三角分