目標(biāo)追蹤之卡爾曼濾波

最近在看Coursera的robotic learning，發(fā)現(xiàn)挺有意思的。這里算是做一下week 2的用卡爾曼濾波來做機(jī)器人目標(biāo)追蹤的筆記。

這篇小文章主要有兩個內(nèi)容

怎樣用一個卡爾曼濾波對一個線性動力系統(tǒng)進(jìn)行建模以及求解

做一個小球追蹤的小實驗

卡爾曼濾波建模

濾波，簡要來說就是如何過濾掉收到干擾的信號。卡爾曼濾波所做的是從一堆觀測數(shù)據(jù)中，去估計出真實數(shù)據(jù)樣子。舉一個簡單的例子，就說機(jī)器人的目標(biāo)追蹤。人感知這個世界靠的是眼睛，機(jī)器人感知這個世界靠的是傳感器。機(jī)器人通過傳感器測量物體位置的時候，傳感器可能帶來測量誤差。例如，上一篇利用高斯分布來檢測小球的文章，我們很有可能由于遮擋、模型的精度不夠?qū)е挛覀儨y量的小球中心的位置會有些許誤差。例如下圖中，我們測量的可能是帶有噪聲的黑點，那么真正的軌跡究竟是如何的呢？

我們把我們所關(guān)心的真實的小球當(dāng)前的狀態(tài)稱之為狀態(tài)（State）x，傳感器測量得到的小球位置稱之為測量值（Measurement）z。其中，x為隱含變量。

＃ 由于看的公開課學(xué)的kalman filter，所以文章中用到的數(shù)學(xué)符號大部分跟隨課件。

＃ 另外是Coursera這門課上講課的那個小哥的ppt總是感覺寫的不太嚴(yán)謹(jǐn)，所以這里我另外自己推導(dǎo)了一下。

所有的模型最重要的開始就是它的假設(shè)，卡爾曼濾波的第一個假設(shè)是當(dāng)前的狀態(tài)只與前一個狀態(tài)相關(guān)。用數(shù)學(xué)表達(dá)是

接下來是卡爾曼濾波的第二個假設(shè)，由于我們對真實的狀態(tài)并不知道，所以我們估計的狀態(tài)實際上都有一定的不確定性，而這種不確定性我們用高斯分布的均值以及方差來表示。對于上一個狀態(tài)，假設(shè)我們估計其均值為方差為。則有

(1)

根據(jù)離散動力系統(tǒng)的構(gòu)造，我們能得到與的關(guān)系

(2)

其中A為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，代表系統(tǒng)誤差并服從高斯分布。在我們的小球追蹤實驗中，x為小球的位置以及速度，A矩陣則是根據(jù)位移以及速度的關(guān)系構(gòu)造。

由公式(1)，假設(shè)誤差服從0中心，方差為的高斯分布。我們可以得出服從以為中心，方差為的高斯分布。

(3)

那么我們可以根據(jù)公式(1)和公式(3)，計算出的邊緣概率分布。

其邊緣概率分布服從

（5）

懶得推導(dǎo)可以根據(jù)PRML這書的這一頁紙直接得到結(jié)論，需要詳細(xì)推導(dǎo)的話可以看看書（PRML第二章）。

接下來是對觀測值建模，觀測值與的關(guān)系，則為

(6)

這里C是從真實值到觀察值的狀態(tài)轉(zhuǎn)移矩陣，為觀測誤差并服從的高斯分布。則同理，服從為中心，為方差的高斯分布。

(7)

我們的目標(biāo)是希望最大化后驗概率。

說人話的話，就是一開始當(dāng)我們還沒觀測到結(jié)果，我們就有在此時刻的狀態(tài)大概是怎么樣的情況，也就是先驗。如今當(dāng)我們觀測到了結(jié)果，我們需要如何修正的概率分布呢？我們需要求解的就是去求得。為了計算的方便，我們先定一些符號

根據(jù)貝葉斯公式（或者直接套用上面PRML的圖），我們可以直接得出后驗概率為

那么估計值應(yīng)該為高斯分布的均值

而估計值的方差則為

為了表達(dá)上的優(yōu)美，我們將的公式做一下變換

先給出一個矩陣逆定理

我們令

那么

我們可以這么理解上面這一條公式，應(yīng)該為t時刻中，沒有觀察值預(yù)測到的狀態(tài)。為預(yù)測的觀察值與實際觀察值的差距。然后通過一個來調(diào)整最終的估計值。所以在卡爾曼濾波中，有一個特殊的意義，叫做卡爾曼增益。

