卡爾曼濾波是什么 卡爾曼濾波與目標(biāo)追蹤技術(shù)分析

一、卡爾曼濾波介紹

1.1 卡爾曼濾波是什么

本節(jié)為卡爾曼濾波，主要講解卡爾曼濾波的具體推導(dǎo)，卡爾曼濾波在行人狀態(tài)估計(jì)中的一個(gè)小例子。

我們通常要對(duì)一些事物的狀態(tài)去做估計(jì)，為什么要做估計(jì)呢？因?yàn)槲覀兺ǔo法精確的知道物體當(dāng)前的狀態(tài)。為了估計(jì)一個(gè)事物的狀態(tài)，我們往往會(huì)去測(cè)量它，但是我們不能完全相信我們的測(cè)量，因?yàn)槲覀兊臏y(cè)量是不精準(zhǔn)的，它往往會(huì)存在一定的噪聲，這個(gè)時(shí)候我們就要去估計(jì)我們的狀態(tài)?？柭鼮V波就是一種結(jié)合預(yù)測(cè)（先驗(yàn)分布）和測(cè)量更新（似然）的狀態(tài)估計(jì)算法。

1.2 為什么要學(xué)卡爾曼濾波

卡爾曼濾波以及其擴(kuò)展算法能夠應(yīng)用于目標(biāo)狀態(tài)估計(jì)，如果這個(gè)目標(biāo)是行人，那么就是行人狀態(tài)估計(jì)（或者說行人追蹤），如果這個(gè)目標(biāo)是自身，那么就是車輛自身的追蹤（結(jié)合一些地圖的先驗(yàn)，GPS等數(shù)據(jù)的話就是自身的定位）。在很多的無人駕駛汽車項(xiàng)目中，都能找到卡爾曼濾波的擴(kuò)展算法的身影（比如說EKF，UKF等等）。本節(jié)我們從最簡單的卡爾曼濾波出發(fā)，完整的理解一遍卡爾曼濾波的推導(dǎo)過程，并實(shí)現(xiàn)一個(gè)簡單的狀態(tài)估計(jì)Python程序。

1.3為什么會(huì)叫濾波算法

以一維卡爾曼濾波為例，如果我們單純的相信測(cè)量的信號(hào)，那么這個(gè)信號(hào)是包含噪聲的，是很毛糙的，但是當(dāng)我們運(yùn)行卡爾曼濾波算法去做估計(jì)，我們估計(jì)的信號(hào)會(huì)很光滑，看起來似乎濾掉了噪聲的影響，所以稱之為濾波算法。實(shí)際上，卡爾曼濾波不僅僅過濾掉了測(cè)量信號(hào)的噪聲，它同時(shí)也結(jié)合了以往的估計(jì)，卡爾曼濾波在線性問題中被證明是最優(yōu)估計(jì)。

1.4一些概率論的基礎(chǔ)知識(shí)

下面是一些概率論的基礎(chǔ)知識(shí)，如果之前有這方面的知識(shí)儲(chǔ)備那當(dāng)然是最好的，很有利于我們理解整個(gè)博客內(nèi)容，如果沒有這方面的基礎(chǔ)而且也看不懂下面的內(nèi)容也沒關(guān)系，我會(huì)以一個(gè)相對(duì)直觀的方式來展現(xiàn)整個(gè)理論部分。

先驗(yàn)概率 P (X)：僅僅依賴主觀上的經(jīng)驗(yàn)，事先根據(jù)已有的只是的推斷

后驗(yàn)概率 P (X∣Z)：是在相關(guān)證據(jù)或者背景給定并納入考慮以后的條件概率

似然 P (Z∣X)：已知結(jié)果區(qū)推測(cè)固有性質(zhì)的可能性

貝葉斯公式：

后驗(yàn)分布正比于先驗(yàn)分布乘以似然。

二、卡爾曼濾波的完整推導(dǎo)

2.1 一個(gè)簡單例子

若干年后，我們的可回收火箭要降落到地球，我們比較關(guān)心的狀態(tài)就是我們的飛行器的高度了，飛行器的高度就是我們想要估計(jì)的狀態(tài)，我們會(huì)通過一些傳感器去測(cè)量我們當(dāng)前的高度信息，比如說使用氣壓計(jì)。假如我們每一次測(cè)量，當(dāng)前高度都變成上一次測(cè)量的95%，那么我們就可以得到如下關(guān)系：

我們可以使用遞歸來表示這樣的公式——為了計(jì)算我們當(dāng)前的高度，我們必須知道我們上一個(gè)測(cè)量的高度，以此遞推，我們就會(huì)推到我們的飛行器的初始高度。

