2. 程式人生 > >運動目標跟蹤（一）--搜尋演算法預測模型之KF,EKF,UKF

運動目標跟蹤（一）--搜尋演算法預測模型之KF,EKF,UKF

阿新 • • 發佈：2019-02-07

這裡先總體介紹下，原文轉自：

任何感測器，鐳射也好，視覺也好，整個SLAM系統也好，要解決的問題只有一個：如何通過資料來估計自身狀態。每種感測器的測量模型不一樣，它們的精度也不一樣。換句話說，狀態估計問題，也就是“如何最好地使用感測器資料”。可以說，SLAM是狀態估計的一個特例。

1. 離散時間系統的狀態估計


　　記機器人在各時刻的狀態為x_1,\ldots,x_k，其中k是離散時間下標。在SLAM中，我們通常要估計機器人的位置，那麼系統的狀態就指的是機器人的位姿。用兩個方程來描述狀態估計問題：
\[\left\{ \begin{array}{l}{x_k} = f\left( {{x_{k - 1}},{u_k},{w_k}} \right)\\{y_k} = g\left( {{x_k},{n_k}} \right)\end{array} \right.\]
　　解釋一下變數： 　　f-運動方程
　　u- 輸入
　　w- 輸入噪聲
　　g- 觀測方程
　　y- 觀測資料
　　n- 觀測噪聲

　　運動方程描述了狀態x_{k-1}
是怎麼變到x_k的，而觀測方程描述的是從x_k是怎麼得到觀察資料y_k的。
　　請注意這是一種抽象的寫法。當你有實際的機器人，實際的感測器時，方程的形式就會變得具體，也就是所謂的引數化。例如，當我們關心機器人空間位置時，可以取x_k = [x,y,z]_k。進而，機器人攜帶了里程計，能夠得到兩個時間間隔中的相對運動，像這樣\Delta x_k=[\Delta x, \Delta y, \Delta z]_k，那麼運動方程就變為：
x_{k+1}=x_k+\Delta x_k+w_k
　　同理，觀測方程也隨感測器的具體資訊而變。例如鐳射感測器可以得到空間點離機器人的距離和角度，記為y_k=[r,\theta]_k，那麼觀測方程為：
\[{\left[ \begin{array}{l}r\\\theta \end{array} \right]_k} = \left[ \begin{array}{l}\sqrt {{{\left\| {{x_k} - {L_k}} \right\|}_2}} \\{\tan ^{ - 1}}\frac{{{L_{k,y}} - {x_{k,y}}}}{{{L_{k,x}} - {x_{k,x}}}}\end{array} \right] + {n_k}\]　　其中L_k=[L_{k,x},L_{k,y}]是一個2D路標點。

　　舉這幾個例子是為了說明，運動方程和觀測方程具體形式是會變化的。但是，我們想討論更一般的問題：當我不限制感測器的具體形式時，能否設計一種方式，從已知的u,y（輸入和觀測資料）從，估計出x
呢？

　　這就是最一般的狀態估計問題。我們會根據f,g是否線性，把它們分為線性/非線性系統。同時，對於噪聲w,n，根據它們是否為高斯分佈，分為高斯/非高斯噪聲系統。最一般的，也是最困難的問題，是非線性-非高斯(NLNG, Nonlinear-Non Gaussian)的狀態估計。下面先說最簡單的情況：線性高斯系統。

2. 線性高斯系統(LG，Linear Gaussian)


　　線上性高斯系統中，運動方程、觀測方程是線性的，且兩個噪聲項服從零均值的高斯分佈。這是最簡單的情況。簡單在哪裡呢？主要是因為高斯分佈經過線性變換之後仍為高斯分佈。而對於一個高斯分佈，只要計算出它的一階和二階矩，就可以描述它（高斯分佈只有兩個引數\mu, \Sigma
）。
　　線性系統形式如下：
\left\{\begin{array}{l}{x_k} = {A_{k - 1}}{x_{k - 1}} + {u_k} + {w_k}\\{y_k} = {C_k}{x_k} + {n_k}\\{w_k}\sim N\left( {0,{Q_k}} \right)\\{n_k}\sim N(0,{R_k})\end{array} \right. 　　其中Q_k,R_k是兩個噪聲項的協方差矩陣。A,C為轉移矩陣和觀測矩陣。
　　對LG系統，可以用貝葉斯法則，計算x的後驗概率分佈——這條路直接通向卡爾曼濾波器。卡爾曼是線性系統的遞推形式（recursive，也就是從x_{k-1}估計x_k）的無偏最優估計。由於解釋EKF和UKF都得用它，所以我們來推一推。如果讀者不感興趣，可以跳過公式推導環節。
　　符號：用\hat{x}表示x的後驗概率，用\[\tilde x\]表示它的先驗概率。因為系統是線性的，噪聲是高斯的，所以狀態也服從高斯分佈，需要計算它的均值和協方差矩陣。記第k時刻的狀態服從：x_k\sim N({{\bar x}_k},{P_k})

　　我們希望得到狀態變數x的最大後驗估計（MAP，Maximize a Posterior），於是計算：
\[\begin{array}{ccl}\hat x &=& \arg \mathop {\max }\limits_x p\left( {x|y,v} \right)\\ &=& \arg \max \frac{{p\left( {y|x,v} \right)p\left( {x|v} \right)}}{{p\left( {y|v} \right)}} \\ &=& \arg \max p(y|x)p\left( {x|v} \right)\end{array}\]
　　第二行是貝葉斯法則，第三行分母和x無關所以去掉。
　　第一項即觀測方程，有： \[p\left( {y|x} \right) = \prod\limits_{k = 0}^K {p\left( {{y_k}|{x_k}} \right)} \]　　很簡單。
　　第二項即運動方程 \[p\left( {x|v} \right) = \prod\limits_{k = 0}^K {p\left( {{x_k}|{x_{k - 1}},v_k} \right)} \]　　也很簡單。
　　現在的問題是如何求解這個最大化問題。對於高斯分佈，最大化問題可以變成最小化它的負對數。當我對一個高斯分佈取負對數時，它的指數項變成了一個二次項，而前面的因子則變為一個無關的常數項，可以略掉（這部分我不敲了，有疑問的同學可以問）。於是，定義以下形式的最小化函式：

\[\begin{array}{l}{J_{y,k}}\left( x \right) = \frac{1}{2}{\left( {{y_k} - {C_k}{x_k}} \right)^T}R_k^{ - 1}\left( {{y_k} - {C_k}{x_k}} \right)\\{J_{v,k}}\left( x \right) = \frac{1}{2}{\left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} \right)^T}Q_k^{ - 1}\left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} \right)\end{array}\]　　那麼最大後驗估計就等價於：
\[\hat x = \arg \min \sum\limits_{k = 0}^K {{J_{y,k}} + {J_{v,k}}} \]
　　這個問題現在是二次項和的形式，寫成矩陣形式會更加清晰。定義：
\[\begin{array}{l}z = \left[ \begin{array}{l}{x_0}\\{v_1}\\ \vdots \\{v_K}\\{y_0}\\ \vdots \\{y_K}\end{array} \right],x = \left[ \begin{array}{l}{x_0}\\ \vdots \\{x_K}\end{array} \right]\\H = \left[ {\begin{array}{*{20}{c}}1&{}&{}&{}\\{ - {A_0}}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - {A_{K - 1}}}&1\\{{C_0}}&{}&{}&{}\\{}& \ddots &{}&{}\\{}&{}& \ddots &{}\\{}&{}&{}&{{C_K}}\end{array}} \right],W = \left[ {\begin{array}{*{20}{c}}{{P_0}}&{}&{}&{}&{}&{}&{}\\{}&{{Q_1}}&{}&{}&{}&{}&{}\\{}&{}& \ddots &{}&{}&{}&{}\\{}&{}&{}&{{Q_K}}&{}&{}&{}\\{}&{}&{}&{}&{{R_1}}&{}&{}\\{}&{}&{}&{}&{}& \ddots &{}\\{}&{}&{}&{}&{}&{}&{{R_K}}\end{array}} \right]\end{array}\]

