什么是卡爾曼濾波？

對于這個濾波器，我們幾乎可以下這么一個定論：只要是存在不確定信息的動態系統，卡爾曼濾波就可以對系統下一步要做什么做出有根據的推測。即便有噪聲信息干擾，卡爾曼濾波通常也能很好的弄清楚究竟發生了什么，找出現象間不易察覺的相關性。

因此卡爾曼濾波非常適合不斷變化的系統，它的優點還有內存占用較小（只需保留前一個狀態）、速度快，是實時問題和嵌入式系統的理想選擇。

如果你曾經Google過卡爾曼濾波的教程（如今有一點點改善），你會發現相關的算法教程非常可怕，而且也沒具體說清楚是什么。事實上，卡爾曼濾波很簡單，如果我們以正確的方式看它，理解是很水到渠成的事。

本文會用大量清晰、美觀的圖片和顏色來解釋這個概念，讀者只需具備概率論和矩陣的一般基礎知識。

我們能用卡爾曼濾波做什么？

讓我們舉個例子：你造了一個可以在樹林里四處溜達的小機器人，為了讓它實現導航，機器人需要知道自己所處的位置。

也就是說，機器人有一個包含位置信息和速度信息的狀態??：

注意，在這個例子中，狀態是位置和速度，放進其他問題里，它也可以是水箱里的液體體積、汽車引擎溫度、觸摸板上指尖的位置，或者其他任何數據。

我們的小機器人裝有GPS傳感器，定位精度10米。雖然一般來說這點精度夠用了，但我們希望它的定位誤差能再小點，畢竟樹林里到處都是土坑和陡坡，如果機器人稍稍偏了那么幾米，它就有可能滾落山坡。所以GPS提供的信息還不夠充分。

我們也可以預測機器人是怎么移動的：它會把指令發送給控制輪子的馬達，如果這一刻它始終朝一個方向前進，沒有遇到任何障礙物，那么下一刻它可能會繼續堅持這個路線。但是機器人對自己的狀態不是全知的：它可能會逆風行駛，輪子打滑，滾落顛簸地形……所以車輪轉動次數并不能完全代表實際行駛距離，基于這個距離的預測也不完美。

這個問題下，GPS為我們提供了一些關于狀態的信息，但那是間接的、不準確的；我們的預測提供了關于機器人軌跡的信息，但那也是間接的、不準確的。

但以上就是我們能夠獲得的全部信息，在它們的基礎上，我們是否能給出一個完整預測，讓它的準確度比機器人搜集的單次預測匯總更高？用了卡爾曼濾波，這個問題可以迎刃而解。

卡爾曼濾波眼里的機器人問題

還是上面這個問題，我們有一個狀態，它和速度、位置有關：

我們不知道它們的實際值是多少，但掌握著一些速度和位置的可能組合，其中某些組合的可能性更高：

卡爾曼濾波假設兩個變量（在我們的例子里是位置和速度）都應該是隨機的，而且符合高斯分布。每個變量都有一個均值??，它是隨機分布的中心；有一個方差??，它衡量組合的不確定性。

在上圖中，位置和速度是不相關的，這意味著我們不能從一個變量推測另一個變量。

那么如果位置和速度相關呢？如下圖所示，機器人前往特定位置的可能性取決于它擁有的速度。

這不難理解，如果基于舊位置估計新位置，我們會產生這兩個結論：如果速度很快，機器人可能移動得更遠，所以得到的位置會更遠；如果速度很慢，機器人就走不了那么遠。

這種關系對目標跟蹤來說非常重要，因為它提供了更多信息：一個可以衡量可能性的標準。這就是卡爾曼濾波的目標：從不確定信息中擠出盡可能多的信息！

為了捕獲這種相關性，我們用的是協方差矩陣。簡而言之，矩陣的每個值是第??個變量和第??個變量之間的相關程度（由于矩陣是對稱的，??和??的位置可以隨便交換）。我們用??表示協方差矩陣，在這個例子中，就是??。

用矩陣描述問題