但是，由于我們的測(cè)量往往來自于一些傳感器（比如說GPS，氣壓計(jì)），所以測(cè)量的結(jié)果總是帶有噪聲的，這個(gè)噪聲是由傳感器本身引起的，那么我們的表達(dá)式就變成了：

這個(gè)噪聲我們稱之為測(cè)量噪聲 (Measurement Noise)。通常來說，這種噪聲都滿足高斯分布。我們用符號(hào)描述以上兩部分：

第一個(gè)式子是我們的過程模型，是我們的經(jīng)驗(yàn)（比如說一些運(yùn)動(dòng)模型，牛頓力學(xué)等等），我們用這種過程模型去 預(yù)測(cè) 我們考察的事物狀態(tài)（在沒有任何信息的情況下，或者說在沒有任何測(cè)量數(shù)據(jù)的情況下）；第二個(gè)式子是測(cè)量的表達(dá)式，它大致描述了我們測(cè)量的組成成分，我們用這個(gè)測(cè)量去更新我們對(duì)狀態(tài)的估計(jì)。其中是我們的飛行器當(dāng)前的狀態(tài)，是我們上一個(gè)狀態(tài)（注意狀態(tài)和測(cè)量的區(qū)別），是我們當(dāng)前對(duì)飛行器的測(cè)量，是我們當(dāng)前的測(cè)量噪聲， a是一個(gè)常數(shù)，在我們這個(gè)例子里面就是 0.95。

思考一下，在這里我們用一個(gè)簡單的比例（常數(shù)）來描述“可回收火箭”的運(yùn)動(dòng)規(guī)律，會(huì)存在什么問題？

很顯然，現(xiàn)實(shí)中的運(yùn)動(dòng)不會(huì)像我們這個(gè)簡單的過程模型一樣高度按比例縮小。在這里為了簡化，我們先假設(shè)我們的模型挺好的，能夠大致描述出這顆“神奇火箭”的運(yùn)動(dòng)規(guī)律，只是偶爾會(huì)存在一定的偏差（比如所空氣湍流的影響），那么我們?cè)谟?jì)算時(shí)再加一個(gè)噪聲來描述我們的過程模型與實(shí)際運(yùn)動(dòng)的差異，這個(gè)噪聲我們稱之為過程噪聲 (Process Noise)，我們用來表示這種噪聲，那么 的計(jì)算公式就變成：

為了簡化，后面的分析我們會(huì)先忽略過程噪聲，但是我們?cè)趥鞲衅魅诤系牟糠謺?huì)將過程噪聲重新考慮進(jìn)來。

2.2 狀態(tài)估計(jì)

因?yàn)槲覀兪且烙?jì)飛行器的狀態(tài)（高度），所以我們把測(cè)量的公式變換成：

顯然，在這里是沒辦法知道的，卡爾曼濾波通過同時(shí)考慮上一狀態(tài)值和當(dāng)前的測(cè)量值來獲得對(duì)當(dāng)前狀態(tài)值的估計(jì)。一般我們用 來表示對(duì)狀態(tài) x 的估計(jì)。那么 就表示當(dāng)前狀態(tài)的估計(jì)。下面我們可以用如下公式來描述卡爾曼濾波如何結(jié)合上一個(gè)估計(jì)和和現(xiàn)在的測(cè)量來產(chǎn)生對(duì)當(dāng)前的估計(jì)： 這里的叫做 卡爾曼增益（Kalman Gain），它描述的是之前的估計(jì)和當(dāng)前的測(cè)量對(duì)當(dāng)前的估計(jì)的影響的分配權(quán)重。為了理解，我們考慮極端的例子，如果 也就是說增益為 0，那么： 也就是說我們非常不信任我們當(dāng)前的測(cè)量，我們直接保留了我們上一次的估計(jì)作為我們對(duì)當(dāng)前狀態(tài)的估計(jì)。如果 ，即增益為 1，則： 即我認(rèn)為當(dāng)前的測(cè)量非常可信，我徹底接受它作為我當(dāng)前狀態(tài)的估計(jì)。那么當(dāng)介于 ( 0 , 1 ) ，則表示對(duì)兩者的權(quán)重分配。

2.3計(jì)算卡爾曼增益

