卡爾曼濾波算法的研究與仿真

張波濤 蒼鵬浩 劉暢

摘要：卡爾曼濾波算法是卡爾曼和布西于1960年和1961年提出來的一種遞推濾波方法，由于系統(tǒng)中狀態(tài)量受到噪聲干擾是隨機(jī)的不能得到精確數(shù)據(jù)，卡爾曼濾波是依據(jù)一組觀測(cè)值消除隨機(jī)干擾，盡可能預(yù)測(cè)出接近真實(shí)值的估計(jì)值。它便于計(jì)算機(jī)編程實(shí)現(xiàn)與現(xiàn)場(chǎng)數(shù)據(jù)實(shí)時(shí)更新處理，是效率高且最為廣泛的濾波方法，在通信、電力、航空等眾多工業(yè)、軍事領(lǐng)域都得到了應(yīng)用。

關(guān)鍵詞：卡爾曼;算法;仿真

濾波本質(zhì)上是信號(hào)處理與變換（去除或減弱不想要的成分，增強(qiáng)所需成分）的過程，這個(gè)過程既可以通過硬件來實(shí)現(xiàn)，也可以通過軟件來實(shí)現(xiàn)?？柭鼮V波屬于一種軟件濾波方法，基本思想是：以最小均方誤差為最佳估計(jì)準(zhǔn)則，采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測(cè)值來更新對(duì)狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值，卡爾曼濾波算法根據(jù)建立的系統(tǒng)方程和觀測(cè)方程對(duì)需要處理的信號(hào)做出滿足最小均方誤差的估計(jì)，即濾波結(jié)果。

1???? 卡爾曼濾波原理

假設(shè)一個(gè)離散控制過程的系統(tǒng)滿足以下描述：系統(tǒng)的狀態(tài)轉(zhuǎn)換過程可以描述為一個(gè)離散時(shí)間的隨機(jī)過程，系統(tǒng)的狀態(tài)受控制輸入的影響，系統(tǒng)狀態(tài)及觀測(cè)過程都不可避免的受到噪聲影響，系統(tǒng)狀態(tài)是非直接可觀測(cè)的。

該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程來描述，即系統(tǒng)方程：

x（k）=Ax（k-1）+Bu（k-1）+w（k-1）

再加上系統(tǒng)的觀測(cè)方程：

z（k）=Hx（k）+v（k）

其中：x（k）是k時(shí)刻的系統(tǒng)狀態(tài)，u（k）是k時(shí)刻對(duì)系統(tǒng)的控制量，w（k），v（k）分別表示過程噪聲和測(cè)量噪聲（假設(shè)為高斯白噪聲），其協(xié)方差分別為Q和R;z（k）是觀測(cè)值：A是系統(tǒng)狀態(tài)矩陣;B是系統(tǒng)控制矩陣;H是輸出矩陣。

首先我們對(duì)系統(tǒng)進(jìn)行預(yù)測(cè)，得到預(yù)測(cè)結(jié)果。

利用系統(tǒng)的過程模型，預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，依據(jù)系統(tǒng)的上一狀態(tài)（即：k-1）和系統(tǒng)模型預(yù)測(cè)現(xiàn)在的狀態(tài)：

x（k|k-1）=Ay（k-1|k-1）+Bu（k）????? （1）

其中，x（k|k-1）是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，y（k-1|k-1）是上一狀態(tài)最優(yōu)的結(jié)果，u（k）為現(xiàn)在狀態(tài)的控制量，若沒有控制量，取值0。

誤差的協(xié)方差預(yù)測(cè)方程為：

p（k|k-1）=Ap（k-1|k-1）A+Q???? （2）

其中，p（k|k-1）是x（k|k-1）的協(xié)方差，p（k-1|k-1）是x（k-1|k-1）的協(xié)方差，A是A的轉(zhuǎn)置矩陣，Q是系統(tǒng)噪聲的協(xié)方差。

然后我們收集現(xiàn)在狀態(tài)的觀測(cè)值，結(jié)合上述的預(yù)測(cè)結(jié)果，可以得到現(xiàn)在狀態(tài)（k）的最優(yōu)化估算值y（k|k）：

y（k|k）=x（k|k-1）+Kg（k）（z（k）-Hx（k|k-1））（3）

其中，Kg為卡爾曼增益，卡爾曼增益方程為：

Kg（k）=p（k|k-1）H/（Hp（k|k-1）H+R）????? （4）

為了卡爾曼濾波算法可以遞推下去，還需要更新現(xiàn)在狀態(tài)（即k）下Y（k|k）的協(xié)方差：

p（k|k）=（I-Kg（k）H）p（k|k-1）（5）

其中，I為單位矩陣，對(duì)于單模型單測(cè)量，I=1。

