【自動駕駛】卡爾曼濾波直觀理解、數學公式及代碼理解

直觀理解

你住在深圳，你的好朋友小明住在烏魯木齊。有一天，小明打算來找你玩，你想預訂一家五星級餐館，在他到深圳的當天晚上和他一起喫晚飯。但是小明是一名戶外運動愛好者，他偏偏選擇走路來深圳，你打開地圖發現臥槽，這走路得走好幾十天。

然而五星級餐廳如果不提前幾天預定好話，當天是沒有位置的。所以你問小明，哪一天才能到？然而他卻沒有正面回覆，而是告訴你他從烏魯木齊市中心出發以及他的速度。

烏魯木齊到深圳全程約4275.3公里，這樣的話四天多就能到了！但是根據你對小明體能的瞭解，你覺得這個估算不太靠譜。而且他路上還會經過雪嶺、草嶺、荒漠、戈壁各種地形，還有各種天氣的原因，他很難保證每天100公里的前進速度。

於是你想了個辦法，你先假設小明速度每天100km，然後每天晚上讓他發個定位，告訴你他的位置在哪，然後你可以根據他的定位更新他的數學模型，預測之後的位置。

但是有個新的問題，就是他的GPS定位不是很準，可能是有十幾公里的偏差，因此你也不能完全相信他的定位。

最後你決定兩手抓，既相信自己的建立的數學模型，同時也參考GPS的定位，綜合兩者的結論來預測小明的位置。

例如，出發後第1天，根據你的數學模型，小明此時應該已經到了達坂城古鎮，然而根據他發來的GPS定位顯示還沒走出市區，今天只走了10km。顯然，你認爲今天GPS的可信度高於自己的數學模型。

因此你綜合“自己的數學模型”和“GPS信息”，得到一個新的數學模型——你認爲小明目前的真正位置應該在距離市中心15km的地方，他的速度爲每天12km，因此他第二天的位置在距離市中心27km的地方（這些數字是隨便取得，大家看一看數字的變化就好）

之後的第三天、第四天、第N天你都採用上面的方法，隨着你不斷修正自己的數學模型，你的預測也會越來越準確，上面就是卡爾曼濾波的思想。

 

數學公式

接下來我們認真看一下卡爾曼濾波的公式，分爲Station prediction（狀態估計）與Measurement update（測量更新）兩部分：

 

Station prediction

（1）公式一

在狀態估計階段，我們要估計物體的狀態有位置$p_x$和速度$v_x$，因此狀態$x$可由向量的形式表示爲：

若第N-1時刻的狀態用$x$表示（即估計前），第N時刻的狀態用$x'$表示（即估計後），則$x'$可寫作：

其中，$F$爲狀態轉移矩陣，即表示第N時刻狀態與第N-1時刻狀態的關係：

這個很好理解，用高中物理知識就能明白，矩陣$F$與$x$相稱後含義即爲：

1. 第N時刻的位置（即$x'$中的$p_x$）= 第N-1時刻的位置（即$x$中的$p_x$）+ 第N-1時刻的速度（即$x$中的$v_x$）*時間間隔（即$F$中的$\Delta t$）；
2. 第N時刻的速度（即$x'$中的$v_x$）= 第N-1時刻的速度（即$x$中的$v_x$）（這裏認爲物體時勻速的）。

$B$輸入增益矩陣，$u$表示輸入向量，$B*u$表示有外部力量對物體施加力的作用。

同時，由於物體運動過程中存在噪聲，我們認爲該過程噪聲爲一個均值爲0，協方差矩陣爲Q的正態分佈。

所以最終得到第一個公式：

 

（2）公式二

$P$表示協方差矩陣，即表示狀態的不確定性，$F$爲上面中提到的狀態轉移矩陣，$Q$表示狀態轉移中的噪聲(也就是上面的$v$中的$Q$)。