　　就得到矩陣形式的，類似最小二乘的問題：
\[J\left( x \right) = \frac{1}{2}{\left( {z - Hx} \right)^T}{W^{ - 1}}\left( {z - Hx} \right)\]
　　於是令它的導數為零，得到：
\[\frac{{\partial J}}{{\partial {x^T}}} = - {H^T}{W^{ - 1}}\left( {z - Hx} \right) = 0 \Rightarrow \left( {{H^T}{W^{ - 1}}H} \right)x = {H^T}{W^{ - 1}}z\](*)
　　讀者會問，這個問題和卡爾曼濾波有什麼問題呢？事實上，卡爾曼濾波就是遞推地求解(*)式的過程。所謂遞推，就是隻用x_{k-1}來計算x_k。對(*)進行Cholesky分解，就可以推出卡爾曼濾波器。詳細過程限於篇幅就不推了，把卡爾曼的結論寫一下：
\[\begin{array}{l}{{\tilde P}_k} = {A_{k - 1}}{{\hat P}_{k - 1}}A_{k - 1}^T + {Q_k}\\{{\tilde x}_k} = {A_{k - 1}}{{\hat x}_{k - 1}} + {v_k}\\{K_k} = {{\tilde P}_k}C_k^T{\left( {{C_k}{{\tilde P}_k}C_k^T + {R_k}} \right)^{ - 1}}\\{{\hat P}_k} = \left( {I - {K_k}{C_k}} \right){{\tilde P}_k}\\{{\hat x}_k} = {{\tilde x}_k} + {K_k}\left( {{y_k} - {C_k}{{\tilde x}_k}} \right)\end{array}\]
　　前兩個是預測，第三個是卡爾曼增益，四五是校正。
　　另一方面，能否直接求解(*)式，得到\hat{x}呢？答案是可以的，而且這就是優化方法（batch optimization）的思路：將所有的狀態放在一個向量裡，進行求解。與卡爾曼濾波不同的是，在估計前面時刻的狀態（如x_1）時，會用到後面時刻的資訊（y_2,y_3等）。從這點來說，優化方法和卡爾曼處理資訊的方式是相當不同的。 增加一篇KF的說明：

一、引言

下面我們引用文獻【1】中的一段話作為本文的開始：

想象你在黃昏時分看著一隻小鳥飛行穿過濃密的叢林，你只能隱隱約約、斷斷續續地瞥見小鳥運動的閃現。你試圖努力地猜測小鳥在哪裡以及下一時刻它會出現在哪裡，才不至於失去它的行蹤。或者再想象你是二戰中的一名雷達操作員，正在跟蹤一個微弱的遊移目標，這個目標每隔10秒鐘在螢幕上閃爍一次。或者回到更遠的從前，想象你是開普勒，正試圖根據一組通過不規則和不準確的測量間隔得到的非常不精確的角度觀測值來重新構造行星的運動軌跡。在所有這些情況下，你都試圖根據隨對問變化並且帶有噪聲的觀察資料去估計物理系統的狀態（例如位置、速度等等）。這個問題可以被形式化表示為時序概率模型上的推理，模型中的轉移模型描述了運動的物理本質，而感測器模型則描述了測量過程。為解決這類問題，人們發展出來了一種特殊的表示方法和推理演算法——卡爾曼濾波。

二、基本概念

回想一下HMM的基本模型（如下圖所示），其中塗有陰影的圓圈（yt-2,yt-1,yt）相當於是觀測變數，空白圓圈（xt-2,xt-1,xt）相當於是隱變數。這其實揭示了卡爾曼濾波與HMM之間擁有很深的淵源。回到剛剛提及的那幾個例子，你所觀測到的物體狀態（例如雷達中目標的位置或者速度）相當於是對其真實狀態的一種估計（因為觀測的過程中必然存在噪聲），用數學語言來表述就是P(yt | xt)，這就是模型中的測量模型或測量概率（Measurement Probability）。另外一方面，物體當前的（真實）狀態應該與其上一個觀測狀態相關，即存在這樣的一個分佈P(xt | xt-1)，這就是模型中的轉移模型或轉移概率（Transition Probability）。當然，HMM中隱變數必須都是離散的，觀測變數並無特殊要求。

而從訊號處理的角度來講，濾波是從混合在一起的諸多訊號中提取出所需訊號的過程[2]。例如，我們有一組含有噪聲的行星執行軌跡，我們希望濾除其中的噪聲，估計行星的真實運動軌跡，這一過程就是濾波。如果從機器學習和資料探勘的角度來說，濾波是一個理性智慧體為了把握當前狀態以便進行理性決策所採取的行動[1]。比如，前兩天我們沒出門，但是我們可以從房間裡觀察路上的行人有沒有打傘（觀測狀態）來估計前兩天有沒有下雨（真實狀態）。基於這些情況，現在我們要來決策今天（是否會有雨以及）外出是否需要打傘，這個過程就是濾波。讀者應該注意把握上面兩個定義的統一性。

所謂估計就是根據測量得出的與狀態X(t) 有關的資料Y(t) = h[X(t)] + V(t) 解算出X(t)的計算值，其中隨機向量V(t) 為測量誤差，稱為X的估計，Y 稱為 X 的測量。因為是根據Y(t) 確定的．所以 是Y(t) 的函式。若 是Y 的線性函式，則  稱作 X 的線性估計。設在 [t0,t1] 時間段內的測量為Y，相應的估計為，則

  • 當t = t1 時， 稱為X(t)的估計；
  • 當t > t1 肘，稱為X(t)的預測；
  • 當t < t1 時， 稱為X(t)的平滑。

最優估計是指某一指標函式達到最值時的估計。卡爾曼濾波就是一種線性最優濾波器。

因為後面會用到，這裡我們補充一下關於協方差矩陣的概念。

設 n 維隨機變數（X1,X2, …,Xn）的2階混合中心距

σij= cov(Xi,Xj) = E[(Xi-E(Xi))(Xj-E(Xj))],  (i,j = 1, 2, …, n)

都存在，則稱矩陣


為 n 維隨機變數（X1,X2, …,Xn）的協方差矩陣，協方差矩陣是一個對稱矩陣，而且對角線是各個維度的方差。

維基百科中還給出了協方差矩陣的一些重要性質，例如下面這兩條（此處不做具體證明）。後續的內容會用到其中的第一條。


