有色噪聲下的卡爾曼濾波

有色噪聲下的卡爾曼濾波摘要Kalman濾波技術(shù)是一種高效率的遞歸濾波器(自回歸濾波器)，它是現(xiàn)代信息處理中的重要工具。但是基本的Kalman濾波基本方程中要求系統(tǒng)噪聲和量測噪聲必須為互不相關(guān)的均值為零的白噪聲過程,限制了應(yīng)用的范圍。本文研究了在系統(tǒng)噪聲和量測噪聲都是有色噪聲條件下的Kalman濾波方法，并推導(dǎo)了全套的濾波方程。最后以GPS多天線三維姿態(tài)測量系統(tǒng)為例，根據(jù)推導(dǎo)出的動態(tài)噪聲、觀測噪聲為有色噪聲的線性系統(tǒng)濾波公式，在MATLAB環(huán)境下進(jìn)行了仿真實(shí)驗(yàn)。關(guān)鍵詞：有色噪聲，卡爾曼濾波，白噪聲ABSTRACTKalmanfilteringtechnologyisakindofefficientalgorithm.onfilter(autoregressivefilter),itisanimportanttoolinmoderninformationprocessing.ButthebasicKalmanfilteringbasicequationsofnoiseandmeasurementrequirementssystemforirrelevantnoisemustbezeroofwhitenoiseprocess,limitthescopeofapplication.InthispaperwestudiedsystemnoisesandmeasurementnoisearecolorednoiseKalmanfilteringmethodundertheconditions,andderivedfullsetoffilterequation.FinallyforexamplewithGPSmulti-antenna3dposemeasurementsystem,CarriedoutinMATLABsimulationexperimentaccordingtothedynamicnoiseisdeduced,observationnoiseforcolorednoiselinearsystemfilteringformula.KeyWords：ColoredNoise,KalmanFilter,WhiteNoise一、引言卡爾曼濾波技術(shù)是20世紀(jì)60年代在現(xiàn)代控制理論的發(fā)展過程中產(chǎn)生的一種最優(yōu)估計(jì)技術(shù)。它不要求保存過去的測量數(shù)據(jù)，而是根據(jù)新的數(shù)據(jù)和前一時(shí)刻的諸量估計(jì)值，借助系統(tǒng)本身的狀態(tài)轉(zhuǎn)移方程（即動態(tài)方程），按照一套遞推公式，即可算出新的諸量的估計(jì)值。因此，這種方法十分適合于動態(tài)測量，即目標(biāo)是運(yùn)動的，如飛機(jī)、船舶、人造衛(wèi)星以及導(dǎo)彈等運(yùn)載體［i］,Kalman濾波最成功的工程應(yīng)用在于高精度組合導(dǎo)航系統(tǒng)的設(shè)計(jì)，并發(fā)展出了分散濾波理論和聯(lián)邦濾波理論等［2］。在基本Kalman濾波方程中，要求系統(tǒng)噪聲矢量和量測噪聲矢量都是互不相關(guān)的零均值的白噪聲過程。實(shí)際工程系統(tǒng)中，系統(tǒng)噪聲和量測噪聲往往都不是白噪聲；噪聲均值即使為零，2種噪聲在不同歷元的協(xié)方差函數(shù)也不為零。如果按照理想狀態(tài)進(jìn)行處理，必將造成濾波發(fā)散，使濾波器無法正常工作。本文在此基礎(chǔ)上進(jìn)一步推導(dǎo)了系統(tǒng)噪聲和量測噪聲都為有色噪聲情況下的濾波方程。二、卡爾曼濾波在卡爾曼濾波方法出現(xiàn)以前，采用的是線性最小方差估計(jì)，它適用于隨機(jī)變量是個(gè)平穩(wěn)的隨機(jī)過程，其確定性部分的變化在觀測值采集期間是不變的或變化很慢，所以是一種靜態(tài)估計(jì)。為了提高觀測精度和減少噪聲的影響，常需要將觀測次數(shù)K增大，這樣就降低了實(shí)時(shí)性。在工程實(shí)踐中，經(jīng)常會碰到待估的隨機(jī)量（狀態(tài)變量）是確定性部分變化比較快的非平穩(wěn)隨機(jī)過程，這時(shí)用靜態(tài)的線性最小方差估計(jì)顯然是不合適的。隨著空間技術(shù)的發(fā)展，60年代初，卡爾曼（Kalman）、布西（Bucy）等人提出了一種遞推式濾波方法，即卡爾曼濾波，它可以滿足要求的，它不是為了尋求最優(yōu)解，而是在數(shù)字計(jì)算機(jī)上得出遞推的數(shù)字解，這是由于下一時(shí)刻（K+1）的數(shù)值估計(jì)是利用了上一時(shí)刻的估計(jì)值和實(shí)時(shí)觀測值，而上一時(shí)刻的估計(jì)是建立在上一時(shí)刻和上一時(shí)刻以前的所有時(shí)刻的估計(jì)基礎(chǔ)上，所以它具有遞推性;其次，卡爾曼濾波把被估計(jì)的隨機(jī)量作為系統(tǒng)的狀態(tài),用系統(tǒng)狀態(tài)方程來描述狀態(tài)的轉(zhuǎn)移過程，各時(shí)刻之間的狀態(tài)相關(guān)函數(shù)，可用狀態(tài)方程的轉(zhuǎn)移特性來描述，從而解決了非平穩(wěn)隨機(jī)過程估計(jì)困難的問題，因此卡爾曼濾波具有動態(tài)性［2］。卡爾曼濾波的對象是用狀態(tài)方程描述的隨機(jī)線性系統(tǒng)。它按照估計(jì)誤差的方差最小的原則，從被觀測噪聲污染了的觀測值中實(shí)時(shí)估計(jì)出系統(tǒng)的各個(gè)狀態(tài)值。卡爾曼濾波的最優(yōu)準(zhǔn)則是無偏最小方差估計(jì)，即在濾波過程中，噪聲方差必須是收斂的，最終達(dá)到接近于無偏的最優(yōu)狀態(tài)。隨機(jī)線性離散系統(tǒng)的狀態(tài)方程和量測方程為［3］：

