1．卡爾曼濾波器的來(lái)源

卡爾曼全名Rudolf Emil Kalman，匈牙利數(shù)學(xué)家，1930年出生于匈牙利首都布達(dá)佩斯。1953，1954年于麻省理工學(xué)院分別獲得電機(jī)工程學(xué)士及碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。卡爾曼濾波器源于他的博士論文和1960年發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems(線性濾波與預(yù)測(cè)問(wèn)題的新方法)?？柭鼮V波器是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法。對(duì)于解決很多問(wèn)題，它是最優(yōu)，效率最高甚至是最有用的。

2.基礎(chǔ)數(shù)學(xué)知識(shí)準(zhǔn)備

概率論里，每一個(gè)隨機(jī)變量X都是具有一個(gè)平均值（這個(gè)值在變量的概率密度函數(shù)分布圖的最中心位置，代表該數(shù)值是最可能發(fā)生的）和（這個(gè)數(shù)值代表方差，表示變量的不確定性程度）。平均值μ就是數(shù)學(xué)期望μ=E(X)。方差D(X)=σ^2=E{[x-E(x)]^2}是衡量隨機(jī)變量或一組數(shù)據(jù)時(shí)離散程度的度量。方差的平方根即標(biāo)準(zhǔn)差或者均方差，即σ。

一談到概率統(tǒng)計(jì)，馬上可以想到：兩個(gè)變量是否相互獨(dú)立，或者互不相關(guān)。相互獨(dú)立是已知其中一個(gè)變量的變換規(guī)律，我們無(wú)法推斷出另外一個(gè)變量的變化規(guī)律，否者就相互不獨(dú)立。例如，根據(jù)牛頓公式測(cè)量的速度和位移隨機(jī)變量就是相關(guān)的。那么相關(guān)的程度我們可以用協(xié)方差來(lái)描述，數(shù)學(xué)表述為：

人們總結(jié)隨機(jī)變量的概率分布，可得到一些典型的分布，比如均勻分布、泊松分布和高斯分布等。其中高斯分布，也稱正態(tài)分布，又稱常態(tài)分布，是我們常見(jiàn)的分布，記為N（μ，σ^2），其中μ，σ^2為分布的參數(shù)，分別為高斯分布的期望和方差。當(dāng)有這兩者確定值時(shí)，概率p(x)也就確定了，特別當(dāng)μ=0，σ^2=1時(shí)，X的分布為標(biāo)準(zhǔn)正態(tài)分布。

我們考察單隨機(jī)變量的高斯分布，其概率密度函數(shù)為：

如果存在兩個(gè)不同的高斯分布，當(dāng)兩個(gè)分布相乘時(shí)，得到什么結(jié)果？（推導(dǎo)過(guò)程感興趣的童鞋可以看隨機(jī)過(guò)程一書(shū)）：

結(jié)論就是互不相關(guān)的兩正態(tài)分布的乘積仍然是正態(tài)分布，且新正態(tài)分布的均值和方差如下：

以上是單變量概率密度函數(shù)的計(jì)算結(jié)果，如果是多變量的，那么，就變成了協(xié)方差矩陣的形式：

3.卡爾曼濾波的通俗理解

講解了隨機(jī)變量和概率，那么為啥要引入卡爾曼濾波呢?？柭鼮V波適用于估計(jì)一個(gè)由隨機(jī)變量組成的動(dòng)態(tài)系統(tǒng)的最優(yōu)狀態(tài)。即便是觀測(cè)到的系統(tǒng)狀態(tài)參數(shù)含有噪聲，觀測(cè)值不準(zhǔn)確，卡爾曼濾波也能夠完成對(duì)狀態(tài)真實(shí)值的最優(yōu)估計(jì)。

為了更好理解為啥需要卡爾曼濾波，我們還是舉個(gè)實(shí)際的例子說(shuō)明。

