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

張波濤 蒼鵬浩 劉暢

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

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

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

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

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

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

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

再加上系統的觀測方程：

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

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

首先我們對系統進行預測，得到預測結果。

利用系統的過程模型，預測下一狀態的系統。假設現在的系統狀態是k，依據系統的上一狀態（即：k-1）和系統模型預測現在的狀態：

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

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

誤差的協方差預測方程為：

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

其中，p（k|k-1）是x（k|k-1）的協方差，p（k-1|k-1）是x（k-1|k-1）的協方差，A是A的轉置矩陣，Q是系統噪聲的協方差。

然后我們收集現在狀態的觀測值，結合上述的預測結果，可以得到現在狀態（k）的最優化估算值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）

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

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

其中，I為單位矩陣，對于單模型單測量，I=1。

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

2???? 建立數學模型

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

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

x（k）=y（k-1）

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

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

根據公式（3）可以得出濾波后的最優估算方程：

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

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

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

根據公式（5）可以得出k狀態下的方差方程：

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

根據上述建立的數學模型，給定初始的預測值x（0）、方差p（0）以及一組觀測數據z，即可實現對該組測量數據的卡爾曼濾波。

3???? Matlab仿真

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

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

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

4結束語

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

參考文獻：

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

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