基于矩陣分解的卡爾曼濾波技術(shù)分析及應(yīng)用

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

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

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

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

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

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

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

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

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

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

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

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

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

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

15、為下三角矩陣。又對(duì)進(jìn)行Q-R分解得到： (4.2.1)因此有經(jīng)過推導(dǎo),得到下述的基于Q-R分解的自適應(yīng)平方根卡爾曼濾波算法。1. 時(shí)間更新 (4.2.2)進(jìn)行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. 測(cè)量更新 (4.2.12) (4.2.13) (4.2.14)4.3 仿真與結(jié)論為了驗(yàn)證算法的有效性,進(jìn)行了一系列的仿真,并在一維的情況下,研究了基于Q-R矩陣分解的當(dāng)前統(tǒng)計(jì)自適應(yīng)濾波算法對(duì)常加速度目標(biāo)運(yùn)動(dòng)的跟蹤特性。仿真中,假設(shè)觀測(cè)噪聲方差(距離觀測(cè)誤差)與目標(biāo)距離的平方

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