考慮軌道上的一個(gè)小車(chē)，無(wú)外力作用。為了便于對(duì)它進(jìn)行控制，需要知道它的位置以及移動(dòng)速度。所以，建立一個(gè)向量，用來(lái)存儲(chǔ)小車(chē)的位置和速度：x(t)=(p,v)。其實(shí)，一個(gè)系統(tǒng)的狀態(tài)有很多，選擇最關(guān)心的狀態(tài)來(lái)建立這個(gè)狀態(tài)向量是很重要的。例如，狀態(tài)還有水庫(kù)里面水位的高低、煉鋼廠高爐內(nèi)的溫度、平板電腦上面指尖觸碰屏幕的位置等等這些需要持續(xù)跟蹤的物理量。對(duì)于小車(chē)我們就采用位置和速度來(lái)描述，它在時(shí)刻t的狀態(tài)向量x(t)只與x(t-1)相關(guān)：比如：x(0)表示小車(chē)在t=0時(shí)刻的狀態(tài)是：速度大小5m/s,速度方向右,位置坐標(biāo)0。反正有了這個(gè)向量就可以完全預(yù)測(cè)t=1時(shí)刻小車(chē)的狀態(tài)。

因?yàn)閄(t)=Fx(t-1)，那么根據(jù)t=0時(shí)刻的初值x(0)，理論上我們可以求出它任意時(shí)刻的狀態(tài)。當(dāng)然，實(shí)際情況不會(huì)這么美好。這個(gè)遞推函數(shù)可能會(huì)受到各種不確定因素的影響（內(nèi)在的，小車(chē)結(jié)構(gòu)不緊密，輪子不圓等，外在的，比如刮風(fēng)，下雨，地震等）導(dǎo)致x(t)并不能精確標(biāo)識(shí)小車(chē)實(shí)際的狀態(tài)。我們假設(shè)每個(gè)狀態(tài)分量受到的不確定因素都服從正態(tài)分布。

如上所述，狀態(tài)向量是有維度的。其實(shí)如果是一維的卡爾曼濾波很容易說(shuō)清楚，不比最小二乘法復(fù)雜多少。如果涉及多維就沒(méi)有那么容易理解，因?yàn)楸怀橄鬄榫仃嚨男问?。?duì)，就是矩陣，一個(gè)和線性代數(shù)相關(guān)的概率，如需要了解多維卡爾曼濾波，請(qǐng)大家自行腦補(bǔ)。

我們首先看最簡(jiǎn)單的一維情況：假設(shè)小車(chē)速度恒定為2cm/s，我們只用位置表示小車(chē)的狀態(tài)，即只對(duì)小車(chē)位置進(jìn)行估計(jì)。

請(qǐng)看下圖：t=k-1時(shí)小車(chē)的位置服從紅色的正態(tài)分布。假設(shè)位置在21cm，位置誤差為0.3cm，即μ=21，σ=0.3。

根據(jù)小車(chē)的這個(gè)位置，我們可以根據(jù)經(jīng)驗(yàn)，預(yù)測(cè)出t=k時(shí)刻它的位置：位置均值是21+2=23cm，我對(duì)自己預(yù)測(cè)不確定度為0.4cm，那么此時(shí)小車(chē)位置預(yù)測(cè)誤差為0.5cm（0.5是這樣得到的：如果k-1時(shí)刻估算出的最優(yōu)位置的偏差是0.3，你對(duì)自己預(yù)測(cè)的不確定度是0.4，他們平方相加再開(kāi)方，就是0.5）。0.5>0.3，此時(shí)分布變“胖”了，這很好理解——因?yàn)樵谶f推的過(guò)程中又加了一層噪聲（即預(yù)測(cè)不穩(wěn)定度），所以不確定度變大了。

分布變“胖”了，這很好理解——因?yàn)樵谶f推的過(guò)程中又加了一層噪聲，所以不確定度變大了。

為了避免純估計(jì)帶來(lái)的偏差，我們?cè)趖=k時(shí)刻對(duì)小車(chē)的位置坐標(biāo)進(jìn)行一次雷達(dá)測(cè)量，當(dāng)然雷達(dá)對(duì)小車(chē)距離的測(cè)量也會(huì)受到種種因素的影響，會(huì)帶來(lái)測(cè)量誤差，這個(gè)測(cè)量誤差的均方差是0.4cm，假設(shè)此時(shí)測(cè)量值是25cm(即均值為25)。而且測(cè)量結(jié)果也復(fù)合正態(tài)分布，于是可以畫(huà)出小車(chē)在t=1時(shí)的位置服從藍(lán)色分布。