整個卡爾曼濾波的算法基本如下：

設(shè)定初始狀態(tài)的均值和方差，

接受下一個觀測值

計算方差，以及卡爾曼增益

更新估計值的方差以及均值

，回到2

實戰(zhàn)：小球追蹤實驗

這次的仿真實驗給出了小球每一幀的運動軌跡，我們需要在每一個時刻中預(yù)測出小球在10幀之后位置，具體的要求見Requirement。

小球的軌跡如上圖所示，為了做對比，我們引入一種最樸素的算法作為估計。對于每一時刻的，我們估計它的速度為

那么十幀之后，它的位置估計為

我們把這種方法叫做Naive Prediction。

另外的一個方法，就是我們的卡爾曼濾波。首先，我們假定x軸與y軸是相互獨立的。卡爾曼濾波中的狀態(tài)我們設(shè)定為x軸，y軸坐標(biāo)，以及x軸方向與y軸方向的速度。

A狀態(tài)轉(zhuǎn)移矩陣根據(jù)位移以及速度的關(guān)系為

我們假定觀測的實際上是狀態(tài)在10幀之前的位置。則矩陣為

建立好動力系統(tǒng)后，接下來是對卡爾曼濾波的參數(shù)進(jìn)行初始化，由于一開始我們并不知道初始狀態(tài)的位置，所以我們可以把第一個觀測值當(dāng)作我們初始狀態(tài)，然后加上一個較大的初始方差。剩下的系統(tǒng)誤差的方差以及觀測誤差的方差則需要自己慢慢的調(diào)試了。具體的matlab代碼如下

function [ predictx, predicty, state, param ] = kalmanFilter( t, x, y, state, param, previous_t ) %UNTITLED Summary of this function goes here % Four dimensional state: position_x, position_y, velocity_x, velocity_y%% Place parameters like covarainces, etc. here:sigm = eye(4);sigo = 1e-3*eye(2);dt = t- previous_t;A = [1 0 dt 0 ;0 1 0 dt;0 0 1 0;0 0 0 1];C = [1 0 -10* dt 0;0 1 0 -10*dt];% Check if the first time running this functionif previous_t<0state = [x,y,0,0];param.P = 10 * sigm;param.R = sigo;predictx = x;predicty = y;return;endparam.R = sigo;P = A*param.P*A'+ sigm;K = P*C'/(param.R+C*P*C');state = A*state'+ K*([x,y]'-C*A*state');state = state';param.P = P - K*C*P;predictx = state(1);predicty = state(2);return end

?由于預(yù)測的10幀后的軌跡會與上圖的軌跡重合，為了更好地可視化出我們的目標(biāo)，我們另外將x軸和y軸抽取出來單獨畫圖。

上圖是x軸以及y軸的追蹤效果，其中紅線代表每一時刻的觀測值，藍(lán)線是10幀之后我們需要預(yù)測的答案。下圖為每一時刻的誤差。我們能發(fā)現(xiàn)，樸素方法的預(yù)測得到的估計值十分不穩(wěn)定，而卡爾曼濾波的估計值卻能比較平滑的接近于真實狀態(tài)的曲線。

這里算是告一段落了，這篇文章只是粗略地介紹一下卡爾曼濾波算法，以及做了一個小仿真實驗。在我個人看來，卡爾曼濾波算法中，有很多人為設(shè)定的東西，例如A、C兩個狀態(tài)轉(zhuǎn)移矩陣，還有不同的方差設(shè)定。

要能準(zhǔn)確的調(diào)節(jié)這些參數(shù)，我覺得最重要的一點就是理解算法的本身，理解每一個假設(shè)還有求解中間的過程。這也是我之所以比較多的寫中間卡爾曼濾波推導(dǎo)過程的原因，最好還是自己動手做一做。歡迎討論～

=====================================================

補(bǔ)充一下的推導(dǎo)，主要還是參考《Probabilistic Robotics》里面的推導(dǎo)公式。只是照搬書上的公式，套回來這里用的數(shù)學(xué)符號。

轉(zhuǎn)載于:https://www.cnblogs.com/lvchaoshun/p/7115515.html

總結(jié)

以上是生活随笔為你收集整理的实践卡尔曼滤波--小球追踪的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。