張波濤 蒼鵬浩 劉暢

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

關鍵詞：卡爾曼;算法;仿真

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

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

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

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

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

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

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

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

首先我們對系統(tǒng)進行預測，得到預測結果。

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

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

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

誤差的協(xié)方差預測方程為：

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的轉置矩陣，Q是系統(tǒng)噪聲的協(xié)方差。

然后我們收集現(xiàn)在狀態(tài)的觀測值，結合上述的預測結果，可以得到現(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為單位矩陣，對于單模型單測量，I=1。

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

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

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

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

x（k）=y（k-1）

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

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ù)學模型，給定初始的預測值x（0）、方差p（0）以及一組觀測數(shù)據(jù)z，即可實現(xiàn)對該組測量數(shù)據(jù)的卡爾曼濾波。

3???? Matlab仿真

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

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

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

4結束語

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

參考文獻：

[1] 彭丁聰.卡爾曼濾波的基本原理及應用.軟件導刊.2009.

[2] 張滿生.張學莊.卡爾曼濾波器及其工程應用.計算機技術與自動化.2008.