好了，現(xiàn)在我們得到兩個(gè)不同的結(jié)果。那么哪個(gè)才是更加可靠的小車(chē)現(xiàn)在的位置呢？有人提過(guò)加權(quán)，那么怎么加權(quán)才科學(xué)呢？

大家讀到這里心里非常清楚的一點(diǎn)是：兩個(gè)事件的發(fā)生都是概率性的，不能完全相信其中的任何一個(gè)！如果我們具有兩個(gè)事件，如果都發(fā)生的話，從直覺(jué)或者是理性思維上講，是不是認(rèn)定兩個(gè)事件發(fā)生就找到了那個(gè)最理想的估計(jì)值？好了，抽象一下，得到：兩個(gè)事件同時(shí)發(fā)生的可能性越大，我們?cè)较嘈潘?！要想考察它們同時(shí)發(fā)生的可能性，就是將兩個(gè)事件單獨(dú)發(fā)生的概率相乘。于是乎，卡爾曼濾波的思想誕生了！Kalman老先生的牛逼之處就在于找到了相應(yīng)權(quán)值，使紅藍(lán)分布合并為下圖這個(gè)綠色的正態(tài)分布。

你問(wèn)我kalman為什么牛逼？那是因?yàn)槿思以诶碚撋献C明了：綠色分布不僅保證了在紅藍(lán)給定的條件下，小車(chē)位于該點(diǎn)的概率最大，而且，它居然還是一個(gè)正態(tài)分布！正態(tài)分布就意味著，可以把它當(dāng)做初值繼續(xù)往下算了！這是Kalman濾波能夠迭代的關(guān)鍵。

由于我們用于估算k時(shí)刻的實(shí)際位置有兩個(gè)位置值，分別是23cm和25cm。究竟實(shí)際位置是多少呢？相信自己預(yù)測(cè)還是相信雷達(dá)測(cè)量呢？究竟相信誰(shuí)多一點(diǎn)，我們可以用kalman的方法來(lái)加權(quán)，即利用他們的方差σ^2來(lái)判斷，求出綠色分布均值位置在紅藍(lán)均值間的比例，即Kalman增益Kg。

根據(jù)數(shù)學(xué)上兩正態(tài)分布相乘仍然是正態(tài)分布，由公式(3)得到Kg=0.5^2/(0.5^2+0.4^2)，所以Kg=0.61。（注意kg是方差的概念，不是均方差，網(wǎng)上很多文獻(xiàn)kg用錯(cuò)，開(kāi)了根號(hào)，得到0.78）我們可以估算出k時(shí)刻的實(shí)際位置是：23+0.61*(25-23)=24.22cm，見(jiàn)公式(1)。可以看出，因?yàn)槔走_(dá)的方差比較?。ū容^相信雷達(dá)），所以估算出的最優(yōu)位置值偏向雷達(dá)測(cè)量的值。

現(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)位置值了，下一步就是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒(méi)看到什么自回歸的東西出現(xiàn)。對(duì)了，在進(jìn)入k+1時(shí)刻之前，我們還要算出k時(shí)刻那個(gè)最優(yōu)值（24.22cm）的偏差。算法如下：((1-Kg)*0.5^2)^0.5=0.312，見(jiàn)公式(2)。這里的0.5就是上面的k時(shí)刻你預(yù)測(cè)的那個(gè)23cm位置的均方差，得出的0.312cm就是進(jìn)入k+1時(shí)刻以后k時(shí)刻估算出的最優(yōu)位置值的均方差（對(duì)應(yīng)于上面的0.3cm），于是我們可以把綠色分布當(dāng)做第一張圖中的紅色分布對(duì)t=k+1時(shí)刻進(jìn)行預(yù)測(cè)，算法就可以開(kāi)始循環(huán)往復(fù)了。