那么如何計(jì)算卡爾曼增益呢？我們使用一種間接的方法，我們雖然不知道測(cè)量噪聲 的值，但是我們知道它的均值，前面我們提到，測(cè)量噪聲來自傳感器本身，并且符合高斯分布，所以我們能夠從傳感器廠商那里獲得測(cè)量噪聲的均值 ，那么 可以表示為： 其中叫預(yù)測(cè)誤差，表達(dá)式為： 那么怎么理解和 呢？假設(shè)前一次的預(yù)測(cè)誤差，那么根據(jù)公式，當(dāng)前的增益，一維則舍棄掉當(dāng)前的測(cè)量而完全采用上一個(gè)時(shí)刻的估計(jì)，如果，那么增益變成 通常 是個(gè)很小的數(shù)值，所以增益為1，所以完全接受這一次的測(cè)量作為我們的估計(jì)（因?yàn)樯弦淮蔚牡念A(yù)測(cè)誤差太大了，為1，所以一旦拿到了新的測(cè)量，如獲至寶，就干脆把不準(zhǔn)確的上次的估計(jì)舍棄掉了） 對(duì)于下面的公式的分析是一樣的，我們考慮極端的例子，當(dāng)增益為 0 ，，因?yàn)槲覀儚氐咨釛壍袅吮敬蔚臏y(cè)量，所以本次的預(yù)測(cè)誤差只能接受上一次的。當(dāng)增益為 1 ，

2.4預(yù)測(cè)和更新

那么現(xiàn)在我們有關(guān)于當(dāng)前的火箭高度狀態(tài)的兩個(gè)公式了，它們分別是：

那么我們到底要用哪一個(gè)呢？答案是我們都用，第一個(gè)公式我們稱之為預(yù)測(cè)，是基于一些先驗(yàn)的知識(shí)（比如說運(yùn)動(dòng)模型，牛頓力學(xué)等等）覺得我們的狀態(tài)應(yīng)該是這樣的，而第二個(gè)公式呢，就是我們基于我們“不完美的”的傳感器的測(cè)量數(shù)據(jù)來更新我們對(duì)狀態(tài)的估計(jì)。另外，預(yù)測(cè)，理論上只考慮了一個(gè)固定的過程模型和過程噪聲，但是由于我們現(xiàn)在是對(duì)機(jī)械的狀態(tài)進(jìn)行估計(jì)，在預(yù)測(cè)過程中需要對(duì)機(jī)械本身的控制建模， 我們?cè)陬A(yù)測(cè)部分再新增一個(gè)控制信號(hào)，我們用表示。實(shí)際的傳感器測(cè)量除了會(huì)有測(cè)量噪聲 以外，還會(huì)存在一定的關(guān)于真實(shí)狀態(tài)的縮放，因此我們使用 表示測(cè)量時(shí)通常還會(huì)在其前面加一個(gè)縮放系數(shù)。結(jié)合這些我們就可以得到卡爾曼濾波預(yù)測(cè)和更新過程了： 預(yù)測(cè) 卡爾曼濾波更新的過程為： 使用線性代數(shù)的方法來表示預(yù)測(cè)和更新 預(yù)測(cè) 卡爾曼濾波更新的過程為： 至此，卡爾曼濾波的完整推導(dǎo)就結(jié)束了。下面，我們來看看卡爾曼濾波在無人汽車的感知模塊的應(yīng)用。

三、卡爾曼濾波在無人駕駛感知模塊的應(yīng)用

3.1無人車感知模塊的傳感器

無人駕駛汽車要安全的在道路上行駛，需要“耳聽六路，眼觀八方”。那么無人車的耳朵和眼睛是什么呢？那就是安裝在無人車上的各種各樣的傳感器了。無人車上的傳感器能夠多達(dá)幾十個(gè)，而且是不同種類的，比如：

立體攝像機(jī)

交通標(biāo)志攝像機(jī)

雷達(dá)（RADAR）

激光雷達(dá)（LIDAR）立體攝像機(jī)往往用于獲取圖像和距離信息；交通標(biāo)志攝像機(jī)用于交通標(biāo)志的識(shí)別；雷達(dá)一般安裝在車輛的前保險(xiǎn)桿里面，用于測(cè)量相對(duì)于車輛坐標(biāo)系下的物體，可以用來定位，測(cè)距，測(cè)速等等，容易受強(qiáng)反射物體的干擾，通常不用于靜止物體的檢測(cè)；激光雷達(dá)往往安裝在車頂，使用紅外激光束來獲得物體的距離和位置，但是空間分辨率高，但是笨重，容易被天氣影響。

