卡爾曼濾波器是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。而且由于觀測包含系統(tǒng)的噪聲和干擾的影響，所以最優(yōu)估計也可看做是濾波過程。

1 卡爾曼濾波的原理與理解

1.1 預測

假設有一輛小車，其在t時刻的位置為??(假設其在一維直線上運動，則位置可以用數(shù)軸上的點表示)，速度為??。

因此在t時刻小車的狀態(tài)可用向量表示為??。

但是我們并沒有捕捉到一切信息，可能存在外部因素會對系統(tǒng)進行控制，帶來一些與系統(tǒng)自身狀態(tài)沒有相關性的改變。如汽車司機可能會操縱油門，讓汽車加速。

假設由于油門的設置或控制命令，我們知道了期望的加速度為??(加速度理解為外部的控制量)，則可由運動學公式從t-1時刻推出其在t時刻的速度與位置如下:

進一步的可以將其寫成向量形式:

即:

令

則通過變量代換可以得到狀態(tài)轉移公式:

其中：

矩陣??為狀態(tài)轉移矩陣，表示如何從上一狀態(tài)來推測當前時刻的狀態(tài)；

?為控制矩陣，表示控制量??如何作用于當前矩陣；

?有頂帽子，表示只是估計值，并不是最優(yōu)的。

到這里，我們已經(jīng)得到了t時刻的狀態(tài)預測，但這里只有狀態(tài)預測的均值（即）。我們是基于高斯分布來建立狀態(tài)變量的，因此還需要協(xié)方差。（即一個多維高斯分布由均值和協(xié)方差決定）

假設我們用 P 來表示狀態(tài)協(xié)方差，即

那么加入狀態(tài)轉換矩陣F后有

協(xié)方差矩陣的性質有:cov(Ax,Bx)=A*cov(x,x)*B'

如果這些狀態(tài)量是基于系統(tǒng)自身的屬性或者已知的外部控制作用來變化的，則不會出現(xiàn)什么問題。但是，如果存在外部未知的干擾呢？

例如，輪子可能會打滑，或者路面上的坡會讓車減速。這樣的話我們就不能繼續(xù)對這些狀態(tài)進行跟蹤，如果沒有把這些外部干擾考慮在內，我們的預測就會出現(xiàn)偏差。

　　在每次預測之后，我們可以添加一些新的不確定性來建立這種與“外界”（即我們沒有跟蹤的干擾）之間的不確定性模型。我們將這些沒有被跟蹤的干擾當作協(xié)方差為??的噪聲來處理。

狀態(tài)協(xié)方差的預測為：

從而，我們得到了狀態(tài)均值和協(xié)方差的預測，即

現(xiàn)在我們得到上面的兩個公式，運用這兩個公式能夠對現(xiàn)在狀態(tài)進行預測。但是預測結果不可能完全正確嘛，肯定有誤差。而恰好，我們可能會有多個傳感器(如測距雷達)來測量系統(tǒng)當前的狀態(tài)，哪個傳感器具體測量的是哪個狀態(tài)變量并不重要，也許一個是測量位置，一個是測量速度，每個傳感器間接地告訴了我們一些狀態(tài)信息，因此我們可以用測量來修正(更新)預測。

1.2 更新

傳感器讀取的數(shù)據(jù)的單位和尺度有可能與我們要跟蹤的狀態(tài)的單位和尺度不一樣，我們用H來表示傳感器的數(shù)據(jù)。

我們可以計算出傳感器讀數(shù)的分布，用之前的表示方法如下式所示

其中H為觀測矩陣，v為觀測噪聲。

卡爾曼濾波的一大優(yōu)點就是能處理傳感器噪聲，換句話說，我們的傳感器或多或少都有點不可靠，并且原始估計中的每個狀態(tài)可以和一定范圍內的傳感器讀數(shù)對應起來。

從測量到的傳感器數(shù)據(jù)中，我們大致能猜到系統(tǒng)當前處于什么狀態(tài)。但是由于存在不確定性，某些狀態(tài)可能比我們得到的讀數(shù)更接近真實狀態(tài)。

我們將這種不確定性（例如：傳感器噪聲）用協(xié)方差??表示，該分布的均值就是我們讀取到的傳感器數(shù)據(jù)，稱之為??。

現(xiàn)在我們有了兩個高斯分布，一個是在預測值附近，一個是在傳感器讀數(shù)附近。我們必須在'預測值'和'傳感器測量值'之間找到最優(yōu)解。我們只需將這兩個高斯分布(前一狀態(tài)的預測以及傳感器的測量)相乘就可以了。

以兩個一維高斯分布為例，其融合高斯分布為：

其中的K稱為卡爾曼增益。

帶入狀態(tài)預測和傳感器測量協(xié)方差可得到:

K的作用：

(1)K權衡預測協(xié)方差P和觀察協(xié)方差矩陣R那個更加重要。相信預測，則殘差的權重??；相信觀察，則殘差權重大（由 K 的表達式可推出這個結論）。

(2)將殘差的表現(xiàn)形式從觀察域轉換到狀態(tài)域（殘差與一個標量，通過K轉換為向量），由狀態(tài) X的更新公式可得到該結論

利用卡爾曼增益，我們可以進行更新操作，即在預測的基礎上，把測量也考慮進去，可以得到：

綜上，我們就得到了卡爾曼濾波的全部方程，重述如下：

2 示例仿真

（1）假設:

傳感器從t=0時刻開始，每秒采集一個位置數(shù)據(jù)，共采集到100個數(shù)據(jù)。且傳感器的測量伴隨著均值為0方差為1的高斯噪聲 Z=(1:2:200)+randn(1,100);

傳感器提供的觀測矩陣為 H=[1,0]

傳感器的觀測噪聲協(xié)方差矩陣為 R=1

初始狀態(tài)為 X=[0;0] 即 [位置;速度]=[0;0]

狀態(tài)協(xié)方差矩陣為 P=[1 0;0 1]

狀態(tài)轉移矩陣為 F=[1 delta(t);0 1]=[1 1;0 1]

外部干擾用狀態(tài)轉移協(xié)方差矩陣為 Q=[0.0001, 0 ; 0, 0.0001]

（2）問題:

這100內汽車的速度和位置估計。

（3）求解釋路：

利用卡爾曼濾波綜合運動學方程計算與傳感器測量。

（4）計算結果:

其中橫坐標表示位置，縱坐標表示速度。

（5）MATLAB代碼: