實踐卡爾曼濾波--小球追蹤

本文轉載自查看原文 2017-07-04 10:50 1446 算法推導

目標追蹤之卡爾曼濾波

最近在看Coursera的robotic learning，發現挺有意思的。這里算是做一下week 2的用卡爾曼濾波來做機器人目標追蹤的筆記。

這篇小文章主要有兩個內容

怎樣用一個卡爾曼濾波對一個線性動力系統進行建模以及求解
做一個小球追蹤的小實驗

卡爾曼濾波建模

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

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

＃由於看的公開課學的kalman filter，所以文章中用到的數學符號大部分跟隨課件。

＃另外是Coursera這門課上講課的那個小哥的ppt總是感覺寫的不太嚴謹，所以這里我另外自己推導了一下。

所有的模型最重要的開始就是它的假設，卡爾曼濾波的第一個假設是當前的狀態只與前一個狀態相關。用數學表達是

$p(x_t|x_{t-1},\dots,x_0)=p(x_t|x_{t-1})$

接下來是卡爾曼濾波的第二個假設，由於我們對真實的狀態並不知道，所以我們估計的狀態實際上都有一定的不確定性，而這種不確定性我們用高斯分布的均值以及方差來表示。對於上一個狀態 $x_{t-1}$ ，假設我們估計其均值為 $\hat{x}_{t-1}$ 方差為 $\hat{P}_{t-1}$ 。則有

$p(x_{t-1})\sim N(\hat{x}_{t-1},\hat{P}_{t-1})$ (1)

根據離散動力系統的構造，我們能得到 x_t

與 $x_{t-1}$ 的關系

$x_t=Ax_{t-1}+\epsilon$ (2)

其中A為系統的狀態轉移矩陣， $\epsilon$ 代表系統誤差並服從高斯分布。在我們的小球追蹤實驗中，x為小球的位置以及速度，A矩陣則是根據位移以及速度的關系構造。

$x=[ v\text{\ \ } \frac{dv}{dt}],A=\begin{pmatrix} 1 & dt\\ 0 & 1\\ \end{pmatrix}$

由公式(1)，假設誤差服從0中心，方差為 $\Sigma_m$ 的高斯分布。我們可以得出 $p(x_t|x_{t-1})$ 服從以 $Ax_{t-1}$ 為中心，方差為 $\Sigma_m$ 的高斯分布。

$p(x_t|x_{t-1}) \sim N(Ax_{t-1} ,\Sigma_m)$ (3)

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

其邊緣概率分布服從

$p(x_t)\sim N(A\hat{x}_{t-1},A\hat{P}_{t-1}A^T+\Sigma_m)$ （5）

懶得推導可以根據PRML這書的這一頁紙直接得到結論，需要詳細推導的話可以看看書（PRML第二章）。

接下來是對觀測值建模，觀測值 z_t 與 x_t 的關系，則為

$z_t=Cx_t+\epsilon$ (6)

這里C是從真實值到觀察值的狀態轉移矩陣， $\epsilon$ 為觀測誤差並服從 $N(0,\Sigma_0)$ 的高斯分布。則同理， p(z_t|x_t) 服從 Cx_t 為中心， $\Sigma_0$ 為方差的高斯分布。

$p(z_t|x_t)\sim N(Cx_t,\Sigma_0)$ (7)

我們的目標是希望最大化后驗概率。

$\hat{x_t}=\underset{x_t}{\operatorname{argmax}} \text{\ }p(x_t|z_t)$

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

$P=A\hat{P}_{t-1}A^T+\Sigma_m$
$R=\Sigma_0$

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

$p(x_t|z_t)\sim N((P^{-1}+C^{T}R^{-1}C)^{-1} (C^{T}R^{-1}z_t+P^{-1}A\hat{x}_{t-1}),(P^{-1}+C^{T}R^{-1}C)^{-1})$

那么估計值 $\hat{x}_t$ 應該為高斯分布的均值

$\hat{x}_t=\underset{x_t}{\operatorname{argmax}} \text{\ }p(x_t|z_t)=(P^{-1}+C^{T}R^{-1}C)^{-1} (C^{T}R^{-1}z_t+P^{-1}A\hat{x}_{t-1})$

而估計值的方差則為

$\hat{P}_{t}=(P^{-1}+C^{T}R^{-1}C)^{-1}$

為了表達上的優美，我們將 $\hat{x}_t$ 的公式做一下變換

先給出一個矩陣逆定理

$(C^TR^{-1}C+P^{-1})^{-1}=P-PC^{T}(R+CPC^T)^{-1}CP$

我們令 $K=PC^{T}(R+CPC^T)^{-1}$

那么

$\hat{x}_t=(C^TR^{-1}C+P^{-1})^{-1}(C^TR^{-1}z_t+P^{-1}A\hat{x}_{t-1})$
$\hat{x}_t=(P - KCP)(C^TR^{-1}z_t+P^{-1}A\hat{x}_{t-1})$
$\hat{x}_t=A\hat{x}_{t-1}+K(z_t-CA\hat{x}_{t-1})$

我們可以這么理解上面這一條公式， $A\hat{x}_{t-1}$ 應該為t時刻中，沒有觀察值預測到的狀態。 $z_t-CA\hat{x}_{t-1}$ 為預測的觀察值與實際觀察值的差距。然后通過一個來調整最終的 $\hat{x}_t$ 估計值。所以在卡爾曼濾波中，有一個特殊的意義，叫做卡爾曼增益。

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

設定初始狀態的均值和方差 $\hat{x}_0,\hat{P}_0,\Sigma_m,\Sigma_0$ ，
接受下一個觀測值
計算方差，以及卡爾曼增益
更新估計值的方差以及均值 $\hat{P}_{t},\hat{x}_t$
，回到2

實戰：小球追蹤實驗

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

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

$v_t = \frac{x_t-x_{t-1}}{t},t=1$

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

$\hat{x_t}=x_t+10v_t$

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

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

A狀態轉移矩陣根據位移以及速度的關系為

$A=\begin{pmatrix} 1,0,t,0\\ 0,1,0,t \end{pmatrix},t=1$

我們假定觀測的 z_t 實際上是狀態 x_t 在10幀之前的位置。則矩陣為

$C=\begin{pmatrix} 1 & 0 & -10t & 0\\ 0 & 1 & 0 & -10t\\ \end{pmatrix},t=1$

建立好動力系統后，接下來是對卡爾曼濾波的參數進行初始化，由於一開始我們並不知道初始狀態的位置，所以我們可以把第一個觀測值當作我們初始狀態，然后加上一個較大的初始方差。剩下的系統誤差的方差以及觀測誤差的方差則需要自己慢慢的調試了。具體的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 function
    if previous_t<0
        state = [x,y,0,0];
        param.P = 10 * sigm;
        param.R =  sigo;
        predictx = x;
        predicty = y;
        return;
    end
    param.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

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

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

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

要能准確的調節這些參數，我覺得最重要的一點就是理解算法的本身，理解每一個假設還有求解中間的過程。這也是我之所以比較多的寫中間卡爾曼濾波推導過程的原因，最好還是自己動手做一做。歡迎討論～

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

補充一下 $K=PC^T(R+CPC^T)^{-1}=(C^TR^{-1}C+P^{-1})^{-1}C^TR^{-1}$ 的推導，主要還是參考《Probabilistic Robotics》里面的推導公式。只是照搬書上的公式，套回來這里用的數學符號。

$\begin{equation} \begin{aligned} K&=(C^TR^{-1}C+P^{-1})^{-1}C^TR^{-1}\\ &=(C^TR^{-1}C+P^{-1})^{-1}C^TR^{-1}(CPC^T+R)(CPC^{T}+R)^{-1}\\ &=(C^TR^{-1}C+P^{-1})^{-1}(C^TR^{-1}CPC^T+C^TR^{-1}R)(CPC^{T}+R)^{-1}\\ &=(C^TR^{-1}C+P^{-1})^{-1}(C^TR^{-1}CPC^T+C^T)(CPC^{T}+R)^{-1}\\ &=(C^TR^{-1}C+P^{-1})^{-1}(C^TR^{-1}CPC^T+P^{-1}PC^T)(CPC^{T}+R)^{-1}\\ &=(C^TR^{-1}C+P^{-1})^{-1}(C^TR^{-1}C+P^{-1})PC^T(CPC^{T}+R)^{-1}\\ &=PC^T(CPC^T+R)^{-1}\\ \end{aligned} \end{equation}$

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 卡爾曼濾波卡爾曼濾波+單目標追蹤+python-opencv 卡爾曼濾波的推導 python 卡爾曼濾波卡爾曼濾波原理卡爾曼濾波學習濾波算法：經典卡爾曼濾波濾波算法：經典卡爾曼濾波卡爾曼濾波(KF)與擴展卡爾曼(EKF) 卡爾曼濾波五個公式推導過程