協方差矩陣就是由協方差組成的矩陣。協方差類似於方差，只不過方差是隻針對一個變量而言，協方差是一個升級版方差，可以用於多個變量，並且還可以得知不同變量之間的相關性。
由於在這裏我們有兩個變量（即位置和速度），因此$P$是一個2*2的矩陣，即：

1. $cov(p,p)$ 表示對於位置的方差，也可以理解爲不確定性，因爲若方差越小，則位置的確定性越大；
2. $cov(p,p)$ 表示對於位置與速度的協方差；
3. $cov(p,p)$ 表示對於速度與位置的協方差，實際與$cov(p,p)$ 的值是一樣的，$P$是一個對角陣；
4. $cov(v,v)$ 表示對於位置的方差。

通過P可以反映出我們模型的對於估計的確定性程度。

若不考慮$Q$，則表示“通過第N-1時刻的$P$求得傳遞之後第N時刻的$P$”，但是由於模型在傳遞過程中也是會存在噪聲的，也就是相當於被估計物體移動這個動作本身就具有不確定性（即加速、減速或改變方向），因此考慮加入狀態轉移噪聲$Q$。代表了每一次運動後，不確定性都會增加。
 

（3）$Q$與$v$的關係

前面提到，$Q$與$v$滿足下列關係，那麼這到底是什麼意思呢，爲什麼二者有關聯，這個問題一開始也困擾了我很久，下面說說我的看法。

首先，我們先確定兩者的維度，由矩陣加減法可知，矩陣$Q$必然是2*2的，$v$是一個二維的列向量。
我一開始疑惑，爲什麼2*2的$Q$最後會變成一個二維的列向量？ Q不是與協方差矩陣有關嗎？那麼Q裏面的四個元素就應該分別爲下面四個（即與$P$應該是一一對應的）：

1. 位置的方差的誤差；
2. 位置與速度的協方差的誤差；
3. 速度與位置的協方差的誤差；
4. 位置的方差的誤差。

而$v$若要和狀態$x$相加，則$v$內兩個元素必然一個代表位置，一個代表速度，那不就丟失了2. 位置與速度的協方差的誤差和3. 速度與位置的協方差的誤差嗎？

造成這種困惑的原因是，沒有理解分佈的含義，$v\sim N(0，Q)$表示的是數據$v$的分佈是一個協方差爲Q的正太分佈，關鍵詞是分佈，也就是說$v$是從這一堆特定分佈中隨機取出的一個數據。

我們生成一組（1000個）二維的，均值爲0，協方差爲[1 1.5; 1.5 3]，滿足正態分佈的數據，以下爲matlab代碼：

mu = [0 0];
Q = [1 1.5; 1.5 3];
rng('default')  % For reproducibility
V = mvnrnd(mu,Q,1000);
plot(V(:,1),V(:,2),'+')

繪製散點圖觀察分佈，確實是我們想要的正態分佈。
再去看生成的每個樣本（即每一行），其實他們都是二維的。有1000個樣本，每個樣本維度是2，因此是1000*2。

在代碼中，$Q$維度是2*2，$v$的兩個維度對應位置和速度，事實證明：$v\sim N(0，Q)$這個關係是沒有問題的，他們各自的維度也是沒毛病的！

上面的代碼中，我刻意使用了大寫的$V$，而不是小寫，因爲$v$代表的是單個樣本（即單獨一行）。在實際狀態估計的時候，$x'=Fx+Bu+v$中的$v$，就是指從$V$中隨機抽取的一個樣本。

你發現了沒有，這就是分佈的神奇所在！因爲這是一個二維的分佈，所以得到的數據$v$必然是二維的！分佈只與樣本的位置有關，但是當$v$的數量足夠多的時候，宏觀的去看這一組數據的時候，就可以呈現出我們想要滿足一定均值、協方差條件的分佈。

舉一個例子，$Q$代表一個班級，$v$代表班級裏的某一個人，老師要從班級裏面隨機選一個人出去參加跑步（只能選一個），老師只能看到兩個維度：身高、體重。
她選了一個平均身高爲175，平均體重爲60kg，身高方差爲15，體重方差爲20，身體與體重協方差爲10（即身高越高體重也越大）的班級。
最後隨機選出了一個人，這個人身高180，體重70，這個人就是實實在在的$v$。