為了把以上關于狀態的信息建模為高斯分布（圖中色塊），我們還需要??時的兩個信息：最佳估計??（均值，也就是??），協方差矩陣??。（雖然還是用了位置和速度兩個變量，但只要和問題相關，卡爾曼濾波可以包含任意數量的變量）

接下來，我們要通過查看當前狀態（k-1時）來預測下一個狀態（k時）。這里我們查看的狀態不是真值，但預測函數無視真假，可以給出新分布：

我們可以用矩陣??表示這個預測步驟：

它從原始預測中取每一點，并將其移動到新的預測位置。如果原始預測是正確的，系統就會移動到新位置。

這是怎么做到的？為什么我們可以用矩陣來預測機器人下一刻的位置和速度？下面是個簡單公式：

換成矩陣形式：

這是一個預測矩陣，它能給出機器人的下一個狀態，但目前我們還不知道協方差矩陣的更新方法。這也是我們要引出下面這個等式的原因：如果我們將分布中的每個點乘以矩陣A，那么它的協方差矩陣會發生什么變化

把這個式子和上面的最佳估計??結合，可得：

外部影響

但是，除了速度和位置，外因也會對系統造成影響。比如模擬火車運動，除了列車自駕系統，列車操作員可能會手動調速。在我們的機器人示例中，導航軟件也可以發出停止指令。對于這些信息，我們把它作為一個向量??，納入預測系統作為修正。

假設油門設置和控制命令是已知的，我們知道火車的預期加速度??。根據運動學基本定理，我們可得：

把它轉成矩陣形式：

?是控制矩陣，??是制向量。如果外部環境異常簡單，我們可以忽略這部分內容，但是如果添加了外部影響后，模型的準確率還是上不去，這又是為什么呢？

外部不確定性

如果狀態根據自己的屬性發展，一切都很好。 如果狀態根據外力發展，只要我們知道那些外部力量是什么，一切都仍然很好。

但是，如果存在我們不知道的力量呢？當我們監控無人機時，它可能會受到風的影響；當我們跟蹤輪式機器人時，它的輪胎可能會打滑，或者粗糙地面會降低它的移速。這些因素是難以掌握的，如果出現其中的任意一種情況，預測結果就難以保障。

這要求我們在每個預測步驟后再加上一些新的不確定性，來模擬和“世界”相關的所有不確定性：

如上圖所示，加上外部不確定性后，??的每個預測狀態都可能會移動到另一點，也就是藍色的高斯分布會移動到紫色高斯分布的位置，并且具有協方差??。換句話說，我們把這些不確定影響視為協方差??的噪聲。

這個紫色的高斯分布擁有和原分布相同的均值，但協方差不同。

我們在原式上加入??：

簡而言之，這里：

?是基于??和??校正后得到的預測。

?是基于??和??得到的預測。

現在，有了這些概念介紹，我們可以把傳感器數據輸入其中。

通過測量來細化估計值

我們可能有好幾個傳感器，它們一起提供有關系統狀態的信息。傳感器的作用不是我們關心的重點，它可以讀取位置，可以讀取速度，重點是，它能告訴我們關于狀態的間接信息——它是狀態下產生的一組讀數。

請注意，讀數的規模和狀態的規模不一定相同，所以我們把傳感器讀數矩陣設為??。

把這些分布轉換為一般形式：

卡爾曼濾波的一大優點是擅長處理傳感器噪聲。換句話說，由于種種因素，傳感器記錄的信息其實是不準的，一個狀態事實上可以產生多種讀數。

從我們觀察到的每個讀數中，我們可能會猜測我們的系統處于特定狀態。但由于存在不確定性，一些狀態比其他狀態更有可能產生我們所看到的讀數：

我們將這種不確定性（即傳感器噪聲）的協方差設為??，讀數的分布均值設為??。

現在我們得到了兩塊高斯分布，一塊圍繞預測的均值，另一塊圍繞傳感器讀數。

如果要生成靠譜預測，模型必須調和這兩個信息。也就是說，對于任何可能的讀數??，這兩種方法預測的狀態都有可能是準的，也都有可能是不準的。重點是我們怎么找到這兩個準確率。