三、卡爾曼濾波的方程推導

直接從數學公式和概念入手來考慮卡爾曼濾波無疑是一件非常枯燥的事情。為了便於理解，我們仍然從一個現實中的例項開始下面的介紹，這一過程中你所需的預備知識僅僅是高中程度的物理學內容。

假如現在有一輛在路上做直線運動的小車（如下所示），該小車在 t 時刻的狀態可以用一個向量來表示，其中pt 表示他當前的位置，vt表示該車當前的速度。當然，司機還可以踩油門或者剎車來給車一個加速度ut，ut相當於是一個對車的控制量。顯然，如果司機既沒有踩油門也沒有踩剎車，那麼ut就等於0。此時車就會做勻速直線運動。


如果我們已知上一時刻 t-1時小車的狀態，現在來考慮當前時刻t 小車的狀態。顯然有


易知，上述兩個公式中，輸出變數都是輸入變數的線性組合，這也就是稱卡爾曼濾波器為線性濾波器的原因所在。既然上述公式表徵了一種線性關係，那麼我們就可以用一個矩陣來表示它，則有

如果另其中的


則得到卡爾曼濾波方程組中的第一條公式——狀態預測公式，而F就是狀態轉移矩陣，它表示我們如何從上一狀態來推測當前狀態。而B則是控制矩陣，它表示控制量u如何作用於當前狀態。

  （1）

上式中x頂上的hat表示為估計值（而非真實值）。等式左端部分的右上標“-”表示該狀態是根據上一狀態推測而來的，稍後我們還將對其進行修正以得到最優估計，彼時才可以將“-”去掉。

既然我們是在對真實值進行估計，那麼就理應考慮到噪聲的影響。實踐中，我們通常都是假設噪聲服從一個0均值的高斯分佈，即Noise~Guassian(0,σ)。例如對於一個一維的資料進行估計時，若要引入噪聲的影響，其實只要考慮其中的方差σ即可。當我們將維度提高之後，為了綜合考慮各個維度偏離其均值的程度，就需要引入協方差矩陣。

回到我們的例子，系統中每一個時刻的不確定性都是通過協方差矩陣 Σ 來給出的。而且這種不確定性在每個時刻間還會進行傳遞。也就是說不僅當前物體的狀態（例如位置或者速度）是會（在每個時刻間）進行傳遞的，而且物體狀態的不確定性也是會（在每個時刻間）進行傳遞的。這種不確定性的傳遞就可以用狀態轉移矩陣來表示，即（注意，這裡用到了前面給出的關於協方差矩陣的性質）


但是我們還應該考慮到，預測模型本身也並不絕對準確的，所以我們要引入一個協方差矩陣 Q 來表示預測模型本身的噪聲（也即是噪聲在傳遞過程中的不確定性），則有

  （2）

這就是卡爾曼濾波方程組中的第二條公式，它表示不確定性在各個時刻間的傳遞關係。

繼續我們的小汽車例子。你應該注意到，前面我們所討論的內容都是圍繞小汽車的真實狀態展開的。而真實狀態我們其實是無法得知的，我們只能通過觀測值來對真實值進行估計。所以現在我們在路上佈設了一個裝置來測定小汽車的位置，觀測到的值記為V(t)。而且從小汽車的真實狀態到其觀測狀態還有一個變換關係，這個變換關係我們記為h(•)，而且這個h(•)還是一個線性函式。此時便有（該式前面曾經給出過）

Y(t) = h[X(t)] + V(t)

其中V(t)表示觀測的誤差。既然h(•)還是一個線性函式，所以我們同樣可以把上式改寫成矩陣的形式，則有

Yt=Hxt + v

就本例而言，觀測矩陣 H = [1 0]，這其實告訴我們x和Z的維度不一定非得相同。在我們的例子中，x是一個二維的列向量，而Z只是一個標量。此時當把x與上面給出的H相乘就會得出一個標量，此時得到的Y 就是x中的首個元素，也就是小車的位置。同樣，我們還需要用一個協方差矩陣R來取代上述式子中的v來表示觀測中的不確定性。當然，由於Z是一個一維的值，所以此時協方差矩陣R也只有一維，也就是隻有一個值，即觀測噪聲之高斯分佈的引數σ。如果我們有很多裝置來測量小汽車的不同狀態，那麼Z就會是一個包含所有觀測值的向量。

接下來要做的事情就是對前面得出的狀態估計進行修正，具體而言就是利用下面這個式子

    （4）

直觀上來說，上式並不難理解。前面我們提到，是根據上一狀態推測而來的，那麼它與“最優”估計值之間的差距現在就是等式右端加號右側的部分。表示實際觀察值與預估的觀測值之間的殘差。這個殘差再乘以一個係數K就可以用來對估計值進行修正。K稱為卡爾曼係數，它也是一個矩陣，它是對殘差的加權矩陣，有的資料上稱其為濾波增益陣。

   （3）

