原理

卡爾曼濾波器是一種基礎(chǔ)預(yù)測(cè)定位算法。原理非常簡(jiǎn)單易懂。核心過(guò)程可以用一個(gè)圖說(shuō)明：

本質(zhì)上就是這兩個(gè)狀態(tài)過(guò)程的迭代，來(lái)逐步的準(zhǔn)確定位。

? 預(yù)測(cè)：當(dāng)前狀態(tài)環(huán)境下，對(duì)下一個(gè)時(shí)間段t的位置估計(jì)計(jì)算的值。 ? 更新：更具傳感器獲取到比較準(zhǔn)確的位置信息后來(lái)更新當(dāng)前的預(yù)測(cè)問(wèn)位置，也就是糾正預(yù)測(cè)的錯(cuò)誤。 ? 你可能要問(wèn)為什么有傳感器的數(shù)據(jù)了還要進(jìn)行更新？因?yàn)樵诂F(xiàn)實(shí)世界中傳感器是存在很多噪聲干擾的，所以也不能完全相信傳感器數(shù)據(jù)?？柭惴ㄒ蕾囉诰€性計(jì)算，高斯分布，我們以一維定位來(lái)介紹算法的實(shí)現(xiàn)。

接下來(lái)我們開(kāi)更新，預(yù)測(cè)后我們獲取到傳感器數(shù)據(jù)，表示目前傳感器發(fā)現(xiàn)小車(chē)的位置應(yīng)該是在26這個(gè)位置，在這種情況下，我們肯定是覺(jué)得傳感器的準(zhǔn)確度比我之前的預(yù)測(cè)瞎猜要來(lái)的準(zhǔn)確。 ? 所以方差自然會(huì)比較小，最終我們覺(jué)得真是的小車(chē)位置應(yīng)該是更靠近傳感器數(shù)據(jù)的，而且方差會(huì)縮小，以至于，想想也很清楚，我猜了一個(gè)預(yù)測(cè)值，現(xiàn)在有個(gè)專(zhuān)家告訴了我相對(duì)比較靠譜的數(shù)據(jù)，那我對(duì)小車(chē)的位置的自信度肯定會(huì)上升啊。 ? 最終小車(chē)的位置經(jīng)過(guò)這個(gè)時(shí)間段t的更新就是下圖紅色的高斯圖：

就這樣不停的移動(dòng)更新，最終小車(chē)的位置就會(huì)越來(lái)越準(zhǔn)確。

一維模型下的Kalman公式：

預(yù)測(cè) ?

更新

參考代碼：

#include 
#include 
#include 

using namespace std;

double new_mean, new_var;

tuple measurement_update(double mean1, double var1, double mean2, double var2)
{
 ? ?new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
 ? ?new_var = 1 / (1 / var1 + 1 / var2);
 ? ?return make_tuple(new_mean, new_var);
}


tuple state_prediction(double mean1, double var1, double mean2, double var2)
{
 ? ?new_mean = mean1 + mean2;
 ? ?new_var = var1 + var2;
 ? ?return make_tuple(new_mean, new_var);
}


int main()
{
 ? ?//Measurements and measurement variance
 ? ?double measurements[5] = { 5, 6, 7, 9, 10 };
 ? ?double measurement_sig = 4;
 ? ?
 ? ?//Motions and motion variance
 ? ?double motion[5] = { 1, 1, 2, 1, 1 };
 ? ?double motion_sig = 2;
 ? ?
 ? ?//Initial state
 ? ?double mu = 0;
 ? ?double sig = 1000;


 ? ?for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
 ? ? ? ?tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
 ? ? ? ?printf("update: ?[%f, %f]
", mu, sig);
 ? ? ? ?tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
 ? ? ? ?printf("predict: [%f, %f]
", mu, sig);
 ? ?}


 ? ?return 0;
}

?