卡爾曼濾波器設(shè)計(jì)

1、 姓姓 名名 王王 超超 星星 專(zhuān)專(zhuān) 業(yè)業(yè) 機(jī)機(jī) 械械 工程工程 導(dǎo)師導(dǎo)師 王王 殿殿 君君 一、卡爾曼濾波產(chǎn)生的背景二、卡爾曼濾波器的基本思想三、卡爾曼濾波器的原理四、直流信號(hào)的卡爾曼濾波器設(shè)計(jì)一、卡爾曼濾波產(chǎn)生的背景 在濾波器發(fā)展的過(guò)程中，早期的維納濾波器是是由數(shù)學(xué)家維納（Rorbert Wiener）提出的一種以最小平方為最優(yōu)準(zhǔn)則的線性濾波器。利用平穩(wěn)隨機(jī)過(guò)程的相關(guān)特性和頻譜特性對(duì)混有噪聲的信號(hào)進(jìn)行濾波的方法。維納濾波器的不足之處為：第一，必須利用全部的歷史觀測(cè)數(shù)據(jù)，存儲(chǔ)量和計(jì)算量都很大；第二，當(dāng)獲得新的觀測(cè)數(shù)據(jù)時(shí)，沒(méi)有合適的遞推算法，必須進(jìn)行重新計(jì)算；第三，很難用于非平穩(wěn)過(guò)程的濾波。為

2、了克服維納濾波器的上述不足之處卡爾曼等人在維納濾波的基礎(chǔ)上，于60年代初提出了一種遞推濾波方法，稱(chēng)為卡爾曼濾波。二、卡爾曼濾波器的基本思想 卡爾曼濾波器是一種由卡爾曼（Kalman）提出的用于時(shí)變線性系統(tǒng)的遞歸濾波器。這個(gè)系統(tǒng)可用包含正交狀態(tài)變量的微分方程模型來(lái)描述，這種濾波器是將過(guò)去的測(cè)量估計(jì)誤差合并到新的測(cè)量誤差中來(lái)估計(jì)將來(lái)的誤差。用前一個(gè)估計(jì)值和最近一個(gè)觀察數(shù)據(jù)來(lái)估計(jì)信號(hào)的當(dāng)前值，是用狀態(tài)方程和遞推的方法進(jìn)行估計(jì)的，其解是以估計(jì)值形式給出。它的系統(tǒng)參數(shù)、系統(tǒng)控制量、估計(jì)的誤差值、系統(tǒng)過(guò)程的協(xié)方差和卡爾曼增益等都是隨著時(shí)間變化而變化的。通過(guò)遞推公式不斷更新以上各種變量的值。首先引入一個(gè)隨機(jī)

3、離散系統(tǒng)模型 預(yù)測(cè)值：X(k)=A X(k-1)+B U(k)+W(k) 測(cè)量值Z(k)=H X(k)+V(k) 式中：X(k) 是k時(shí)刻的系統(tǒng)狀態(tài)。 U(k) 是k時(shí)刻對(duì)系統(tǒng)的控制量。 A和B 是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，他們?yōu)榫仃嚒?Z(k) 是k時(shí)刻的測(cè)量值。 H 是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。 W(k)和V(k) 分別表示過(guò)程和測(cè)量的噪聲。假設(shè)為 為高斯白噪音，對(duì)應(yīng)協(xié)方差分別為Q、R三、卡爾曼濾波器的原理卡爾曼濾波包括兩個(gè)階段：預(yù)測(cè)和更新。卡爾曼濾波包括兩個(gè)階段：預(yù)測(cè)和更新。 預(yù)測(cè)階段： 濾波器使用上一狀態(tài)的估計(jì)，做出對(duì)當(dāng)前狀態(tài)的估計(jì)。 更新階段： 濾波器利用對(duì)當(dāng)前狀態(tài)的

4、觀測(cè)值優(yōu)化在預(yù)測(cè)階段獲得預(yù)測(cè)值，以獲得一個(gè)更精確的新估計(jì)值。下面來(lái)介紹卡爾曼算法的五條公式現(xiàn)在我們要利用系統(tǒng)的過(guò)程模型，來(lái)預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)： X(k|k-1)=A X(k-1|k-1)+B U(k) (1) 式(1)中：X(k|k-1)是利用上一狀態(tài)預(yù)測(cè)的結(jié)果。 X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果。 U(k)為現(xiàn)在狀態(tài)的控制量，如果沒(méi)有控制量，它可以為0到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1)的covariance還沒(méi)更新。我們用P表示covariance： P(k|k-1)=A

5、P(k-1|k-1) A+Q (2) 式(2)中：P(k|k-1)是X(k|k-1)對(duì)應(yīng)的covariance。 P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的covariance。 A表示 A的轉(zhuǎn)置矩陣。 Q是系統(tǒng)過(guò)程的covariance。式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)。 現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)： X(k|k)=X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1) (3) 其中Kg為卡爾曼增益(Kalman Gain)： Kg(k)

6、= P(k|k-1) H/(H P(k|k-1) H+ R) (4) 到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要令卡爾曼濾波不斷的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance： P(k|k)=（I-Kg(k) H）P(k|k-1) (5) 式（5）中：I 為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。根據(jù)這5個(gè)公式，可以很容易的實(shí)現(xiàn)計(jì)算機(jī)的程序。 四、直流信號(hào)的卡爾曼濾波器設(shè)計(jì) 本文設(shè)計(jì)的卡爾曼濾波器是一個(gè)簡(jiǎn)單的濾波器，將一個(gè)直流信號(hào)加高斯白噪音后組合到一起的信號(hào)經(jīng)過(guò)卡爾曼濾波得到與原始直流信號(hào)無(wú)線接近的直流信號(hào)。先在程序面板上添加一個(gè)直流信號(hào)發(fā)生器并添加高斯白噪聲形成一個(gè)帶有干擾源的直