上式的推導比較複雜，有興趣深入研究的讀者可以參閱文獻【2】（P35~P37）。如果有時間我會在後面再做詳細推導。但是現在我們仍然可以定性地對這個係數進行解讀：濾波增益陣首先權衡預測狀態協方差矩陣Σ 和觀測值矩陣R的大小，並以此來覺得我們是更傾向於相信預測模型還是詳細觀測模型。如果相信預測模型多一點，那麼這個殘差的權重就會小一點。反之亦然，如果相信觀察模型多一點，這個殘差的權重就會大一點。不僅如此，濾波增益陣還負責把殘差的表現形式從觀測域轉換到了狀態域。例如本題中觀測值Z 僅僅是一個一維的向量，狀態 x 是一個二維的向量。所以在實際應用中，觀測值與狀態值所採用的描述特徵或者單位都有可能不同，顯然直接用觀測值的殘差去更新狀態值是不合理的。而利用卡爾曼係數，我們就可以完成這種轉換。例如，在小車運動這個例子中，我們只觀察到了汽車的位置，但K裡面已經包含了協方差矩陣P的資訊（P裡面就給出了速度和位置的相關性），所以它利用速度和位置這兩個維度的相關性，從位置的殘差中推算出了速度的殘差。從而讓我們可以對狀態值 x 的兩個維度同時進行修正。

最後，還需對最優估計值的噪聲分佈進行更新。所使用的公式為

  （5）

至此，我們便獲得了實現卡爾曼濾波所需的全部五個公式，我在前面分別用（1）~（5）的標記進行了編號。我現在把它們再次羅列出來：


我們將這五個公式分成預測組和更新組。預測組總是根據前一個狀態來估計當前狀態。更新組則根據觀測資訊來對預測資訊進行修正，以期達到最優估計之目的。

四、一個簡單的例項

當然，你可能困惑於卡爾曼濾波是否真的有效。下面利用文獻[4]中給出的例子（為提升顯示效果，筆者略有修改）來演示卡爾曼濾波的威力。這個例子模擬質點進行勻速直線運動（速度為1），然後引入一個非常大的噪聲，再用卡爾曼濾波來對質點的運動狀態進行軌跡。注意是勻速直線運動，所以其中不含有控制變數。

[plain] view plain copy print?在CODE上檢視程式碼片派生到我的程式碼片
  1. Z=(1:100); %觀測值    
  2. noise=randn(1,100); %方差為1的高斯噪聲    
  3. Z=Z+noise;    
  4. X=[0; 0]; %狀態    
  5. Sigma = [1 0; 0 1]; %狀態協方差矩陣    
  6. F=[1 1; 0 1]; %狀態轉移矩陣    
  7. Q=[0.0001, 0; 0 0.0001]; %狀態轉移協方差矩陣    
  8. H=[1 0]; %觀測矩陣    
  9. R=1; %觀測噪聲方差    
  10. figure;    
  11. hold on;    
  12. for i=1:100    
  13.   X_ = F*X;    
  14.   Sigma_ = F*Sigma*F'+Q;    
  15.   K = Sigma_*H'/(H*Sigma_*H'+R);    
  16.   X = X_+K*(Z(i)-H*X_);    
  17.   Sigma = (eye(2)-K*H)*Sigma_;    
  18.   plot(X(1), X(2), '.','MarkerSize',10); %畫點，橫軸表示位置，縱軸表示速度    
  19. end  
  20. plot([0,100],[1,1],'r-');   
Z=(1:100); %觀測值  
noise=randn(1,100); %方差為1的高斯噪聲  
Z=Z+noise;  
  
X=[0; 0]; %狀態  
Sigma = [1 0; 0 1]; %狀態協方差矩陣  
F=[1 1; 0 1]; %狀態轉移矩陣  
Q=[0.0001, 0; 0 0.0001]; %狀態轉移協方差矩陣  
H=[1 0]; %觀測矩陣  
R=1; %觀測噪聲方差  
  
figure;  
hold on;  
  
for i=1:100  
  
  X_ = F*X;  
  Sigma_ = F*Sigma*F'+Q;  
  K = Sigma_*H'/(H*Sigma_*H'+R);  
  X = X_+K*(Z(i)-H*X_);  
  Sigma = (eye(2)-K*H)*Sigma_;  
    
  plot(X(1), X(2), '.','MarkerSize',10); %畫點，橫軸表示位置，縱軸表示速度  
end

plot([0,100],[1,1],'r-');

下圖給出了上述程式碼的執行結果。可見經過最開始的幾次迭代後，質點運動的狀態估計就回到了正確軌跡上，而且估計的結果基本圍繞在真實值附近，效果還是很理想的。


五、後記

