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

本質(zhì)上就是這兩個(gè)狀態(tài)過程的迭代,來逐步的準(zhǔn)確定位。 預(yù)測(cè):當(dāng)前狀態(tài)環(huán)境下,對(duì)下一個(gè)時(shí)間段t的位置估計(jì)計(jì)算的值。 更新:更具傳感器獲取到比較準(zhǔn)確的位置信息后來更新當(dāng)前的預(yù)測(cè)問位置,也就是糾正預(yù)測(cè)的錯(cuò)誤。 你可能要問為什么有傳感器的數(shù)據(jù)了還要進(jìn)行更新?因?yàn)樵诂F(xiàn)實(shí)世界中傳感器是存在很多噪聲干擾的,所以也不能完全相信傳感器數(shù)據(jù)。卡爾曼算法依賴于線性計(jì)算,高斯分布,我們以一維定位來介紹算法的實(shí)現(xiàn)。


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

就這樣不停的移動(dòng)更新,最終小車的位置就會(huì)越來越準(zhǔn)確。
一維模型下的Kalman公式:
預(yù)測(cè)

更新


參考代碼:
using namespace std;
double new_mean, new_var;
tuple<double, double> 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<double, double> 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;
}
審核編輯 :李倩
聲明:本文內(nèi)容及配圖由入駐作者撰寫或者入駐合作網(wǎng)站授權(quán)轉(zhuǎn)載。文章觀點(diǎn)僅代表作者本人,不代表電子發(fā)燒友網(wǎng)立場(chǎng)。文章及其配圖僅供工程師學(xué)習(xí)之用,如有內(nèi)容侵權(quán)或者其他違規(guī)問題,請(qǐng)聯(lián)系本站處理。
舉報(bào)投訴
-
傳感器
+關(guān)注
關(guān)注
2565文章
52995瀏覽量
767372 -
定位算法
+關(guān)注
關(guān)注
0文章
61瀏覽量
15273 -
卡爾曼濾波器
+關(guān)注
關(guān)注
0文章
54瀏覽量
12390
發(fā)布評(píng)論請(qǐng)先 登錄
相關(guān)推薦
熱點(diǎn)推薦
卡爾曼濾波
卡爾曼濾波的噪聲協(xié)方差怎么配置?。?/div>
發(fā)表于 08-01 10:05
卡爾曼濾波有哪些應(yīng)用
卡爾曼濾波風(fēng)力發(fā)電機(jī)中的風(fēng)速估計(jì),轉(zhuǎn)速估計(jì)甚至扭矩估計(jì)都設(shè)計(jì)到卡爾曼濾波,如果只是單一傳感變量的
發(fā)表于 07-12 06:00
卡爾曼濾波簡(jiǎn)介
在這里我就不介紹卡爾曼的數(shù)學(xué)推算了,網(wǎng)上的數(shù)學(xué)推導(dǎo)一抓一大把,如果想了解推導(dǎo)過程的小伙伴可以去大佬的博客。如果你是想直接簡(jiǎn)單運(yùn)用卡爾曼濾波來
發(fā)表于 02-28 14:24
淺析卡爾曼濾波
在 飛行器姿態(tài)計(jì)算 中,卡爾曼濾波是最常用的姿態(tài)計(jì)算方法之一。今天就以目前的理解講以下卡爾曼濾波
卡爾曼濾波在圖像處理中的應(yīng)用實(shí)例 如何調(diào)優(yōu)卡爾曼濾波參數(shù)
卡爾曼濾波在圖像處理中的應(yīng)用實(shí)例 卡爾曼濾波在圖像處理中主要應(yīng)用于目標(biāo)跟蹤、噪聲消除和圖像恢復(fù)等
評(píng)論