由此可知，各種傳感器都有其優(yōu)點(diǎn)和缺點(diǎn)，在實(shí)際的無人駕駛汽車?yán)铮覀兺Y(jié)合多種傳感器的數(shù)據(jù)去感知我們的車輛周邊的環(huán)境。這種結(jié)合各種傳感器的測(cè)量數(shù)據(jù)的過程我們稱之為傳感器融合（Sensor Fusion）。本系列博客后面的章節(jié)我將詳細(xì)給大家介紹基于擴(kuò)展卡爾曼濾波和無損卡爾曼濾波在傳感器融合中的應(yīng)用。在本節(jié)中，我們主要考慮卡爾曼濾波基于單一的傳感器數(shù)據(jù)來估算行人位置的方法。

3.2基于卡爾曼濾波的行人位置估算

卡爾曼濾波雖然簡單，確實(shí)無人駕駛汽車的技術(shù)樹中非常重要的一部分，當(dāng)然，在真實(shí)的無人駕駛汽車項(xiàng)目中使用到的技術(shù)是相對(duì)更加復(fù)雜的，但是其基本原理仍然是本博客介紹的這些內(nèi)容。在無人駕駛中，卡爾曼濾波主要可以用于一些狀態(tài)的估計(jì)，主要應(yīng)用于行人，自行車以及其他汽車的狀態(tài)估計(jì)。下面，我們以行人的狀態(tài)估計(jì)為例展開。當(dāng)我們要估計(jì)一個(gè)行人的狀態(tài)時(shí)，首先就是要建立起腰估計(jì)的狀態(tài)的表示。這里，行人的狀態(tài)大致可以表示為， 其中為行人的位置，而則是行人此時(shí)的速度。我們用一個(gè)向量來表示一個(gè)狀態(tài)： 在確定了我們要估計(jì)的狀態(tài)以后，我們需要確定一個(gè)過程模型，如前文所說，過程模型用于預(yù)測(cè)階段來產(chǎn)生一個(gè)對(duì)當(dāng)前估計(jì)的先驗(yàn)。在本文中，我們先以一個(gè)最簡單的過程模型來描述行人的運(yùn)動(dòng)——恒定速度模型： 即：

之所以稱之為恒定速度模型，是因?yàn)槲覀儗⑸厦孢@個(gè)行列式展開可以得到：

恒定速度過程模型假定預(yù)測(cè)的目標(biāo)的運(yùn)動(dòng)規(guī)律是恒定的速度的，在行人狀態(tài)預(yù)測(cè)這個(gè)問題中，很顯然行人并不一定會(huì)以恒定的速度運(yùn)動(dòng)，所以我們的過程模型包含了一定的 過程噪聲，在這個(gè)問題中過程噪聲也被考慮了進(jìn)來，其中的是我們這個(gè)過程模型的過程噪聲。在行人狀態(tài)估計(jì)中的過程噪聲其實(shí)就是行人的加減速，那么我們?cè)瓉淼奶幚磉^程就變成了：

我們預(yù)測(cè)的第二步就變成了：

這就是的更新過程，它本質(zhì)上是我們的估計(jì)狀態(tài)概率分布的協(xié)方差矩陣。是我們的過程噪聲的協(xié)方差矩陣，由于過程噪聲是隨機(jī)帶入的，所以本質(zhì)是一個(gè)高斯分布：， 是過程噪聲的協(xié)方差， 的形式為：

進(jìn)而表示為：

其中， 對(duì)于行人我們可以設(shè)置為（這一部分大家如果想要詳細(xì)了解可以閱讀這篇文章：http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5940526）。

測(cè)量步驟中，我們直接可以測(cè)量到速度和，所以我們的測(cè)量矩陣可以表示為：

測(cè)量噪聲的協(xié)方差矩陣 為：

其中的 和 描述了我們的傳感器的測(cè)量能夠有多“差”，這是傳感器固有的性質(zhì)，所以往往是傳感器廠商提供。

最后，在測(cè)量的最后一步，我們使用單位矩陣來代替原式中的1:

至此，基于恒定速度過程模型卡爾曼濾波的行人狀態(tài)估計(jì)的整個(gè)流程我們就講完了，下面我們使用Python將整個(gè)過程實(shí)現(xiàn)一下。

四、卡爾曼濾波在行人狀態(tài)估計(jì)中的Python例子

首先我們看一下卡爾曼濾波的整個(gè)流程，其實(shí)在實(shí)際的論文和資料中，預(yù)測(cè)矩陣通常使用來表示（我們前文中一直是使用）,測(cè)量矩陣通常使用 表示（我們前文中使用的是）,卡爾曼增益通常使用來表示（我們前文中使用的是）,下面是文獻(xiàn)材料中通常意義的卡爾曼濾波過程：