本文相當於是卡爾曼濾波的入門文，在下一篇中我將深入挖掘一些本文未曾提及的細節（以及一些本文沒有給出的數學上的推導）。如果你想將自己對卡爾曼濾波的認識提升到一個更高的檔次，推薦你關注我的後續博文。Cheers~ 

如果你是影象處理的同道中人，歡迎加入影象處理學習群（529549320）。為保證本群質量，入群前請先閱讀群規（即本部落格置頂文章），並在部落格置頂帖中留言，否則將不予入群。Cheers~


參考文獻：

【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.

【2】秦永元，張洪鉞，汪叔華，卡爾曼濾波與組合導航原理，西北工業大學出版社

【3】徐亦達博士關於卡爾曼濾波的公開課，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

【4】卡爾曼濾波的原理以及在MATLAB中的實現，http://blog.csdn.net/revolver/article/details/37830675


3. 擴充套件卡爾曼濾波器

　　線性高斯系統當然性質很好啦，但許多現實世界中的系統都不是線性的，狀態和噪聲也不是高斯分佈的。例如上面舉的鐳射觀測方程就不是線性的。當系統為非線性的時候，會發生什麼呢？
　　一件悲劇的事情是：高斯分佈經過非線性變換後，不再是高斯分佈。而且，是個什麼分佈，基本說不上來。（攤手）
　　如果沒有高斯分佈，上面說的那些都不再成立了。於是EKF說，嘛，我們睜一隻眼閉一隻眼，用高斯分佈去近似它，並且，在工作點附近對系統進行線性化。當然這個近似是很成問題的，有什麼問題我們之後再說。
　　EKF的做法主要有兩點。其一，在工作點附近\[{{\hat x}_{k - 1}},{{\tilde x}_k}\]，對系統進行線性近似化：
\[\begin{array}{l}f\left( {{x_{k - 1}},{v_k},{w_k}} \right) \approx f\left( {{{\hat x}_{k - 1}},{v_k},0} \right) + \frac{{\partial f}}{{\partial {x_{k - 1}}}}\left( {{x_{k - 1}} - {{\hat x}_{k - 1}}} \right) + \frac{{\partial f}}{{\partial {w_k}}}{w_k}\\g\left( {{x_k},{n_k}} \right) \approx g\left( {{{\tilde x}_k},0} \right) + \frac{{\partial g}}{{\partial {x_k}}}{n_k}\end{array}\]
　　這裡的幾個偏導數，都在工作點處取值。於是呢，它就被活生生地當成了一個線性系統。
　　第二，在線性系統近似下，把噪聲項和狀態都當成了高斯分佈。這樣，只要估計它們的均值和協方差矩陣，就可以描述狀態了。經過這樣的近似之後呢，後續工作都和卡爾曼濾波是一樣的了。所以EKF是卡爾曼濾波在NLNG系統下的直接擴充套件（所以叫擴充套件卡爾曼嘛）。EKF給出的公式和卡爾曼是一致的，用線性化之後的矩陣去代替卡爾曼濾波器裡的轉移矩陣和觀測矩陣即可。
\[\begin{array}{l}{{\tilde P}_k} = {F_{k - 1}}{{\hat P}_{k - 1}}F_{k - 1}^T + Q_k'\\{{\tilde x}_k} = f\left( {{{\hat x}_{k - 1}},{v_k},0} \right)\\{K_k} = {{\tilde P}_k}G_k^T{\left( {{G_k}{{\tilde P}_k}G_k^T + R_k'} \right)^{ - 1}}\\{{\hat P}_k} = \left( {I - {K_k}{G_k}} \right){{\tilde P}_k}\\{{\hat x}_k} = {{\tilde x}_k} + {K_k}\left( {{y_k} - g({{\tilde x}_k},0)} \right)\end{array}\]　　 其中 \[F_{k-1} = {\left. {\frac{{\partial f}}{{\partial {x_{k - 1}}}}} \right|_{{{\hat x}_{k - 1}}}},{G_k} = {\left. {\frac{{\partial f}}{{\partial {x_k}}}} \right|_{{{\tilde x}_k}}}\]

　　這樣做聽起來還是挺有道理的，實際上也是能用的，但是問題還是很多的。
　　考慮一個服從高斯分佈的變數x \sim N(0,1)，現在y=x^2，問y服從什麼分佈？
　　我概率比較差，不過這個似乎是叫做卡爾方布。y應該是下圖中k=1那條線。
　　但是按照EKF的觀點，我們要用一個高斯分佈去近似y。假設我們取樣時得到了一個x=0.5，那麼就會近似成一個均值為0.25的高斯分佈，然而卡方分佈的期望應該是1。……但是各位真覺得k=1那條線像哪個高斯分佈嗎？
　　所以EKF面臨的一個重要問題是，當一個高斯分佈經過非線性變換後，如何用另一個高斯分佈近似它？按照它現在的做法，存在以下的侷限性：（注意是濾波器自己的侷限性，還沒談在SLAM問題裡的侷限性）。
  1. 即使是高斯分佈，經過一個非線性變換後也不是高斯分佈。EKF只計算均值與協方差，是在用高斯近似這個非線性變換後的結果。（實際中這個近似可能很差）。
  2. 系統本身線性化過程中，丟掉了高階項。
  3. 線性化的工作點往往不是輸入狀態真實的均值，而是一個估計的均值。於是，在這個工作點下計算的F,G，也不是最好的。
  4. 在估計非線性輸出的均值時，EKF算的是\mu_y=f(\mu_x)的形式。這個結果幾乎不會是輸出分佈的真正期望值。協方差也是同理。