說(shuō)明一下，由于上面我們只對(duì)小車(chē)位移這個(gè)一維量（向量）做了估計(jì)，因此Kalman增益是標(biāo)量。如果狀態(tài)向量是多維，那么方差就該變?yōu)閰f(xié)方差，從而引入?yún)f(xié)方差矩陣的迭代，Kalman增益也變成了矩陣。需要采用公式(4)-(6)來(lái)求解Kalman增益矩陣和k時(shí)刻的最優(yōu)值和協(xié)方差矩陣。無(wú)論是多少維的向量空間，上面描述給出了Kalman濾波算法的本質(zhì)就是利用兩個(gè)正態(tài)分布的融合仍是正態(tài)分布這一特性進(jìn)行迭代而已。

4.卡爾曼濾波的嚴(yán)格數(shù)學(xué)描述

有了上述的理解，下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼濾波。

首先，我們先要引入一個(gè)離散控制過(guò)程的系統(tǒng)。該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程（Linear Stochastic Difference equation）來(lái)描述：

X(k)=A X(k-1)+B U(k)+W(k)

再加上系統(tǒng)的測(cè)量值：

Z(k)=H X(k)+V(k)

上兩式子中，X(k)是k時(shí)刻的系統(tǒng)狀態(tài)，U(k)是k時(shí)刻對(duì)系統(tǒng)的控制量，A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，它們?yōu)榫仃嚒６鳽(k)是k時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過(guò)程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲(White Gaussian Noise)，他們的協(xié)方差(covariance)分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。

對(duì)于滿足上面的條件(線性隨機(jī)微分系統(tǒng)，過(guò)程和測(cè)量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們結(jié)合他們的協(xié)方差來(lái)估算系統(tǒng)的最優(yōu)化輸出（類(lèi)似上一節(jié)小車(chē)一維的例子，現(xiàn)在擴(kuò)展到多維）。

首先我們要利用系統(tǒng)的過(guò)程模型，來(lái)預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (7)

式(7)中，X(k|k-1)是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒(méi)有控制量，它可以為0。

到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1)的協(xié)方差還沒(méi)更新。我們用P表示協(xié)方差(covariance)：

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (8)

式(8)中，P(k|k-1)是X(k|k-1)對(duì)應(yīng)的協(xié)方差，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的協(xié)方差，A’表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過(guò)程的協(xié)方差。式子7，8就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)。

現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))……… (9)

其中Kg為卡爾曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)……… (10)

到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要令卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的協(xié)方差：

P(k|k)=（I-Kg(k) H）P(k|k-1) ………(11)

其中I為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(8)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。

5.卡爾曼濾波的編程實(shí)現(xiàn)

卡爾曼濾波器的原理基本描述了，式子8，9，10，11和12就是他的5個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易用計(jì)算機(jī)編程實(shí)現(xiàn)，具體思想如下：

假設(shè)如下一個(gè)系統(tǒng)：

房間內(nèi)連續(xù)兩個(gè)時(shí)刻溫度差值的標(biāo)準(zhǔn)差為0.02度，溫度計(jì)的測(cè)量值誤差的標(biāo)準(zhǔn)差為0.5度，房間溫度的真實(shí)值為24度，對(duì)溫度的初始估計(jì)值為23.5度，誤差的方差為1。

MatLab仿真的代碼如下：

% Kalman filter example of temperaturemeasurement in Matlab

% This M code is modified from Xuchen Yao'smatlab on 2013/4/18

%房間當(dāng)前溫度真實(shí)值為24度，認(rèn)為下一時(shí)刻與當(dāng)前時(shí)刻溫度相同，誤差為0.02度（即認(rèn)為連續(xù)的兩個(gè)時(shí)刻最多變化0.02度）。

%溫度計(jì)的測(cè)量誤差為0.5度。

%開(kāi)始時(shí)，房間溫度的估計(jì)為23.5度，誤差為1度。

clear all;

close all;

% intial parameters

n_iter = 100; %計(jì)算連續(xù)n_iter個(gè)時(shí)刻

sz = [n_iter, 1]; % size of array. n_iter行，1列

x = 24; %溫度的真實(shí)值

Q = 4e-4; %過(guò)程方差，反應(yīng)連續(xù)兩個(gè)時(shí)刻溫度方差。更改查看效果

R = 0.25; %測(cè)量方差，反應(yīng)溫度計(jì)的測(cè)量精度。更改查看效果

z = x + sqrt(R)*randn(sz); % z是溫度計(jì)的測(cè)量結(jié)果，在真實(shí)值的基礎(chǔ)上加上了方差為0.25的高斯噪聲。