注意：公式還包含一項(xiàng)： ,這一項(xiàng)是指我們?cè)谧粉櫼粋€(gè)物體的狀態(tài)的時(shí)候把它內(nèi)部的控制也考慮進(jìn)去了，這在行人，自行車，其他汽車的狀態(tài)估計(jì)問題中是無法測(cè)量的，所以在這個(gè)問題中我們?cè)O(shè)置 為 0 .

我們的代碼將在IPython中執(zhí)行，大家可以在jupyter notebook中交互式地運(yùn)行。

首先我們載入必要的庫：

import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
fromscipy.statsimportnorm

接著我們初始化行人狀態(tài)x, 行人的不確定性（協(xié)方差矩陣）P，測(cè)量的時(shí)間間隔dt，處理矩陣F以及測(cè)量矩陣H：

x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
print(x, x.shape)
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
print(P, P.shape)


dt = 0.1 # Time Step between Filter Steps
F = np.matrix([[1.0, 0.0, dt, 0.0],
              [0.0, 1.0, 0.0, dt],
              [0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(F, F.shape)


H = np.matrix([[0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(H, H.shape)


ra = 10.0**2


R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R,R.shape)

計(jì)算測(cè)量過程的噪聲的協(xié)方差矩陣R和過程噪聲的協(xié)方差矩陣Q：

ra = 0.09
R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R, R.shape)


sv = 0.5
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2
from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('dt')
Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts]])
Qs*Qs.T

定義一個(gè)單位矩陣：

I = np.eye(4)
print(I,I.shape)

我們隨機(jī)產(chǎn)生一些測(cè)量數(shù)據(jù)：

m = 200 # Measurements
vx= 20 # in X
vy= 10 # in Y


mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my))


print(measurements.shape)
print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx))
print('You assumed %.2f in R.' % R[0,0])


fig = plt.figure(figsize=(16,5))
plt.step(range(m),mx, label='$dot x$')
plt.step(range(m),my, label='$dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})

一些過程值，用于結(jié)果的顯示：

xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []


def savestates(x, Z, P, R, K):
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Rdx.append(float(R[0,0]))
    Rdy.append(float(R[1,1]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
Kdy.append(float(K[3,0]))

卡爾曼濾波：

for n in range(len(measurements[0])):
 
    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = F*x
    
    # Project the error covariance ahead
    P = F*P*F.T + Q
    
    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)
    
    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)
    
    # Update the error covariance
    P = (I - (K*H))*P
    
    # Save states (for Plotting)
savestates(x,Z,P,R,K)

顯示一下關(guān)于速度的估計(jì)結(jié)果：

def plot_x():
    fig = plt.figure(figsize=(16,9))
    plt.step(range(len(measurements[0])),dxt, label='$estimateVx$')
    plt.step(range(len(measurements[0])),dyt, label='$estimateVy$')
    
    plt.step(range(len(measurements[0])),measurements[0], label='$measurementVx$')
    plt.step(range(len(measurements[0])),measurements[1], label='$measurementVy$')


    plt.axhline(vx, color='#999999', label='$trueVx$')
    plt.axhline(vy, color='#999999', label='$trueVy$')


    plt.xlabel('Filter Step')
    plt.title('Estimate (Elements from State Vector $x$)')
    plt.legend(loc='best',prop={'size':11})
    plt.ylim([0, 30])
    plt.ylabel('Velocity')
plot_x()

預(yù)測(cè)的結(jié)果如圖所示：

位置的估計(jì)結(jié)果：

def plot_xy():
    fig = plt.figure(figsize=(16,16))
    plt.scatter(xt,yt, s=20, label='State', c='k')
    plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
    plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')


    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Position')
    plt.legend(loc='best')
    plt.axis('equal')
plot_xy()

預(yù)測(cè)的結(jié)果如圖所示：

雖然本例中以無人駕駛的行人檢測(cè)作為落腳點(diǎn)講解卡爾曼濾波，但是實(shí)際上我們的無人車并不會(huì)僅僅使用原始的卡爾曼濾波作為行人感知的狀態(tài)估計(jì)，因?yàn)榭柭鼮V波存在著一個(gè)非常大的局限性——它僅能對(duì)線性的過程模型和測(cè)量模型進(jìn)行精確的估計(jì)，在非線性的場景中并不能達(dá)到最優(yōu)的估計(jì)效果，為了能夠設(shè)定線性的環(huán)境，我們假定了我們的過程模型為恒定速度模型，但是顯然在顯示的應(yīng)用中不是這樣的，不論是過程模型還是測(cè)量模型，都是非線性的，下一篇我將介紹一種能夠應(yīng)用于非線性情況的卡爾曼濾波算法——擴(kuò)展卡爾曼濾波。

審核編輯：黃飛