那麼，怎麼克服以上的缺點呢？途徑很多，主要看我們想不想維持EKF的假設。如果我們比較乖，希望維持高斯分佈假設，可以這樣子改：
  1. 為了克服第3條工作點的問題，我們以EKF估計的結果為工作點，重新計算一遍EKF，直到這個工作點變化夠小。是為迭代EKF（Iterated EKF, IEKF）。
  2. 為了克服第4條，我們除了計算\mu_y=f(\mu_x)，再計算其他幾個精心挑選的取樣點，然後用這幾個點估計輸出的高斯分佈。是為Sigma Point KF（SPKF，或UKF）。

　　如果不那麼乖，可以說：我們不要高斯分佈假設，憑什麼要用高斯去近似一個長得根本不高斯的分佈呢？於是問題變為，丟掉高斯假設後，怎麼描述輸出函式的分佈就成了一個問題。一種比較暴力的方式是：用足夠多的取樣點，來表達輸出的分佈。這種蒙特卡洛的方式，也就是粒子濾波的思路。
　　如果再進一步，可以丟棄濾波器思路，說：為什麼要用前一個時刻的值來估計下一個時刻呢？我們可以把所有狀態看成變數，把運動方程和觀測方程看成變數間的約束，構造誤差函式，然後最小化這個誤差的二次型。這樣就會得到非線性優化的方法，在SLAM裡就走向圖優化那條路上去了。不過，非線性優化也需要對誤差函式不斷地求梯度，並根據梯度方向迭代，因而區域性線性化是不可避免的。
　　可以看到，在這個過程中，我們逐漸放寬了假設。

再給一篇EKF的文章:原文：

已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著重於將卡爾曼濾波應用於非線性系統。

　　為什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因為卡爾曼濾波器從一開始就是為線性系統設計的演算法，不能用於非線性系統中

相關推薦

運動目標跟蹤（一）--搜尋演算法預測模型之KF,EKF,UKF

這裡先總體介紹下，原文轉自： 任何感測器，鐳射也好，視覺也好，整個SLAM系統也好，要解決的問題只有一個：如何通過資料來估計自身狀態。每種感測器的測量模型不一樣，它們的精度也不一樣。換句話說，狀態估計問題，也就是“如何最好地使用感測器資料”。可以說，SLAM是狀態估計的一

目標檢測（一）--Objectness演算法總體理解，整理及總結

1.原始碼下載及轉換為VS2012 WIN32版本。 http://www.cnblogs.com/larch18/p/4560690.html 2.原文： http://wenku.baidu.com/link?url=ls5vmcYnsUdC-ynKdBzWgx

tensorflow利用預訓練模型進行目標檢測（一）：預訓練模型的使用

err sync numpy sna sta porting trac git int32 一、運行樣例 官網鏈接：https://github.com/tensorflow/models/blob/master/research/object_detection/obje

運動目標跟蹤（十一）--CN及CSK跟蹤原理

原文： http://blog.csdn.net/hlinghling/article/details/44308199 基於自適應顏色屬性的目標追蹤（就是CN） Adaptive Color Attributes for Real-Time Visual Trackin

【萬能搜尋】萬能DFS之全排列（一）——普通演算法

DFS相信大家都很熟悉，下面就給出一種用DFS實現的演算法。 全排列 大家對於全排列是很熟悉的，比如123的全排列就有： 123 132 213 231 312 321 這六種。 現在，我們來設計一個演算法，來

OpenCV目標跟蹤（四）-運動模板

OpenCV中運動與跟蹤這一章節中，在前面的介紹中，主要給出了LK光流法，以及基於概率統計，視窗搜尋的meanshif演算法以及meanshif演算法的改進版-camshift演算法，這兩天主要在看運動模板的跟蹤運動方法，下面就簡要的介紹下。 運動模板的方法是

運動目標跟蹤（十六）--OAB，SemiT,BSBT跟蹤

原文： http://blog.csdn.net/ikerpeng/article/details/18985573 文章：on-line boosting and vision  這是cvpr2006年的文章，非常經典的一篇文章。還有原始碼和資料集下面記錄一下我讀這篇文

運動目標跟蹤（十四）--MIL跟蹤