%對(duì)數(shù)組進(jìn)行初始化

xhat=zeros(sz); %對(duì)溫度的后驗(yàn)估計(jì)。即在k時(shí)刻，結(jié)合溫度計(jì)當(dāng)前測(cè)量值與k-1時(shí)刻先驗(yàn)估計(jì)，得到的最終估計(jì)值

P=zeros(sz); %后驗(yàn)估計(jì)的方差

xhatminus=zeros(sz); %溫度的先驗(yàn)估計(jì)。即在k-1時(shí)刻，對(duì)k時(shí)刻溫度做出的估計(jì)

Pminus=zeros(sz); %先驗(yàn)估計(jì)的方差

K=zeros(sz); %卡爾曼增益，反應(yīng)了溫度計(jì)測(cè)量結(jié)果與過(guò)程模型（即當(dāng)前時(shí)刻與下一時(shí)刻溫度相同這一模型）的可信程度

% intial guesses

xhat(1) = 23.5; %溫度初始估計(jì)值為23.5度

P(1) =1; %誤差方差為1

for k = 2:n_iter

%時(shí)間更新（預(yù)測(cè)）

xhatminus(k) = xhat(k-1); %用上一時(shí)刻的最優(yōu)估計(jì)值來(lái)作為對(duì)當(dāng)前時(shí)刻的溫度的預(yù)測(cè)

Pminus(k) = P(k-1)+Q; %預(yù)測(cè)的方差為上一時(shí)刻溫度最優(yōu)估計(jì)值的方差與過(guò)程方差之和

%測(cè)量更新（校正）

K(k) = Pminus(k)/( Pminus(k)+R ); %計(jì)算卡爾曼增益

xhat(k) =

xhatminus(k)+K(k)*(z(k)-xhatminus(k)); %結(jié)合當(dāng)前時(shí)刻溫度計(jì)的測(cè)量值，對(duì)上一時(shí)刻的預(yù)測(cè)進(jìn)行校正，得到校正后的最優(yōu)估計(jì)。該估計(jì)具有最小均方差

P(k) = (1-K(k))*Pminus(k); %計(jì)算最終估計(jì)值的方差

end

FontSize=14;

LineWidth=3;

figure();

plot(z,'k+'); %畫(huà)出溫度計(jì)的測(cè)量值

hold on;

plot(xhat,'b-','LineWidth',LineWidth) %畫(huà)出最優(yōu)估計(jì)值

hold on;

plot(x*ones(sz),'g-','LineWidth',LineWidth);

%畫(huà)出真實(shí)值

legend('溫度計(jì)的測(cè)量結(jié)果', '后驗(yàn)估計(jì)', '真實(shí)值');

xl=xlabel('時(shí)間(分鐘)');

yl=ylabel('溫度');

set(xl,'fontsize',FontSize);

set(yl,'fontsize',FontSize);

hold off;

set(gca,'FontSize',FontSize);

figure();

valid_iter = [2:n_iter]; % Pminus not validat step 1

plot(valid_iter,P([valid_iter]),'LineWidth',LineWidth);

%畫(huà)出最優(yōu)估計(jì)值的方差

legend('后驗(yàn)估計(jì)的誤差估計(jì)');

xl=xlabel('時(shí)間(分鐘)');

yl=ylabel('℃^2');

set(xl,'fontsize',FontSize);

set(yl,'fontsize',FontSize);

set(gca,'FontSize',FontSize);

計(jì)算結(jié)果如圖所示：

另外，跨平臺(tái)計(jì)算機(jī)視覺(jué)庫(kù)Opencv目標(biāo)版本中帶有kalman這個(gè)類(lèi)，可以使用它來(lái)完成一些跟蹤目的。下面來(lái)看看使用Kalman編程的主要步驟：

步驟一：

Kalman這個(gè)類(lèi)需要初始化下面變量：

轉(zhuǎn)移矩陣，測(cè)量矩陣，控制向量(沒(méi)有的話，就是0)，過(guò)程噪聲協(xié)方差矩陣，測(cè)量噪聲協(xié)方差矩陣，后驗(yàn)錯(cuò)誤協(xié)方差矩陣，前一狀態(tài)校正后的值，當(dāng)前觀察值。