卡爾曼濾波算法本質(zhì)上是一個(gè)遞推反饋算法，它的兩部分：時(shí)間更新方程（狀態(tài)方程）和測(cè)量狀態(tài)更新方程。其中，前者負(fù)責(zé)遞推，后者負(fù)責(zé)反饋，將先驗(yàn)估計(jì)和新的測(cè)量變量結(jié)合，以構(gòu)造改進(jìn)后的后驗(yàn)估計(jì)。為了更直觀理解卡爾曼濾波，給出卡爾曼濾波流程圖如圖1 所示。依據(jù)流程圖，只要給定初值x（0）和p（0），根據(jù)k時(shí)刻的測(cè)量值z(mì)（k）就可以通過遞推計(jì)算得到時(shí)刻的狀態(tài)估計(jì)x（k）。

2???? 建立數(shù)學(xué)模型

卡爾曼濾波作為一種數(shù)值估計(jì)優(yōu)化方法，與應(yīng)用領(lǐng)域的背景結(jié)合性很強(qiáng)。因此在應(yīng)用卡爾曼濾波解決實(shí)際問題時(shí)，重要的不僅僅是算法的實(shí)現(xiàn)與優(yōu)化問題，更重要的是利用獲取的領(lǐng)域知識(shí)對(duì)被認(rèn)識(shí)系統(tǒng)進(jìn)行形式化描述，建立起精確的數(shù)學(xué)模型，再從這個(gè)模型出發(fā)，進(jìn)行濾波器的設(shè)計(jì)與實(shí)現(xiàn)工作。

本文建立一個(gè)簡(jiǎn)單的線性測(cè)量系統(tǒng)模型，測(cè)量與移動(dòng)目標(biāo)之間的距離，即單個(gè)變量。假設(shè)當(dāng)前時(shí)間的距離和前一時(shí)間的距離是相同的，所以選擇A=1，不存在控制量，u（k）=0。根據(jù)公式（1）可以得出預(yù)測(cè)方程：

x（k）=y（k-1）

根據(jù)公式（2）可以得出預(yù)測(cè)方差方程：

p1（k）=p（k-1）+Q

根據(jù)公式（3）可以得出濾波后的最優(yōu)估算方程：

y（k）=x（k-1）+Kg（k）（z（k）-x（k-1））

根據(jù)公式（4）可以得出卡爾曼濾波增益方程：

Kg（k）=p1（k）/（p1（k）+R）

根據(jù)公式（5）可以得出k狀態(tài)下的方差方程：

p（k）=（1-Kg（k））*p1（k）

根據(jù)上述建立的數(shù)學(xué)模型，給定初始的預(yù)測(cè)值x（0）、方差p（0）以及一組觀測(cè)數(shù)據(jù)z，即可實(shí)現(xiàn)對(duì)該組測(cè)量數(shù)據(jù)的卡爾曼濾波。

3???? Matlab仿真

采用Matlab對(duì)上述建立的數(shù)學(xué)模型進(jìn)行仿真，檢測(cè)卡爾曼濾波的效果和影響，采用一組工程應(yīng)用中的觀測(cè)數(shù)據(jù)，由于卡爾曼的遞推特性，預(yù)測(cè)值可以隨意賦值，方差的初始值可以取非0 的任何值。

通過調(diào)整過程噪聲和測(cè)量噪聲的協(xié)方差Q和R的值可以調(diào)節(jié)卡爾曼濾波的效果及濾波數(shù)據(jù)的實(shí)時(shí)性。仿真結(jié)果見圖2、圖3、圖4、圖5，選擇的Q和R的值見表1。由濾波效果可以看出：Q值主要影響濾波效果，值越大，濾波效果越差，值越小濾波效果越顯著;R值主要影響濾波后數(shù)據(jù)的實(shí)時(shí)性，值越大，濾波后數(shù)據(jù)的實(shí)時(shí)性越差，值越小濾波后數(shù)據(jù)的實(shí)時(shí)性越好。

根據(jù)上述仿真結(jié)論得知，在實(shí)際應(yīng)用過程中，可以根據(jù)系統(tǒng)的需求在濾波效果和實(shí)時(shí)性方面做出選擇，即選擇不同的Q和R值。

4結(jié)束語

本文介紹了卡爾曼濾波的原理，依據(jù)其系統(tǒng)方程和觀測(cè)方程推導(dǎo)出了卡爾曼濾波的五個(gè)遞推方程，在此基礎(chǔ)上實(shí)現(xiàn)建立了最簡(jiǎn)單的線性離散系統(tǒng)數(shù)學(xué)模型，通過Matlab對(duì)其進(jìn)行仿真，通過仿真實(shí)驗(yàn)結(jié)果得出系統(tǒng)噪聲和觀測(cè)噪聲的協(xié)方差是影響濾波效果和實(shí)時(shí)性的因素，在實(shí)際應(yīng)用中可以依據(jù)系統(tǒng)的需求在兩者之間選擇。

參考文獻(xiàn)：

[1] 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用.軟件導(dǎo)刊.2009.

[2] 張滿生.張學(xué)莊.卡爾曼濾波器及其工程應(yīng)用.計(jì)算機(jī)技術(shù)與自動(dòng)化.2008.