x=qx+rw1)k k,k-1k-1k-1k-11)Z=HX+Vk kkkx式中：k:狀態(tài)方程；①kkT:狀態(tài)轉(zhuǎn)移矩陣；x：前一時(shí)刻的狀態(tài)量；k-1rk-1：系統(tǒng)噪聲矩陣；W：輸入信號，是高斯白噪聲；k-1Zk：系統(tǒng)的量測數(shù)據(jù)；H：系統(tǒng)量測值的矩陣；kVk：輸出信號的觀測噪聲，是高斯白噪聲Kalman濾波基本方程為［3］：Kalman濾波基本方程為［3］：/X /XX二①Xk/k-1 k,k-1k二X+Kk/k-1 k二P HtSk/k-1k二①P?t+Qk-1 k.k-1kk,k-1 k-1=（I-KH）Pkk k/k-1XkKkPk/k-1Pk（L 八）X-HXkkk/k-1PHt+R人kk,k-1kk2)八X式中： k-1為前一步計(jì)算的狀態(tài)估計(jì)；Rk為量測噪聲方差陣；Pk-1為前一時(shí)刻估值均方差；Qk-1為系統(tǒng)上一時(shí)刻的噪聲方差陣；I為單位矩陣。在上面的基本Kalman濾波方程中，要求系統(tǒng)噪聲矢量W,和量測噪聲矢量kVk都是互不相關(guān)的、零均值的白噪聲過程［3］。k

三、有色噪聲下的卡爾曼濾波1、系統(tǒng)噪聲為有色噪聲有色噪聲可以看成是由白噪聲通過一個(gè)動態(tài)系統(tǒng)形成的，Wk可以描述成為:kw=nw+g （3）k k,k-1k-1k-1式中：Ek為零均值的白噪聲序列。在設(shè)計(jì)濾波器時(shí)可以將Wk看成系統(tǒng)狀態(tài)進(jìn)行處理，可以得到系統(tǒng)的狀態(tài)方k程和量測方程：Xa二①aXa+TaWa4）k k,k-1k-1 k-1k-14）Z=HaXa+Vk kkk式中：氣。亦=g丿是零均值的白噪聲序列，方差為q；。2、量測噪聲為有色噪聲有色量測噪聲Vk可以描述成：kV二屮V+匚k k2、量測噪聲為有色噪聲有色量測噪聲Vk可以描述成：kV二屮V+匚k k,k-1k-1 k-1方差Rk，匚k與Wk不相關(guān)。由（4）中量測方程可得：Vk=Z—HaXakkk由（5）、（6）可得：Z 一屮k+1Zk+1,k k=a①a —屮 Haa+k+1 k+1,k k+1,k kkHaTaWa+k+1 kkk令：Z*=Z—TZk k+1 k+1,k kH*=Ha①a—THak k+1 k+1,k k+1,k kV*=HaTaWa+Qk k+1kkk可以得到狀態(tài)方程和量測方程：（5）（6）（7）（8）（9）10）Xa=①aXq+TaWak k,k-1 k-1 k-1 k-1Z*=H*Xa+V*k kkkR*=HaTaQR*=HaTaQaTaTHaTk k+1kkk k+1Vk*與Wa相關(guān)，且Sk=。門THk+]T。