步驟二：

調(diào)用kalman這個(gè)類(lèi)的predict方法得到狀態(tài)的預(yù)測(cè)值矩陣，預(yù)測(cè)狀態(tài)的計(jì)算公式如下：

predicted state (x'(k)):x'(k)=A*x(k-1)+B*u(k)

其中x(k-1)為前一狀態(tài)的校正值，第一個(gè)循環(huán)中在初始化過(guò)程中已經(jīng)給定了，后面的循環(huán)中Kalman這個(gè)類(lèi)內(nèi)部會(huì)計(jì)算。A,B,u(k),也都是給定了的值。這樣進(jìn)過(guò)計(jì)算就得到了系統(tǒng)狀態(tài)的預(yù)測(cè)值x'(k)了。

步驟三：

調(diào)用kalman這個(gè)類(lèi)的correct方法得到加入觀察值校正后的狀態(tài)變量值矩陣，其公式為：

corrected state (x(k)):x(k)=x'(k)+K(k)*(z(k)-H*x'(k))

其中x'(k)為步驟二算出的結(jié)果，z(k)為當(dāng)前測(cè)量值，是我們外部測(cè)量后輸入的向量。H為Kalman類(lèi)初始化給定的測(cè)量矩陣。K(k)為Kalman增益，其計(jì)算公式為：

Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)

計(jì)算該增益所依賴的變量要么初始化中給定，要么在kalman理論中通過(guò)其它公式可以計(jì)算。

經(jīng)過(guò)步驟三后，我們又重新獲得了這一時(shí)刻的校正值，后面就不斷循環(huán)步驟二和步驟三即可完成Kalman濾波過(guò)程。

6、卡爾曼濾波在自動(dòng)駕駛上的應(yīng)用

①擴(kuò)展卡爾曼濾波定位

這是優(yōu)達(dá)學(xué)員Jeremy寫(xiě)得博文，他向我們介紹了為什么要使用卡爾曼濾波器以及它們是如何工作的。有趣的GIF動(dòng)圖形象的展示了在Lidar和Radar兩種傳感器下，經(jīng)過(guò)卡爾曼濾波之后形成的軌跡。

這是兩套傳感器測(cè)量結(jié)果（黃色）和相關(guān)的地面實(shí)況（藍(lán)色），以及卡爾曼濾波器估算值（綠色）顯示如下：

測(cè)量結(jié)果來(lái)自LiDAR和雷達(dá)測(cè)量。面臨的挑戰(zhàn)是使卡爾曼濾波器解釋這兩種類(lèi)型的測(cè)量，因?yàn)殡m然LiDAR測(cè)量符合我們的狀態(tài)向量的（歐幾里得）參數(shù)，但雷達(dá)測(cè)量使用極坐標(biāo)并需要在坐標(biāo)系之間進(jìn)行轉(zhuǎn)換。

這只是一個(gè)預(yù)測(cè)的循環(huán)（”根據(jù)你之前的動(dòng)作，我希望你在這里 n秒鐘之后）和測(cè)量更新（但我的傳感器認(rèn)為你在這里），從中做出妥協(xié)并且確定新的狀態(tài)向量和協(xié)方差矩陣。

原文鏈接：

http:///2017/04/21/udacity-sdcnd-extended-kalman-filter.html

代碼鏈接：

https://github.com/jeremy-shannon/CarND-Extended-Kalman-Filter-Project

②擴(kuò)展卡爾曼濾波器和C ++中的傳感器融合

該項(xiàng)目涵蓋了很多信息：LIDAR + RADAR傳感器，Python中的卡爾曼濾波器，C ++編程和卡爾曼濾波器數(shù)學(xué)計(jì)算。梅賽德斯 - 奔馳的傳感器融合工程師Andrei Vatavu在解釋各種流程管道公式和變換方面做得非常出色，包括推導(dǎo)。他提供的卡爾曼跟蹤查看器是一個(gè)了不起的工具，可以幫助可視化模型的性能。

使用Catch將行為驅(qū)動(dòng)開(kāi)發(fā)（BDD）測(cè)試添加到項(xiàng)目中。