原文： http://blog.csdn.net/ikerpeng/article/details/19235391 文章：Robust object tracking with on-line  multiple instance learning Boris Baben

目標檢測演算法（一）——常見演算法比較

一、分類：幀間差分法、背景減除法和光流法。 (1)背景減除法通過統計前若千巾貞的變化情況,從而學習背景擾動的規律。此類演算法的缺點是由於通常需要緩衝若干幀頻來學習背景,因此往往需要消耗大量的記憶體,這使其使用範圍受到了限制。此外,對於大範圍的背景擾動,此類演算法的檢測

使用OpenCv實現人臉跟蹤（一）

簡介 OpenCV是一個基於BSD許可開源發行的跨平臺計算機視覺庫。擁有C++，Python和Java介面，並且支援Windows, Linux, Mac OS, iOS 和 Android系統。實現了影象處理和計算機視覺方面的很多通用演算法。 主要模組以及功能

《機器學習實戰》（一）knn演算法

K最近鄰（k-Nearest Neighbor，KNN）分類演算法可以說是最簡單的機器學習演算法了。它採用測量不同特徵值之間的距離方法進行分類。它的思想很簡單：存在一個樣本資料集合，也稱作訓練樣本集，並且樣本集中每個資料都存在標籤，即我們知道樣本集中每一個數據與所屬分類的對應關係。輸入沒有標

演算法（一）：演算法簡介

演算法簡介 ● 什麼是演算法        ➢一系列的計算步驟，用來將輸入資料轉化成輸出結果 ● 演算法的意義        ➢用於解決特定的問題        ➢解決同一個問題的不同演算法的效率常常相差非常大，這種差距的影響往往比硬體和軟體方面的差距還要大 ● 比較常聽到的

目標檢測（一）——R-CNN

一 R-CNN R-CNN（R. Girshick et al., 2014）是基於region proposal方法的目標檢測算法系列開山之作，其先進行區域搜尋，然後再對候選區域進行分類。在R-CNN中，選用Selective search方法（J.R.R. Uijli

雙攝像頭的實時視訊拼接及目標跟蹤（二）

視訊拼接演算法 視訊拼接思想及總體框圖 影象拼接是視訊拼接的基礎，是將多張影象生成一張全景圖，衡量的指標主要在於全景圖的質量，而對拼接速度的要求並不高。對於視訊拼接而言，視訊流的輸入是源源不斷的，需要將多個攝像頭採集的圖片進行拼接，每個攝像頭的影象幀序列必須一一

雙攝像頭的實時視訊拼接及目標跟蹤（三）

實時視訊拼接 初步視訊拼接虛影問題的探討 經過上述方案處理之後的初步拼接視訊，將會存在目前視訊拼接技術中一個普遍的問題，即重合區域存在運動物體產生虛影。效果圖如下， 產生偽影的影象幀 可以看到，重疊區域內，存在運動物體時，出現了虛影，並且都

雙攝像頭的實時視訊拼接及目標跟蹤（五）

實時視訊拼接速度的優化方案 優化方案，提高拼接速度 根據之前章節的方法，進行初步視訊拼接得到的效果，其速度並不是最快，需要進行一定的優化。通過實驗，得到下面幾種優化方式。 1.程式碼執行的環境是在VS2017上執行的，其中有兩種執行模式，分別為Debug模式和R

手把手教你如何用objection detection API實現實時目標檢測（一）

本系列文章共分為三篇，包括環境配置、對影象的目標檢測和利用攝像頭實現實時檢測三個部分，本文主要介紹相關的環境配置部分。 step1：準備相關檔案 在這裡我們使用的是GPU版本的TensorFlow，因此在此之前我們需要下載所需要的相關資料。主要有如下幾部分： CUDA9.

決策樹（一）——基礎演算法

決策樹 定義 根據wikipedia的定義，決策樹學習方法是一種通過決策樹實現從某幾個特徵屬性（分枝）到目標值（葉節點）的預測模型。 一種實現方法是貪心演算法：資料來源按照屬性測試條件分割為子集，然後在生成的每個子集上遞迴執行遞迴分割過程，直到該節點所有子集具有相同的目標變數。

演算法快學筆記（一）：演算法入門

1. 演算法的定義 “演算法”一詞在不同的書籍以及網站上可能會存在一些差異，但是下面的定義個人覺得最為貼切： 1. 演算法代表著用系統的方法描述解決問題的策略機制 2. 能夠對一定規範的輸入，在有限時間內獲得所要求的輸出 3. 一個演算法的優劣可以用空間複雜度與時間複雜度來衡量

MATLAB R2018a 全域性優化工具箱學習（一）遺傳演算法

Global Optimization Toolbox   在MATLAB R2018a 中發現了全域性優化的工具箱以及增加的資料和機器學習工具箱，對於求解全域性最優值很是方便。 MATLAB Global Optimization Toolbox 學習： 全域性