最後說一下$Q$與$v$的關係：

1. $Q$表示過程噪聲，這些噪聲會影響的是位置、速度、位置與速度的相關性，是從數據的整體分佈的角度去影響；
2. $v$是指滿足上述分佈的具體某一個噪聲，這個噪聲是切切實實的影響位置與速度，是直接加到我們的狀態上的；
3. $Q$與$v$是指同一種噪聲，即過程噪聲，但是$Q$是是從宏觀的角度體現出噪聲分佈的特點，$v$則是具體某一個過程噪聲。
    

Measurement update

（1）公式一

$z$爲第N-1時刻的測量值（因爲只能觀測到速度，因此其含義爲速度值）。$H$表示測量矩陣，因爲我們的$x'$是狀態，裏面既有位置信息也有速度信息，而我們能夠觀測到的只有位置信息，因此需要乘個$H$除去速度信息。

具體怎麼除去呢，其實很簡單，只要令$H=[1,0]$即可，因爲速度對應的權重爲0，所以$H$與$x'$相乘之後就只剩下位置信息了。

當乘以$H$後，$z$與$Hx'$維度就相同了，$z$爲測量得到的實際位置信息（你可以認爲是一種反饋），$Hx'$爲模型估計值（你可以當作一種理論值），因此兩者相減便可得到測量的誤差值$y$，然後用這個誤差$y$去修正我們的模型，以便讓我們的模型在下一次預測中更加的準確。
 

（2）公式二、三

$R$代表測量誤差（即傳感器本身的誤差），$K$爲傳說中的卡爾曼增益，$S$爲一個計算卡爾曼增益的中間值，一般時候也可以把下面兩行直接寫成一行把$S$省略掉。

具體卡爾曼增益的推導請看最後面的P.S.，這裏說下卡爾曼增益$K$的含義。$K$實際是在權衡模型與實際測量到底哪一個更準確。