k k k kk k+根據(jù)白噪聲條件下一步預(yù)測的一般方程[4fXa=oaXa+K丄

k+1 k+1,k k k+1K丄k+1方差為有：aPH*T+TaSk+1,k kk kkJ*-H*X*k kk丿H*PH*t+R*kkkP=①ak+1 kKk+1ZP①aT+TaQaTa'1,k k k+1,k kkkH*P①aT+StTaTkk k+1,k kk12)13)14)15)由此就可以確定系統(tǒng)噪聲有量測噪聲都為有色噪聲時(shí)的Kalman濾波方程。四、實(shí)驗(yàn)仿真對于GPS多天線三維姿態(tài)測量系統(tǒng)，當(dāng)考慮該系統(tǒng)為隨機(jī)系統(tǒng)時(shí)，要采用狀態(tài)反饋控制，必須對狀態(tài)進(jìn)行估計(jì)，常用的狀態(tài)估計(jì)器就是卡爾曼濾波器。通過卡爾曼濾波器對三維姿態(tài)角（偏航角、俯仰角、橫滾角）進(jìn)行測量估計(jì)。有色噪聲可以看作是白色噪聲通過具有某個(gè)傳輸函數(shù)的網(wǎng)絡(luò)時(shí)的響應(yīng)，所以，我們就可以對有色噪聲建立數(shù)學(xué)模型。也就是說，不論是連續(xù)信號還是離散信號我們總是有辦法將有色噪聲白色化，從而可以用卡爾曼濾波器對其進(jìn)行所需的處理。根據(jù)測姿原理，可列出如下系統(tǒng)方程和量測方程：①二G①+TW16)k k,k-1k-1k-1k-16)Z二H①+vk kkk當(dāng)W是有色噪聲時(shí)，對其建模，有下式：k-117)W=FW+17)k k,k-1k-1k-1S為零均值白噪聲。k-1對系統(tǒng)噪聲W做白色化處理，即將W也列為狀態(tài)，得到狀態(tài)方程和量測k-1 k-1方程：

XGXWkk,k1k1k1k1ZHX（18）kkkkGGk,k1k1式中：k,k10Fk,k10k1IWSk1k1HH0k1k1因?yàn)橄到y(tǒng)噪聲中Wk1Sk1,即⑷k］為零均值白噪聲，量測方程中1＜也為零均值白噪聲，所以（18）式符合卡爾曼濾波基本方程。因此，就完成了對原系統(tǒng)有色噪聲的白色化處理。對（18）式應(yīng)用卡爾曼濾波方程，則有：狀態(tài)一步預(yù)測：XGXk/k1k,k1k1狀態(tài)估計(jì)：XXKZHXk k/k1kkkk/k1濾波增益：KPHTHPHTR1k k/k1 kkk/k1kk一步預(yù)測均方誤差：PGPGTQTk/k1 k,k1k1k,k1k1k1k1估計(jì)均方誤差：PIKHPIKHTKRKTkkkk/k1 kk kkkIKHPkkk/k1本文仿真以風(fēng)云一號氣象衛(wèi)星-C星（FY-1C）為模型。當(dāng)取初始條件，通過計(jì)算機(jī)仿真，可以得出結(jié)果：由圖（1）可以看出，三維姿態(tài)角的仿真曲線均能迅速收斂，達(dá)到穩(wěn)態(tài)值，沒有出現(xiàn)發(fā)散現(xiàn)象。可以說明該卡爾曼濾波模型是成功的，能有效的應(yīng)用于系統(tǒng)的數(shù)值估計(jì)。0O-^-gcdK向航0O-^-gcdK向航仰俯滾橫圖1仿真結(jié)果五、小結(jié)本文研究了在系統(tǒng)噪聲和量測噪聲都是有色噪聲條件下的K