最簡單的方法是兩者相乘：

兩塊高斯分布相乘后，我們可以得到它們的重疊部分，這也是會出現最佳估計的區域。換個角度看，它看起來也符合高斯分布：

事實證明，當你把兩個高斯分布和它們各自的均值和協方差矩陣相乘時，你會得到一個擁有獨立均值和協方差矩陣的新高斯分布。最后剩下的問題就不難解決了：我們必須有一個公式來從舊的參數中獲取這些新參數！

結合高斯

讓我們從一維看起，設方差為??，均值為??，一個標準一維高斯鐘形曲線方程如下所示：

那么兩條高斯曲線相乘呢？

把這個式子按照一維方程進行擴展，可得：

如果有些太復雜，我們用k簡化一下：

以上是一維的內容，如果是多維空間，把這個式子轉成矩陣格式：

這個矩陣??就是我們說的卡爾曼增益，easy！

把它們結合在一起

截至目前，我們有用矩陣??預測的分布，有用傳感器讀數??預測的分布。把它們代入上節的矩陣等式中：

相應的，卡爾曼增益就是：

考慮到??里還包含著一個??，我們再精簡一下上式：

最后，??是我們的最佳估計值，我們可以把它繼續放進去做另一輪預測：

應用例子：估計小車的運動狀態

1、數學模型：

有一個勻加速運動的小車，它受到的合力為ft，由勻加速運動的位移和速度公式，能得到由t-1到t時刻的位移和速度變化公式：

該系統的狀態向量包括位移和速度，分別用xt和xt的導數表示。控制輸入變量為u，也就是加速度，于是有如下形式：

所以這個系統的狀態方程為：

這里對應的矩陣A大小為2*2，矩陣B大小為2*1.

注意Z是測量值，大小為m*1，H也是狀態變量到測量的轉換矩陣，大小為m*n。隨機變量v是測量噪聲。

假設車是勻加速遠離我們，站在原點用超聲波測量小車離我們的距離。

對于小車勻加速運動的模型，假設系統的噪聲向量只存在速度分量上，且速度噪聲的方差是一個常量0.01，位移分量上的系統噪聲為0.測量值只有位移，它的協方差矩陣大小是1*1，就是測量噪聲的方差本身。

Q中，疊加在速度上系統噪聲方差為0.01，位移上的為0，它們間的協方差為0，即噪聲間沒有關聯。

2、C++實現代碼：