$H$是常數我們不妨假設$H$爲1，這樣可以將$K$簡化爲：
$K=\frac{P'H^T}{HP'H^T+R}=\frac{P'}{P'+R}=\frac{1}{1+\frac{R}{P'}}$

模型的不確定性對應$P'$，測量的不確定性對應$R$。

1. 若$R>P'$，則說明測量的不確定性更大，此時K較小，更新時偏向於相信模型；
2. 若$R<P'$，則說明模型的不確定性更大，因此K較大，更新時偏向於相信實際測量。
    

（3）公式四、五

在計算得到卡爾曼增益$K$後，便對之前的模型進行修正更新，下面這兩個公式都與$K$有關我們同時看。

前面說到，若$K$變小，則偏向於相信模型，在此也可以得到驗證，不妨假設一種極端情況，$K$=0，也就是$R$比$P'$大很多很多（說明測量的數據相比於模型更不靠譜），那麼根據上面的兩個公式，此時：
$x=x', P=P'$

也就是說對測量的數據完全不理會了，對模型的更新不造成影響，我只相信我原本的模型，完全不相信測量所得的反饋數據。

當$K$慢慢變大的時候，與測量相關的數據（誤差$y$與測量誤差$H$）對於狀態$x$與$P$的更新影響也越大。

P.S.
關於預測方程的具體推導，可以看卡爾曼濾波器推導與解析 - 案例與圖片，非常詳細！
最核心的地方在於：根據高斯函數的性質，得到“兩個舊高斯分佈推出一個新高斯分佈”的公式，然後把“模型”和“傳感器”分別當作就高斯分佈，然後套公式，就可以得到新高斯分佈“更新後的模型”：
即把：

代入到下面的公式中：

然後把H消掉就是預測過程的公式了（以上來張圖片來自卡爾曼濾波器推導與解析 - 案例與圖片）

 

（4）卡爾曼濾波的思路

下面將所有公式串起來，說一下整體思路：

1. 首先我們會有一個被預測物體的數學模型，我們用這個模型去預測物體未來的運動軌跡。
2. 然而這個數學模型是不完美的（運動中存在噪聲模型本身也具有不確定性），因此我們會通過傳感器去採集物體實際的運動數據，與我們的預測值比較，利用誤差來修正我們模型。
3. 但是我們又並不完全相信傳感器的數據，因爲我們傳感器也有誤差。所以我們會比較“傳感器的誤差”和“模型的誤差”看誰比較小。 在更新模型的時候，誤差比較小的一方佔的比比重會更大。
4. 起初數學模型會非常地不準確，通過不斷地循環步驟1~3，不斷地迭代“更新-預測”，就可以逐步得到一個準確地模型

舉一個形象的例子：
有一個科學家提出一個“理論”，但是他知道這個“理論”不一定是完全正確的，有不完美的地方，因此他不斷地做實驗，每天都去根據“實驗結果”更新他原有的理論。
當“實驗結果”出現於“理論”衝突時，他會先思考實驗出錯的概率和理論出錯的概率。

1. 如果“實驗結果”只是偶爾與“理論”衝突，那他可能認爲這更有可能實驗誤差造成的，因爲在他第二天新的“理論”中，會大部分保留前一天的“理論”內容，“實驗結果”只佔他新理論中的一小部分；
2. 如果“實驗結果”總是與“理論”衝突，他就會認爲是這個不夠“理論”完善，在更新“理論”時，他會加入更多在“實驗結果”中的發現，甚至推翻其原有的“理論”。

這個過程很類似自動控制原理的思想，都是利用誤差反饋，只不過自動控制原理中側重於控制，被控對象的數學模型是固定的（即一開始數學模型就是非常準確的），而在卡爾曼濾波中側重於預測，被控對象的數學模型是一直不斷地被修正（即最初的數學模型不一定是準確地）。
 

代碼理解

看公式可能還是有點抽象，接下來我們用代碼來實現一個卡爾曼濾波器級加強我們的理解，以下爲matlab代碼（如果沒有matlab的話，用octave也是可以運行的）：

（1）Station prediction

1. 由於$x' = F * x + B*u+v$預測的是平均狀態，而過程噪聲$v$本身均值就爲0，因此在代碼中不需要加上$v$。

% 狀態估計
function [x, P] = prediction(x, F, B, u, P, Q)
    x = F * x + B*u;
    P = F * P * F'+Q;
end

（2）Measurement update

% 測量更新函數
function [x, P] = update(x, z, H, P, R, I)
    y = z - H * x;
    S = H * P *H' + R;
    K = P * H' * inv(S);
    x = x + K * y;
    P = (I - K*H)*P;
end

（3）設置初始值

1. $x$是任意取的，可以是準確的也可以是不準確的，後面的討論會提到；
2. $P$也是任意取得，在此我們認爲位置與速度正相關，並且位置和速度有極大的不確定性，因此選取$P = [1000, 100; 100, 1000]$；
3. $Q$的選取就很複雜了，實際應用中是通過實驗得到的，在此我們隨便取一個值；
4. 由於我們假設不存在外部動作，因此$u = [0; 0]$，$B$也是隨便取的，真正考慮外力的時候要根據外力作用的物理原理去設置對應的$B$和$u$；
5. 測量噪聲$R$由你的傳感器決定，這裏隨便取了一個值。

% 設置初始位置和速度
x = [0; 0];
% 設置不確定性矩陣初始值，以下設置即認爲：
% 位置與速度不相關，並且位置和速度有極大的不確定性
P = [1000, 100; 100 1000];
% 設置過程噪聲
Q = [1, 0.5; 2, 3];

% 指定外部動作
B = [1 0; 0 1];
u = [0; 0];

% 構建狀態函數
F = [1, 1; 0 1];
% 構建測量函數，表示僅可觀測位置而不是速度
H = [1, 0];
% 觀測不確定性矩陣
R = 1;
I = [1, 0; 0 1];

（4）設想物體的實際運動過程並設置觀測值

1. 我們不妨假設物體實際運動之後11個時刻的位置分別爲2、4、6、8、10、12、14、16、18、20、22；
2. 由於測量也是不準確的，所以我們根據條件1做出一些假設，假設之後10個時刻測量值是[2.3, 3.8, 6.2, 7.5, 9.6, 11, 13.5, 17, 18.5, 20.4]；
3. 在第11個時刻的位置是22，我們並沒有放到測量結果中，因爲我們要用這個值去判斷當更新了10次之後，模型是否能夠預測出物體下一時刻出現在22，以此來判斷卡爾曼濾波是否對模型的準確性進行有效的更新。

% 實際所得的位置測量值
measurements  = [2.3, 3.8, 6.2, 7.5, 9.6, 11, 13.5, 17, 18.5, 20.4];

（5）迭代過程

for i = 1:1:length(measurements)
    fprintf('**********第%d論迭代開始**********\n',i)
    % 首先測量更新
    [x, P] = update(x, measurements(i), H, P, R, I);
    fprintf('*****開始第%d次更新*****',i)
    x
    P
    % 然後狀態估計
    [x, P] = prediction(x, F, B, u, P, Q);
    fprintf('\n')
    fprintf('*****開始第%d次預測*****',i)
    x
    P
    fprintf('\n')
end

（6）完整代碼及運行結果

clc;clear;

% 設置初始位置和速度
x = [0; 0];
% 設置不確定性矩陣初始值，以下設置即認爲：
% 位置與速度不相關，並且位置和速度有極大的不確定性
P = [1000, 100; 100, 1000];
% 設置過程噪聲
Q = [1, 0.5; 2, 3];

% 指定外部動作
B = [1 0; 0 1];
u = [0; 0];

% 構建狀態函數
F = [1, 1; 0 1];
% 構建測量函數，表示僅可觀測位置而不是速度
H = [1, 0];
% 觀測不確定性矩陣
R = 1;
I = [1, 0; 0 1];

% 實際所得的位置測量值
measurements  = [2.3, 3.8, 6.2, 7.5, 9.6, 11, 13.5, 17, 18.5, 20.4];

for i = 1:1:length(measurements)
    fprintf('**********第%d論迭代開始**********\n',i)
    % 首先測量更新
    [x, P] = update(x, measurements(i), H, P, R, I);
    fprintf('*****開始第%d次更新*****',i)
    x
    P
    % 然後狀態估計
    [x, P] = prediction(x, F, B, u, P, Q);
    fprintf('\n')
    fprintf('*****開始第%d次預測*****',i)
    x
    P
    fprintf('\n')
end

% 狀態估計
function [x, P] = prediction(x, F, B, u, P, Q)
    x = F * x + B*u;
    P = F * P * F'+Q;
end

% 測量更新函數
function [x, P] = update(x, z, H, P, R, I)
    y = z - H * x;
    S = H * P *H' + R;
    K = P * H' * inv(S);
    x = x + K * y;
    P = (I - K*H)*P;
end

最新一次對位置的預測爲22.1900，非常接近我們的理想值22，說明模型經過修正後越來越準確了。

（7）討論

1. 如果測量次數足夠多的話，初始狀態$x$不準確也沒有關係，例如取$x = [-10; 100]$，最後預測位置結果爲22.1899，差別不大。同樣，初始$P$設置很大也沒關係，因爲在初始階段我們確實認爲自己的模型具有很大的不確定性。（當然$P$初始設置很小也可以，因爲$P$是在不斷更新的）

2. 每次更新後，$P$值變小，說明對於位置和速度的不確定性降低。每次預測後，因爲存在過程噪聲，$P$值又會變大，因爲物體移動導致位置和速度的不確定性增加了。

3. 每次更新後，$x$值經過修正後，會向實際的測量值貼合，可以看出測量的結果確實是在慢慢地修正模型。

4. 你可以不更新，選連續預測再後一次的位置，預測位置結果爲23.9263，與我們預想的結果24很接近