原文鏈接：

https:///@andrew.d.wilkie/self-driving-car-engineer-diary-8-b17b009497a1

代碼鏈接：

https://github.com/catchorg/Catch2/issues/886

③使用擴(kuò)展卡爾曼濾波算法的傳感器融合和目標(biāo)跟蹤 

如果對(duì)驅(qū)動(dòng)擴(kuò)展卡爾曼濾波器的所有向量和矩陣背后的數(shù)學(xué)感興趣，這篇博文是一個(gè)很好的內(nèi)容。

文中提到“激光雷達(dá)可以測(cè)量附近物體的距離，可以很容易地轉(zhuǎn)換成笛卡爾坐標(biāo)(px, py) 。雷達(dá)傳感器可以(drho)使用稱為多普勒效應(yīng)的東西來(lái)測(cè)量其視線內(nèi)的速度。它還可以測(cè)量附近物體的距離，可以很容易地將其轉(zhuǎn)換為極坐標(biāo)，(rho, phi)但分辨率低于激光雷達(dá)?！?/span>

該算法是一個(gè)遞歸兩步過(guò)程：預(yù)測(cè)和更新。該預(yù)測(cè)步驟產(chǎn)生電流變量的估值與他們一起的不確定性。這些估計(jì)值是根據(jù)估計(jì)如何隨時(shí)間變化的假設(shè)模型。所述更新步驟的下一個(gè)測(cè)量時(shí)完成（受噪聲）被觀察到。在這一步中，根據(jù)預(yù)測(cè)的狀態(tài)和基于當(dāng)前測(cè)量的狀態(tài)的加權(quán)平均值來(lái)更新估計(jì)值（讓我們從這里開(kāi)始稱之為狀態(tài)）。較低的權(quán)重給予具有較高不確定性的權(quán)重。

原文鏈接：

https:///@mithi/object-tracking-and-fusing-sensor-measurements-using-the-extended-kalman-filter-algorithm-part-1-f2158ef1e4f0

④Kalman Filter, Extended Kalman Filter, Unscented Kalman Filter

本篇博文作者Alena對(duì)Kalman Filter, Extended Kalman Filter, Unscented Kalman Filter之間差異的細(xì)分非常好。她認(rèn)為：“在非線性變換的情況下，EKF可以給出好的結(jié)果，對(duì)于高度非線性的變換，最好使用UKF?！?/span>

原文鏈接：

https:///@kastsiukavets.alena/kalman-filter-extended-kalman-filter-unscented-kalman-filter-dbbd929f83c5

⑤卡爾曼濾波器：預(yù)測(cè)，測(cè)量，更新，重復(fù)

本篇博文作者Joshua，將卡爾曼濾波器從最高級(jí)的直覺(jué)，數(shù)學(xué)理論，一直敘述到算法實(shí)現(xiàn)。

卡爾曼濾波算法可以粗略地按以下步驟組織：
1.我們根據(jù)以前的一些值和模型預(yù)測(cè)一個(gè)狀態(tài)。
我們從傳感器獲得該狀態(tài)的測(cè)量結(jié)果。
3.根據(jù)我們的錯(cuò)誤更新我們的預(yù)測(cè)
4.重復(fù)。

原文鏈接：

https:///self-driving-cars/all-about-kalman-filters-8924abe3aa88

本文圖片來(lái)源及參考文獻(xiàn)：

http://www.cs./~welch/kalman/media/pdf/Kalman1960.pdf

http://www.cs./~welch/kalman/

http://www.cl./~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf

http://baike.baidu.com/link?url=WxwHmxXgxUD8gYuaJEFWOuLjTd-swZg6EYvnboM7Og2sU_vKUhlVBGLxD3BG0Ne-uHt1eoOVKekge1UoYX1XT8HJJ-_J4ktxbV-gx1dYgx3ctrlosvivE0ZjM33PxcKgqlXiny51AhN-C_zs4afgGK

https://www.zhihu.com/question/23971601/answer/137325095

https://zhuanlan.zhihu.com/p/25598462

https://www.jianshu.com/p/d3b1c3d307e0utm_campaign=maleskine&utm_content=note&utm_medium=seo_notes&utm_source=recommendation