#include <iostream> #include <fstream> #include <string> #include <vector> #include <Eigen/Dense>//包含Eigen矩陣運算庫，用于矩陣計算 #include <cmath> #include <limits>//用于生成隨機分布數列using namespace std; using Eigen::MatrixXd;int main(int argc, char **argv[]) {//""中是txt文件路徑，注意：路徑要用//隔開ofstream fout("..//result.txt");double generateGaussianNoise(double mu, double sigma);//隨機高斯分布數列生成器函數const double delta_t = 0.1;//控制周期，100msconst int num = 100;//迭代次數const double acc = 10;//加速度，ft/mMatrixXd A(2, 2);A(0, 0) = 1;A(1, 0) = 0;A(0, 1) = delta_t;A(1, 1) = 1;MatrixXd B(2, 1);B(0, 0) = pow(delta_t, 2) / 2;B(1, 0) = delta_t;MatrixXd H(1, 2);//測量的是小車的位移，速度為0H(0, 0) = 1;H(0, 1) = 0;MatrixXd Q(2, 2);//過程激勵噪聲協方差，假設系統的噪聲向量只存在速度分量上，且速度噪聲的方差是一個常量0.01，位移分量上的系統噪聲為0Q(0, 0) = 0;Q(1, 0) = 0;Q(0, 1) = 0;Q(1, 1) = 0.01;MatrixXd R(1, 1);//觀測噪聲協方差，測量值只有位移，它的協方差矩陣大小是1*1，就是測量噪聲的方差本身。R(0, 0) = 10;//time初始化，產生時間序列vector<double> time(100, 0);for (decltype(time.size()) i = 0; i != num; ++i) {time[i] = i * delta_t;//cout<<time[i]<<endl;}MatrixXd X_real(2, 1);vector<MatrixXd> x_real, rand;//生成高斯分布的隨機數for (int i = 0; i<100; ++i) {MatrixXd a(1, 1);a(0, 0) = generateGaussianNoise(0, sqrt(10));rand.push_back(a);}//生成真實的位移值for (int i = 0; i < num; ++i) {X_real(0, 0) = 0.5 * acc * pow(time[i], 2);X_real(1, 0) = 0;x_real.push_back(X_real);}//變量定義，包括狀態預測值，狀態估計值，測量值，預測狀態與真實狀態的協方差矩陣，估計狀態和真實狀態的協方差矩陣，初始值均為零MatrixXd X_evlt = MatrixXd::Constant(2, 1, 0), X_pdct = MatrixXd::Constant(2, 1, 0), Z_meas = MatrixXd::Constant(1, 1, 0),Pk = MatrixXd::Constant(2, 2, 0), Pk_p = MatrixXd::Constant(2, 2, 0), K = MatrixXd::Constant(2, 1, 0);vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;x_evlt.push_back(X_evlt);x_pdct.push_back(X_pdct);z_meas.push_back(Z_meas);pk.push_back(Pk);pk_p.push_back(Pk_p);k.push_back(K);//開始迭代for (int i = 1; i < num; ++i) {//預測值X_pdct = A * x_evlt[i - 1] + B * acc;x_pdct.push_back(X_pdct);//預測狀態與真實狀態的協方差矩陣，Pk'Pk_p = A * pk[i - 1] * A.transpose() + Q;pk_p.push_back(Pk_p);//K:2x1MatrixXd tmp(1, 1);tmp = H * pk_p[i] * H.transpose() + R;K = pk_p[i] * H.transpose() * tmp.inverse();k.push_back(K);//測量值zZ_meas = H * x_real[i] + rand[i];z_meas.push_back(Z_meas);//估計值X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);x_evlt.push_back(X_evlt);//估計狀態和真實狀態的協方差矩陣，PkPk = (MatrixXd::Identity(2, 2) - k[i] * H) * pk_p[i];pk.push_back(Pk);}cout << "含噪聲測量" << " " << "后驗估計" << " " << "真值" << " " << endl;for (int i = 0; i < num; ++i) {cout<<z_meas[i]<<" "<<x_evlt[i](0,0)<<" "<<x_real[i](0,0)<<endl;fout << z_meas[i] << " " << x_evlt[i](0, 0) << " " << x_real[i](0, 0) << endl;//輸出到txt文檔，用于matlab繪圖//cout<<k[i](1,0)<<endl;//fout<<rand[i](0,0)<<endl;//fout<<x_pdct[i](0,0)<<endl;}fout.close();getchar();return 0; }//生成高斯分布隨機數的函數，網上找的 double generateGaussianNoise(double mu, double sigma) {const double epsilon = std::numeric_limits<double>::min();const double two_pi = 2.0*3.14159265358979323846;static double z0, z1;static bool generate;generate = !generate;if (!generate)return z1 * sigma + mu;double u1, u2;do{u1 = rand() * (1.0 / RAND_MAX);u2 = rand() * (1.0 / RAND_MAX);} while (u1 <= epsilon);z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);return z0 * sigma + mu; }

3、MATLAB繪圖：

a = dlmread('result.txt');

plot(a)

4、調參經驗

？（待實踐）

(a) 一開始預測不準，則更相信測量值，增大初始的Pk->增大Pk`->k->更相信測量值

(b) 狀態方程建立不正確，相當于過程噪聲增大了，所以這個時候應該增大過程噪聲的方差Q

?

參考：

How a Kalman filter works, in pictures

圖說卡爾曼濾波，一份通俗易懂的教

卡爾曼濾波 -- 從推導到應用

https://blog.csdn.net/m0_38089090/article/details/79523784

總結

以上是生活随笔為你收集整理的图说卡尔曼滤波（C++实现）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。