由淺入深的擴展卡爾曼濾波器教程

本篇譯文翻譯自 The Extended Kalman Filter : An Interactive Turorial for Non-Experts. 原文

本文慣例及說明 :

  1. 譯文中的Demo請至原文處運行
  2. 最好具有高等數學和線性代數基礎
  3. 本文基本原理和飛控中的EKF代碼實現之間是有差距的

在接觸 OpenPilotPixhawk飛控時,我經常遇到關於EKF的參考。我谷歌搜索 EKF,引導我到不同的網頁和參考論文,但其中大部分都讓我難以理解 [ 1 ] ^{[1]} 。所以,我決定親自創建關於EKF基本原理的教程。這套教程假定你具有高中數學水平,並會在適當時候引入高等數學中的線性代數,而不是假定你已經瞭解它們。我會從一些簡單的例子和標準卡爾曼濾波開始,並最終讓你理解 EKF。

Part 1 : 一個簡單的例子

想像一架飛機正在降落。雖然我們需要擔心的事情有好多,如風速、燃料等等,但最重要的是飛機的高度(海平面高度)。做一個簡單的近似,我們認爲當前高度是上一時刻高度的一部分。舉例來說,如果我們每次觀測飛機都下降上一時刻高度的2%,那麼當前時刻的高度是上一時刻的 98% :
a l t i t u d e c u r r e n t _ t i m e = 0.98 a l t i t u d e p r e v i o u s _ t i m e altitude_{current\_time}=0.98*altitude_{previous\_time}
  工程師將這種一個量根據上一時刻的值來定義的情形稱爲遞歸:爲了計算當前值,我們必須「返回」上一時刻的值。最終我們迴歸到一些起始的「基值」,比如已知的起始高度。下圖的Demo中移動滑動條,查看飛機以不同的百分比做出高度改變。
(譯者注:滑動時 0.98 係數發生變化,高度—時間曲線發生形狀變化)
遞歸係數改變


[1]其中兩個例外是 Kalman Filtering for Dummiesthe Wikipedia page。另外一個非常形象化的教程是 本篇教程 。如果你懂得很多數學概念,請查看這篇教程而不用在我的教程中花費任何時間。本教程受益於 OpenPilot 和 DIY Drones 社區有建設性的評論(Tim Wilkin糾正我我表述中很多不精確的地方)

Part 2 : 考慮噪聲

當然,真實世界中的測量,比如高度,是從 GPS 或者氣壓計等傳感器中獲得的。這些傳感器精度各異 [ 2 ] ^{[2]} 。如果傳感器具有恆定的偏置的誤差,我們可以簡單得加上或者減去這個偏置值來獲得高度。然而,一般來說,傳感器的精度每時每刻都是不可預測的,這使觀測到的傳感器讀數是真實高度疊加上噪聲。
o b s e r v e d _ a l t i t u d e c u r r e n t _ t i m e = a l t i t u d e c u r r e n t _ t i m e + n o i s e c u r r e n t _ t i m e observed\_altitude_{current\_time}=altitude_{current\_time}+noise_{current\_time}

移動滑動條查看噪聲對觀測到的高度的影響,噪聲被表示爲觀測高度範圍的百分比。(譯者注:拖動滑動條可以看到隨着噪聲加大,曲線變得越來越不光滑)
file


[2] 舉個例子,Garmin公佈其氣壓高度計得讀數精度爲 「 10 英尺(合適校準下)」。所以,如果高度計得讀數是 1000 英尺,那麼我們真實得高度可能是 990~1010 英尺高度中的任何值。

Part 3 : 將 Part1 和 Part2 合併

現在我們有了兩個等式來描述飛機的狀態 :
a l t i t u d e c u r r e n t _ t i m e = 0.98 a l t i t u d e p r e v i o u s _ t i m e altitude_{current\_time}=0.98*altitude_{previous\_time}
o b s e r v e d _ a l t i t u d e c u r r e n t _ t i m e = a l t i t u d e c u r r e n t _ t i m e + n o i s e c u r r e n t _ t i m e observed\_altitude_{current\_time}=altitude_{current\_time}+noise_{current\_time}

這些公式很容易理解,但是他們並不適合除了我們飛機高度示例之外的系統。爲了讓這些等式更通用化,工程師採取相似的數學慣例,即使用 x x y y z z 命名變量,$a $ 、 b b 等命名常量,並且用角標 k k 代表時間 [ 3 ] ^{[3]} 。所以,我們的等式變成:
x k = a x k 1 x_{k}=ax_{k - 1}
z k = x k + v k z_{k}=x_{k}+v_{k}

其中 x k x_k 是我們系統的當前狀態, x k 1 x_k-1 是上一時刻的狀態, a 是常量。 z k z_k 是對系統的當前觀測值,並且 v k v_k 是當前測量噪聲。卡爾曼濾波器如此流行的一個原因是它允許我們,根據觀測值 z k z_k 、常量 a a 、整體測量噪聲 v v 來獲得當前狀態 x k x_k 的一個良好估計。更進一步,我們也應當考慮到飛機的實際高度變化可能不是完美光滑的。因爲任何做過飛機的人都可以告訴你,飛機在着陸過程中一般會經歷一定數量的擾流。這個擾流可以定義爲噪聲,並且可以作爲另外一種噪聲源(系統過程噪聲,不同於上文中的傳感器測量噪聲) :

a l t i t u d e c u r r e n t _ t i m e = 0.98 a l t i t u d e p r e v i o u s _ t i m e + t u r b u l e n c e c u r r e n t _ t i m e altitude_{current\_time}=0.98*altitude_{previous\_time}+turbulence_{current\_time}

更一般的可以寫爲 :
x k = a x k 1 + w k x_k = a x_{k-1} + w_k

其中 w k w_k 稱爲過程噪聲,就像擾流,因爲它是過程中的固有部分,而不是觀測或者測量的一部分。爲了將焦點聚集在其它主題,我們將會先忽略過程噪聲,但是我們在 Part13 傳感器融合 [ 4 ] ^{[4]} 的部分將會考慮過程噪聲。


[3] 爲什麼不用 t t 來表示時間,可能因爲在這裏時間被認爲是一系列離散步長序列,對此,使用腳標索引 k k 是慣例。
[4]在這裏我同時忽略了狀態等式中的控制信號,因爲對於大部分 Kalman Filter 這有點離題(就是不常用),並且當需要時可以很容易加上。

Part 4 :狀態估計

忽略過程噪聲,這裏是兩個等式,它們描述了我們正在觀測的系統的狀態 :
x k = a x k 1 + w k x_k=ax_{k-1}+w_k
z k = x k + v k z_k=x_k+v_k

因爲我們的目標是從觀測$ z$ 中獲得狀態 x x ,我們可以將第二個等式重新寫爲 :

x k = z k v k x_k = z_k - v_k

當然,問題是我們並不知道當前噪聲 v k v_k ,由其定義我們可以知道其是不可預測的。幸運的是,Kalman 對此有深刻見解,他認爲我們可以通過考慮當前觀測和上一時刻估計的狀態值來估計當前的狀態。工程師在變量上畫「小帽子」來表示它是估計量 :所以 x k ^ \hat{x_k} 是對當前狀態的估計。然後,我們能夠表達當前的狀態的估計爲 上一時刻估計值和當前觀測值的權衡折衷(譯者注:原文單詞爲tradeoff,即兩者綜合作用得出的或者理解爲動態加權平均)
x ^ k = x ^ k 1 + g k ( z k x ^ k 1 ) \hat{x}_k = \hat{x}_{k-1} + g_k(z_k - \hat{x}_{k-1})

其中 g k g_k 是表達權衡折衷的增益 [ 5 ] ^{[5]} 。我將這個等式高亮爲紅色,因爲我們會直接用它來實現 Kalman 濾波器。
現在,這看起來有點複雜,但是考慮增益項爲其極值時。對於 g k = 0 g_k=0 ,我們得到
x ^ k = x ^ k 1 + 0 ( z k x ^ k 1 ) = x ^ k 1 \hat{x}_k = \hat{x}_{k-1} + 0(z_k - \hat{x}_{k-1}) = \hat{x}_{k-1}
  換句話說,當增益爲零,觀測不起作用,我們直接從上一時刻的狀態得到當前狀態。(譯者注:一般系統狀態方程是建模得到的微分方程,卡爾曼第一條公式得到一步預報值,預測的含義便由此而來,這裏等號右邊第一個$\hat{x_k-1} 應該指一步預報值,當並沒有微分方程時,我們就令一步預報值等於上一時刻的最優估計值 \hat{x_k-1}$ 。這裏用觀測修正一步預報值時直接採用了上一時刻最優估計值,原文作者並未區分)
對於 g k = 1 g_k=1 , 我們得到
x ^ k = x ^ k 1 + 1 ( z k x ^ k 1 ) = x ^ k 1 + z k x ^ k 1 = z k \hat{x}_k = \hat{x}_{k-1} + 1(z_k - \hat{x}_{k-1}) = \hat{x}_{k-1} + z_k - \hat{x}_{k-1} = z_k

換句話說,當增益是 1 ,過去的狀態不再重要,並且我們完全從當前觀測中獲得當前狀態估計。當然,實際增益值將會落在這兩個極值之間。嘗試移動下方滑動條來查看增益對當前狀態估計的效果 :
file


[5]變量 k k 經常被用作代表增益,因爲作爲 Kalman 增益而廣爲人知。爲了對 Rudolf Kalman (卡爾曼魯道夫)表示尊重,我發現對於變量和腳標使用相同的字母令人困惑,所以,我選擇了另外一個字母 g g

Part 5 : 計算增益

現在我們有了這樣一個公式,它基於上一時刻的狀態估計 x k 1 ^ \hat{x_k-1} 、當前觀測 z k z_k 和當前增益 g k g_k 來計算當前狀態量的估計值 x ^ k \hat{x}_k

x ^ k = x ^ k 1 + g k ( z k x ^ k 1 ) \hat{x}_k = \hat{x}_{k-1} + g_k(z_k - \hat{x}_{k-1})

譯者注:展開這個公式我們可以看到卡爾曼濾波的實質:
x ^ k = 1 g k x ^ k 1 + g k z k \hat{x}_k =(1-g_k)\hat{x}_{k-1} +g_kz_k

即動態加權平均上一時刻的最優估計和當前觀測,而這個動態加權是卡爾曼濾波的聰明之處。

接下來,我們如何計算增益?答案是:非直接的,從噪聲中計算出增益。回想一下,每一次觀測都伴隨一個特定的噪聲值 :
z k = x k + v k z_k = x_k + v_k
  我們並不知道具體一次觀測的噪聲,但是我們通常知道平均噪聲 :例如,廠商公佈的傳感器精度近似告訴我們它輸出噪聲大小,我把這個值稱爲 r r 。這裏並未使用腳標,因爲噪聲並不和時間相關,而是傳感器本身的屬性。然後,我們根據 r r 能夠計算當前增益 g k g_k :
g k = p k 1 / ( p k 1 + r ) g_k = p_{k-1}/(p_{k-1} + r)
其中 p k p_k 是由遞歸計算的預測誤差 [ 6 ] ^{[6]} :
p k = ( 1 g k ) p k 1 p_k = (1 - g_k)p_{k-1}

至於狀態估計等式,讓我們想想這兩個等式的意義,然後再繼續。我們假定在上一次預測中誤差 p k 1 p_k-1 是0。然後當前增益 g k = 0 / 0 + r = 0 g_k = 0/(0+r)=0 ,並且下一狀態估計將會和當前狀態估計一模一樣。這是可以理解的,因爲如果我們的預測是準確的,我們就不應當修改預測來得到狀態估計。在另一個極端情形,假定預測誤差是 1 ,然後增益將是 g k = 1 / ( 1 + r ) g_k=1/(1+r) 。如果 $r $是0,或者我們系統傳感器中只有一點點噪聲,那麼增益將會是 1 ,並且我們新的狀態估計 x k x_k 將受到我們觀測 z k z_k 的強烈影響。 但隨着 r r 變大,增益能變得任意小。換句話說,當系統噪聲足夠嚴重,使 g k g_k 很小,糟糕的預測(這裏我覺得應該是修正或者叫做更新,而不是預測)不得不被忽略。噪聲嚴重得使我們不能校正糟糕的預測。(這裏原文作者說先不考慮系統過程噪聲,應該只有傳感器引入的觀測噪聲或者稱爲測量噪聲)
  關於第三個等式,計算預測誤差 p k p_k 遞歸自其上一時刻的值 p k 1 p_k-1 和當前增益 g k g_k 。再一次,我們使用極端情形來幫我們思考 :當 g k = 0 g_k=0 ,我們由 p k = p k 1 p_k = p_k-1 。所以,就如狀態估計一樣,一個零增益意味着預測誤差沒有更新。當 g k = 1 g_k=1 ,我們有 p k p_k =0 。因此,最大增益對應於零預測誤差,此時僅用當前觀測來更新當前狀態。


[6]專業上講, r r 實際上是噪聲信號的方差,即各個噪聲值與其平均值的平均平方距離。如果允許這種噪聲的方差隨時間變化,卡爾曼濾波器同樣有效,但在大多數應用中,它可以被假定爲常數。相似的, p k p_k 專業上講是第 k k 步估計過程的協方差。它是我們預測的平均平方誤差。事實上,正如 Tim Wilkin 所指出的,狀態是隨機變量/矢量(隨機過程的一個瞬時值),並且它根本上並沒有真實值!估計僅僅是描述狀態的過程模型的最可能的值而已。

Part 6 : 預測和更新

我們已經準備好了來運行我們的Kalman濾波器並且可以看到一些結果。首先,你可能想知道我們最初等式中的常量 a a 發生了什麼 :
x k = a x k 1 x_k = a x_{k-1}

它似乎從我們狀態估計等式中消失了 :
x ^ k = x ^ k 1 + g k ( z k x ^ k 1 ) \hat{x}_k = \hat{x}_{k-1} + g_k(z_k - \hat{x}_{k-1})

答案是,我們需要這兩個等式來估計狀態。事實上,這兩個等式,基於不同種類的信息,代表了對於狀態的估計。我們最開始的等式代表了關於狀態將會怎樣的預測,並且我們的第二個等式代表了上述預測之後的更新(修正),這基於觀測 [ 7 ] ^{[7]} 。所以我們重寫我們最開始的等式爲(加上 「小帽子」):
x ^ k = a x ^ k 1 \hat{x}_k = a \hat{x}_{k-1}

最終,我們在誤差預測中也使用常量a [ 8 ] ^{[8]} :
p k = a p k 1 a p_k = a p_{k-1} a

這兩個等式一起代表了 Kalman 濾波器的預測階段。所以我們能夠週期得預測/更新,預測/更新……,我們可以想重複多少次就多少次。



[7]專業得講,第一次估計叫做先驗估計(即觀測之前),第二次叫做後驗估計,並且大多數情況下會引入額外的上標或下標以示區分。因爲我嘗試着讓事情變得簡單,並且在你所熟悉的編程語言中編寫代碼,我避免將概念進一步複雜化

(所以沒用更準確的專業術語)


[8]正如 Zichao Zhang(張子超,是誰呀)所指出的,我們乘了兩次 a a ,因爲預測誤差 p k p_k 本身就是平方誤差。因此, p k p_k 乘以與狀態值相關的係數的平方。對於把誤差預測表達爲 a p k 1 a ap_k-1a 而不是 a 2 p k 1 a^2p_k-1 將會在Part 12中變得清晰。

Part 7 : 運行濾波器

所以,現在我們有了運行Kalman 濾波器的所有東西 :
預測
x ^ k = a x ^ k 1 \hat{x}_k = a \hat{x}_{k-1}
p k = a p k 1 a p_k = a p_{k-1} a

更新
g k = p k / ( p k + r ) g_k = p_k / (p_k + r)
x ^ k x ^ k + g k ( z k x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - \hat{x}_k)
p k ( 1 g k ) p k p_k \leftarrow (1 - g_k)p_k

應當指出,我在更新的最後兩行沒有使用標準等號,而是使用賦值(箭頭)符號。雖然這樣用不符傳統,但傳遞出這樣的思想, x k ^ \hat{x_k} p k p_k 的更新是修正它們的當前值(當前值從預測步驟中獲得),而不是根據之前的步驟定義它們(正如預測步驟所做的一樣 [ 9 ] ^{[9]}

爲了嘗試我們的濾波器,我們將需要 :

  1. 觀測序列 z k z_k
  2. 狀態估計變量的初始值(基值) x 0 ^ \hat{x_0} 。這可以是我們第一次的觀測值 z 0 z_0
  3. 預測誤差 p 0 p_0 的初始值。它不可以是 0 ,否則由於乘法, p k p_k 將會一直是 0 ,所以我們隨意地設置其爲 1

對於本次實驗的觀測而言,我們不會嘗試觀測一個真實的系統(像飛機即將降落),而是設定這樣一個情形,狀態變量的理想化迭代公式是 x k = 0.75 x k 1 x_k=0.75x_k-1 ,起點 x 0 = 1000 x_0=1000 ,我們在其上添加位於區間 [-200,+200] 的隨機噪聲 [ 10 ] ^{[10]} v k v_k 來作爲觀測值。

file
  一旦你準備好運行濾波器,點擊 Run 按鈕來看卡爾曼濾波器是如何產生一個噪聲信號(紅色)的光滑版本(綠色),而這個光滑版本經常顯著地更接近原始乾淨的信號(藍色)。你可以嘗試不同的 x 0 x_0 r r a a 的值。

  譯者注:卡爾曼濾波器狀態方程一般爲微分方程,這保證了最優估計(Kalman濾波)的連續性,而離散的觀測序列保證了最優估計會收斂。卡爾曼濾波是最優估計的意思是,卡爾曼濾波器的輸出是無偏估計,並且其最終估計值與真值的方差最小。無偏估計是指最終估計值的均值等於真值的均值,而方差最小保證了最終估計值離開真值的波動範圍最小。兩條指標加持,卡爾曼濾波器被稱爲最優估計。

事實上,如果你查看這個網頁的源代碼,你會發現預測和更新的 JavaScript 是如此簡單:

// Predict
    xhat = a * xhat;
    p    = a * p * a;
    // Update
    g    = p  / (p  + r);
    xhat = xhat + g * (z - xhat);
    p    = (1 - g) * p;

我感謝Marco Camurri和John Mahoney指出本教程早期版本中等式的不一致性。


[10]Kalman濾波器假定噪聲服從高斯分佈,而我在這裏添加的噪聲服從均勻分佈,雖然這樣做,但是從本教程的層面看不會有太大區別。

Part 8 : 一個更切實際的模型

回想描述我們系統的兩條等式 :
x k = a x k 1 x_k = a x_{k-1}
z k = x k + v k z_k = x_k + v_k

其中 x k x_k 是系統的當前狀態 , x k 1 x_k-1 是上一步狀態, a a 是常量, z k z_k 是我們對系統的當前觀測, v k v_k 是當前觀測噪聲。雖然這兩條等式適用於很多種類的系統,但是並不適用於所有情形。首先,我們沒有考慮飛行員在飛機上練習時的時變控制,例如,向前向後移動控制桿。爲了考慮上控制項,我們引入另一個帶腳標的變量 u k u_k 來代表飛行員發送給飛機的控制信號的值。就如前一時刻的狀態 x k 1 x_k-1 乘以常係數 a a ,這個控制信號也可以乘以一個我們喜歡的常量,稱爲 b b 。所以,狀態的完整等式變爲 :
x k = a x k 1 + b u k x_k = a x_{k-1}+bu_k

其中,新的分量用藍色高亮。
  通常,除了噪聲之外的任何信號都可以乘上常量(線性放縮),所以觀測等式 z k z_k 能被重寫爲 [ 11 ] ^{[11]}
z k = c x k + v k z_k =c x_k + v_k
[11]爲了與最初的例子保持一致,我選擇了使用變量 a b c a b c 來表示常量,而不是選擇更爲常規的 f b h f b h

Part 9 : 修正估計

這裏再一次列寫出對於系統中狀態和觀測變量的更實際\通用的等式 :
x k = a x k 1 + b u k x_k = a x_{k-1} + b u_k
z k = c x k + v k z_k = c x_k + v_k

如我們所料,模型中新分量的引入需要預測和更新等式做出相應的修改 :
預測
x ^ k = a x ^ k 1 + b u k \hat{x}_k = a\hat{x}_{k-1}+ bu_k
p k = a p k 1 a p_k = ap_{k-1}a

更新
g k = p k c / ( c p k c + r ) g_k = p_kc/ (cp_kc+r)
x ^ k x ^ k + g k ( z k c x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c\hat{x}_k)
p k ( 1 g k c ) p k p_k \leftarrow (1 - g_kc)p_k

這裏是我們飛機Demo的一個擴展,展示了更長的時長,並增加了代表飛行員穩定地拉回控制桿以使飛機高度上升地控制信號。嘗試移動滑動條來調整不同常量地值。就如先前的 Demo 一樣,原始信號爲藍色,觀測信號爲紅色,Kalman 濾波後的信號爲綠色。
file

Part 10:增加速度項

回想我們關於飛機高度的原始等式 :
a l t i t u d e c u r r e n t _ t i m e = 0.98 a l t i t u d e p r e v i o u s _ t i m e altitude_{current\_time}=0.98*altitude_{previous\_time}

寫爲更通用的形式 :
x k = a x k 1 x_k = a x_{k-1}

回想高中數學和物理,這種公式似乎有點奇怪。畢竟,高度,是一種距離(海拔高度或地面以上),因爲我們學習過這樣的公式:
d i s t a n c e = v e l o c i t y t i m e distance=velocity*time

我們能夠將這兩種思考距離的方式統一嗎?答案是肯定的,但是這需要兩步來實現。
  首先,我們需要在我們高中所學的公式中引入當前時刻和上一時刻的概念,並且思考以離散時間步長考慮距離,而不是整個距離 :
d i s t a n c e c u r r e n t = d i s t a n c e p r e v i o u s + v e l o c i t y p r e v i o u s ( t i m e c u r r e n t t i m e p r e v i o u s ) distance_{current}=distance_{previous}+velocity_{previous}*(time_{current}-time_{previous})

換句話說,我們現在在哪,是從上一時刻我們在哪 加上 我們移動的距離獲得的。如果我們以固定時間間隔或者時間步長(1s ,100ns,6個月等等)執行這個計算,然後我們就能簡化爲 :
d i s t a n c e c u r r e n t = d i s t a n c e p r e v i o u s + v e l o c i t y p r e v i o u s t i m e s t e p distance_{current}=distance_{previous}+velocity_{previous}*timestep

這個等式使我們更加接近我們的一般形式:
x k = a x k 1 x_k = a x_{k-1}

但是我們看起來仍然有兩個不同的系統:一個包含一個簡單的乘積,而另外一個包含乘積和求和。得到一個兩者統一的等式,我們還需要線性代數。

Part 11 : 線性代數

我們有了根據速度和時間來表達距離的等式:
d i s t a n c e c u r r e n t = d i s t a n c e p r e v i o u s + v e l o c i t y p r e v i o u s t i m e s t e p distance_{current}=distance_{previous}+velocity_{previous}*timestep

我們嘗試將其與更一般的方程式統一起來 :
x k = a x k 1 x_k = a x_{k-1}

我們是很幸運的,數學家很久以前發明了一個 「奇怪的技巧「,即 以同樣的方式表示兩種方程。技巧就是把這樣一種情況(像系統狀態)不認爲是一個單一的數字,而是數字列表,這稱爲矢量,它就像Excel表格中的一列一樣。矢量的大小(元素個數)相應於我們想要編碼的關於狀態的數量。對於距離和速度,我們有兩項在矢量中:

x k [ d i s t a n c e k v e l o c i t y k ] x_k \equiv \begin{bmatrix} distance_k \\ velocity_k \end{bmatrix}

在這裏我使用了三等號來表明這是一個定義:當前狀態被定義爲矢量,它包含了當前距離和速度。
所以,線性代數如何幫助我們?Well,我們從其中獲得的另外一個工具是矩陣。如果將矢量比作電子表格中的列,那麼矩陣就像整個電子表格或者值表。當我們將矩陣乘以向量時,我們得到另一個相同大小得向量 :

[ a b c d ] [ x y ] = [ a x + b y c x + d y ] \begin{bmatrix} a & b \\ c & d \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} =\begin{bmatrix} ax + by \\ cx + dy \end{bmatrix}

例如:

[ 8 3 4 7 ] [ 2 5 ] = [ 31 43 ] \begin{bmatrix} 8 & 3 \\ 4 & 7 \end{bmatrix} \begin{bmatrix} 2 \\ 5 \end{bmatrix} =\begin{bmatrix} 31 \\ 43 \end{bmatrix}

矢量和矩陣可以是任意大小,只要它們匹配:

[ a b c d e f g h i ] [ x y z ] = [ a x + b y + c z d x + e y + f z g x + h y + i z ] \begin{bmatrix} a & b & c\\ d & e & f\\ g & h & i \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} =\begin{bmatrix} ax + by + cz\\ dx + ey + fz\\ gx + hy + iz \end{bmatrix}

我們也可以將兩個矩陣相乘來獲得另一個矩陣:

[ a b c d e f g h i ] [ r s t u v w x y z ] = [ a r + b u + c x a s + b v + c y a t + b w + c z d r + e u + f x d s + e v + f y d t + e w + f z g r + h u + i x g s + h v + i y g t + h w + i z ] \begin{bmatrix} a & b & c\\ d & e & f\\ g & h & i \end{bmatrix} \begin{bmatrix} r & s & t \\ u & v & w \\ x & y & z \end{bmatrix} =\begin{bmatrix} ar+bu+cx & as+bv+cy & at+bw+cz\\ dr+eu+fx & ds+ev+fy & dt+ew+fz\\ gr+hu+ix & gs+hv+iy & gt+hw+iz\\ \end{bmatrix}

矩陣相加很簡單,我們僅僅需要將相應位置得元素相加:

[ a b c d e f g h i ] + [ r s t u v w x y z ] = [ a + r b + s c + t d + u e + v f + w g + x h + y i + z ] \begin{bmatrix} a & b & c\\ d & e & f\\ g & h & i \end{bmatrix} + \begin{bmatrix} r & s & t \\ u & v & w \\ x & y & z \end{bmatrix} =\begin{bmatrix} a+r & b+s & c+t\\ d+u & e+v & f+w\\ g+x & h+y & i+z \end{bmatrix}

回到手中得任務,我們定義矩陣:

A = [ 1 t i m e s t e p 0 1 ] A = \begin{bmatrix} 1 & timestep\\ 0 & 1 \end{bmatrix}
遵從用大寫字母表示矩陣的慣例。然後我們的通用等式得到統一:
x k = A x k 1 x_k = A x_{k-1}

就和我們想要得一樣,展開來看:

[ d i s t a n c e k v e l o c i t y k ] = [ 1 t i m e s t e p 0 1 ] [ d i s t a n c e k 1 v e l o c i t y k 1 ] \begin{bmatrix} distance_k\\ velocity_k \end{bmatrix} =\begin{bmatrix} 1 & timestep\\ 0 & 1 \end{bmatrix} \begin{bmatrix} distance_{k-1}\\ velocity_{k-1} \end{bmatrix}
= [ 1 d i s t a n c e k 1 + t i m e s t e p v e l o c i t y k 1 0 d i s t a n c e k 1 + 1 v e l o c i t y k 1 ] =\begin{bmatrix} 1 * distance_{k-1} + timestep * velocity_{k-1}\\ 0 * distance_{k-1} + 1 * velocity_{k-1}\\ \end{bmatrix}
= [ d i s t a n c e k 1 + t i m e s t e p v e l o c i t y k 1 v e l o c i t y k 1 ] =\begin{bmatrix} distance_{k-1} + timestep * velocity_{k-1}\\ velocity_{k-1}\\ \end{bmatrix}

換句話說,當前距離是前一時刻得距離加上 前一時刻的速度乘以時間步長,而當前速度認爲不變。如果我們想要爲一個速度時變系統建模,我們很容易得修改我們的矢量和矩陣來包含加速度 [ 12 ] ^{[12]} :

[ d i s t a n c e k v e l o c i t y k a c c e l e r a t i o n k ] = [ 1 t i m e s t e p 0 0 1 t i m e s t e p 0 0 1 ] [ d i s t a n c e k 1 v e l o c i t y k 1 a c c e l e r a t i o n k 1 ] \begin{bmatrix} distance_k\\ velocity_k\\ acceleration_k \end{bmatrix} =\begin{bmatrix} 1 & timestep & 0\\ 0 & 1 & timestep \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} distance_{k-1}\\ velocity_{k-1}\\ acceleration_{k-1} \end{bmatrix}

[12]很多人問我,爲什麼我在矩陣右上角放了一個 0 而不是我們從高中物理中學到得 0.5 t 2 0.5t^2 . 這主要是爲了簡化:通過使距離僅僅取決於上一時刻距離和速度,以及速度取決於上一時刻速度和加速度,你能夠建立一個不錯得(非混亂)運動模型。爲了明白這些,你可以運行這個小 Python 程序,它產生恆定加速度下期望的拋物線距離曲線。同時,對於小的時間步長,平方使右上角的值接近於0。我感謝John Mahoney的這些見解。

Part 12 : 回頭再看預測和更新

這裏再一次寫出系統狀態的修改後的公式 :
x k = A x k 1 x_k = A x_{k-1}

其中 x x 是矢量, A A 是矩陣。正如你回想的那樣,這個等式的原始形式是 :
x k = a x k 1 + b u k x_k = a x_{k-1} + b u_k

其中 u k u_k 是控制信號,並且 b b u k u_k 所乘的常係數。我們同時也有等式 :
z k = c x k + v k z_k = c x_k + v_k

其中 z k z_k 是測量(觀測、傳感器)信號, v k v_k 是由於傳感器精度受限所增加的噪聲。所以,我們如何修改這些原始形式來與矢量/矩陣方法統一?正如你所猜測的,線性代數讓這非常簡單:我們把係數 b b c c 寫爲大寫字母,讓他們成爲矩陣而不是標量值(單個值):
x k = A x k 1 + B u k x_k = A x_{k-1} + B u_k
z k = C x k + v k z_k = C x_k + v_k

然後,所有的變量(狀態、觀測、噪聲、控制)都被認爲是矢量,並且我們有一組矩陣矢量乘法 [ 13 ] ^{[13]}
所以,我們預測和更新等式會怎樣?回想一下:
預測
x ^ k = a x ^ k 1 + b u k \hat{x}_k = a \hat{x}_{k-1} + b u_k
p k = a p k 1 a p_k = a p_{k-1} a
更新
g k = p k c / ( c p k c + r ) g_k = p_k c / (c p_k c + r)
x ^ k x ^ k + g k ( z k c x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c \hat{x}_k)
p k ( 1 g k c ) p k p_k \leftarrow (1 - g_k c) p_k

我們想要將所有常量 $a b c r $ 用大寫字母代替,以將其變爲矩陣 A B C R A B C R 。但是對於線性代數,這有點棘手。你可能記得我承諾解釋爲什麼我沒有簡化 a p k 1 a ap_k-1a a 2 p k 1 a^2p_k-1 (平方沒有寫成2次方)。答案是線性代數乘法並不像普通乘法一樣簡單。爲了獲得正確答案,我們通常不得不以一定的順序執行乘法,例如, A x B AxB 不一定等於 B x A BxA 。我們可能也需要轉置矩陣,以矩陣右側上標 T T 表示。通過把每行變成列和每列變成行,我們完成矩陣的轉置,像這樣 :
[ a b c d ] T = [ a c b d ] \begin{bmatrix} a & b\\ c & d \end{bmatrix} ^T= \begin{bmatrix} a & c\\ b & d \end{bmatrix}
[ a b c d e f g h i ] T = [ a d g b e h c f i ] \begin{bmatrix} a & b & c\\ d & e & f \\ g & h & i \end{bmatrix} ^T = \begin{bmatrix} a & d & g\\ b & e & h \\ c & f & i \end{bmatrix}
擁有了這些知識,我們能夠重寫我們的預測等式如下 :
x ^ k = A x ^ k 1 + B u k \hat{x}_k = A \hat{x}_{k-1} + B u_k
P k = A P k 1 A T P_k = A P_{k-1} A^T

指出我們將 A A P k P_k 用大寫字母重寫,表明它們是矩陣。我們已經知道,爲什麼 A A 是矩陣,我將會在後面解釋爲什麼 P k P_k 會是矩陣。我們的更新等式會怎麼樣?對於狀態估計 x k ^ \hat{x_k} 第二個更新等式是簡單的(沒有除法):
x ^ k x ^ k + g k ( z k C x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - C \hat{x}_k)

但是增益更新涉及到了除法 :
g k = p k c / ( c p k c + r ) g_k = p_k c / (c p_k c + r)

因爲乘法和除法是如此緊密相關,我們遇到一個關於矩陣除法的相似問題正如我們在矩陣乘法時遇到的那樣。爲了明白我們如何處理這些問題,讓我們首先重寫我們第一個更新等式,並使用熟悉的上標 -1次冪,來將除法表示爲逆( a 1 = 1 / a a^{-1} = 1/a ):
g k = p k c ( c p k c + r ) 1 g_k = p_k c (c p_k c + r)^{-1}
  現在,輪到 c c r r g g 和常量 1 1 變成矩陣,轉置 C C 如果需要如此 ,再一次推遲解釋爲什麼它們需要是矩陣 :
G k = P k C T ( C p k C T + R ) 1 G_k = P_k C^T (C p_k C^T + R)^{-1}
P k ( I G k C ) P k P_k \leftarrow (I - G_k C) P_k

接下來,我們如何計算 ( C p k C T + R ) 1 (C p_k C^T + R)^{-1} 正如普通的求逆,其中 a a 1 = 1 a * a^{-1} = 1 我們需要矩陣求逆產生這樣的效果,一個矩陣乘以它自己的逆產生單位矩陣,單位矩陣乘以另外一個矩陣不會改變 :
A A 1 = I A A^{-1} = I

其中單位矩陣(對3×3矩陣)
I = [ 1 0 0 0 1 0 0 0 1 ] I = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}
  計算矩陣的逆並不和求矩陣中每個元素的逆一樣簡單,這將會在絕大多數情形下是不正確的。雖然矩陣求逆超出了本教程的範圍,有絕佳的資源像MathWorld來學習。更好的是,你通常並不需要自己寫代碼。它內置於編程語言之中,像 Matlab,並且通過在 Python中調用包來使用,其他語言類似。


[13]事實上,我們可以認爲這些等式的原始標量形式是線性代數形式的一種特殊情形。

Part 13 : 傳感器融合

現在我們有了線性代數(矢量、矩陣)形式的卡爾曼濾波器的所有公式。
模型
x k = A x k 1 + B u k x_k = A x_{k-1} + B u_k
z k = C x k + v k z_k = C x_k + v_k

預測
x ^ k = A x ^ k 1 + B u k \hat{x}_k = A \hat{x}_{k-1} + B u_k
P k = A P k 1 A T P_k = A P_{k-1} A^T

更新
G k = P k C T ( C P k C T + R ) 1 G_k = P_k C^T (C P_k C^T + R)^{-1}
x ^ k x ^ k + G k ( z k C x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + G_k(z_k - C \hat{x}_k)
P k ( I G k C ) P k P_k \leftarrow (I - G_k C) P_k

似乎爲了在狀態變量中增加額外的項,我們做了很多的工作。事實上,線性代數的使用支持了kalman濾波器極其有價值的功能,即傳感器融合。回到我們的飛機的例子中,我當時指出除了高度,飛行員還需要更多的信息(觀測):飛行員有儀表盤來顯示飛機的空速、地速、航向、緯度和經度、外部氣溫等等。想像一架飛機僅有三個傳感器,每一個都相應於(多維矢量)狀態的給定部分:氣壓計測量高度、羅盤測量航向、皮托管測量空速。暫時假設這些傳感器完全準確(沒有噪聲)。然後我們的觀測等式 :
z k = C x k 1 + v k z_k = C x_{k-1} + v_k

展開 :
[ b a r o m e t e r k c o m p a s s k p i t o t k ] = [ 1 0 0 0 1 0 0 0 1 ] [ a l t i t u d e k 1 h e a d i n g k 1 a i r s p e e d k 1 ] \begin{bmatrix} barometer_k\\ compass_k \\ pitot_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} altitude_{k-1}\\ heading_{k-1} \\ airspeed_{k-1} \end{bmatrix}

現在想象我們有另外一個傳感器測量高度,例如 GPS 。氣壓計和 GPS 都會被高度影響。所以我們的等式變爲 :

[ b a r o m e t e r k c o m p a s s k p i t o t k g p s k ] = [ 1 0 0 0 1 0 0 0 1 1 0 0 ] [ a l t i t u d e k 1 h e a d i n g k 1 a i r s p e e d k 1 ] \begin{bmatrix} barometer_k\\ compass_k \\ pitot_k \\ gps_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} altitude_{k-1}\\ heading_{k-1} \\ airspeed_{k-1} \end{bmatrix}
  現在我們有了這個系統的第一個例子,這個系統沒有簡單的,傳感器和狀態變量之間一一對應關係 [ 14 ] ^{[14]} 。任何這樣的系統(即多個傳感器互補測量同一個量)都爲我們提供了傳感器融合的機會,也就是,能夠結合多個傳感器(氣壓計、GPS)的讀數來推斷狀態的一個分量(這裏是高度)。
正如當我們尋求第二位醫生對某種疾病的看法時,我們的直覺告訴我們,擁有一個以上重要信息來源會更好。在接下來的部分,我們將會看到kalman濾波器是如何使用傳感器融合來獲得比只用一個傳感器更好的狀態估計。


[14]我們的C矩陣讓人覺得有點不切實際,因爲矩陣中的元素可能不是1。可以這樣認爲,C矩陣中的非零值對應着相應傳感器和狀態矢量分量之間的某種關係。

Part 14 : 傳感器融合示例

爲了感受傳感器融合是如何工作的,讓我們將系統限制到只有一個狀態值 [ 15 ] ^{[15]} 。爲了進一步簡化,我們假定我們不知道狀態變換模型(矩陣A),並且我們不得不依賴於傳感器的值。也許我們在使用兩種不同的溫度計來測量外部溫度。所以,我們將會設置狀態變換矩陣爲1 :
x ^ k = A x ^ k 1 = 1 x ^ k 1 = x ^ k 1 \hat{x}_k = A \hat{x}_{k-1} = 1 * \hat{x}_{k-1} = \hat{x}_{k-1}

因爲缺乏溫度計的狀態變換模型(沒有系統狀態微分方程),我們假定當前狀態和上一時刻的狀態沒有變化。
當然,對於傳感器融合,在觀測矢量 z k z_k 中我們需要不止一個傳感器的值,在本例中我們可以認爲是這兩個溫度計的當前讀數。我們假定兩個傳感器對溫度估計做出相等的貢獻,所以,C矩陣是這樣的 :
z k = C x k + v k = [ 1 1 ] x k + v k z_k = C x_k + v_k = \begin{bmatrix} 1 \\ 1 \end{bmatrix} x_k + v_k
  現在我們有了爲了完成預測和更新所需要的三個矩陣( A C R A C R )中的兩個( A C A C )。接下來,我們如何獲得 R R
回想之前我們單個傳感器的示例,我們定義 $r 爲觀測噪聲信號 v_k$ 的方差,即和均值之間的平均平方離散程度。對於有多於兩個傳感器的系統, R R 是包含每對傳感器之間協方差的矩陣。 R R 矩陣對角線上的元素將會是每個傳感器的 r r 值,也就是傳感器關於自己的方差。非對角線元素代表了一個傳感器的噪聲隨着其它傳感器噪聲而變化的程度。對於這個例子和很多現實世界的應用而言,我們假定這些值是 0 (也就是可以簡單認爲傳感器之間是不相關的,互不影響)。我們曾經在溫度穩定的氣候可控條件下觀測我們的溫度計,並且觀測到他們的值在平均值附近平均上下波動 0.8°,也就是說,溫度計讀數的標準差是 0.8 ,即方差爲 0.8×0.8=0.64 。這給了我們矩陣 R R
R = [ 0.64 0 0 0.64 ] R =\begin{bmatrix} 0.64 & 0\\ 0 & 0.64\end{bmatrix}

現在我們應該明白爲什麼 P k P_k G k G_k 必須是矩陣:正如之前腳註所提到的, P k P_k 是步驟 k k 中估計過程的協方差。所以,就像傳感器協方差矩陣 R R P k P_k 也是一個矩陣。並且,因爲 $G_k $是關聯這些矩陣的增益,所以 G k G_k 也必須是矩陣,並且對於每一個協方差值都有一個增益值。這些矩陣 $P_k $和 G k G_k 的維度大小被它們所代表的含義決定。在我們現在的例子中, P k P_k 的維度大小是 1×1(即單值),因爲它代表了唯一估計值 $\hat{x_k} $關於其自身的協方差。增益 $G_k 1 × 2 是一個 1×2 的矩陣,因爲它將單個狀態估計 \hat{x_k} $和兩個傳感器觀測聯繫起來。
擁有了對於傳感器融合的理解,我們先把溫度計放到一邊,回到飛機的例子上。把之前所有東西放在一起,對於我們的飛機,我們獲得如下預測和更新等式(和之前一樣,使用協方差噪聲值在 0 到 200 英尺之間):
預測
x ^ k = A x ^ k 1 = 1 x ^ k 1 = x ^ k 1 \hat{x}_k = A \hat{x}_{k-1} = 1 * \hat{x}_{k-1} = \hat{x}_{k-1}
P k = A P k 1 A T = 1 P k 1 1 = P k 1 P_k = A P_{k-1} A^T = 1 * P_{k-1} * 1 = P_{k-1}

更新
G k = P k C T ( C P k C T + R ) 1 = P k [ 1    1 ] ( [ 1 1 ] P k [ 1    1 ] + [ 200 0 0 180 ] ) 1 G_k = P_k C^T (C P_k C^T + R)^{-1} =P_k \begin{bmatrix} 1~~1 \end{bmatrix} (\begin{bmatrix} 1 \\ 1 \end{bmatrix} P_k \begin{bmatrix} 1~~1 \end{bmatrix} + \begin{bmatrix} 200 & 0\\ 0 & 180 \end{bmatrix})^{-1}
x ^ k x ^ k + G k ( z k C x ^ k ) = x ^ k + G k ( z k [ 1 1 ] x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + G_k(z_k - C \hat{x}_k) =\hat{x}_k + G_k(z_k - \begin{bmatrix} 1 \\ 1 \end{bmatrix} \hat{x}_k)
P k ( I G k C ) P k = ( 1 G k [ 1 1 ] ) P k P_k \leftarrow (I - G_k C) P_k =(1 - G_k\begin{bmatrix} 1 \\ 1 \end{bmatrix}) P_k

事實證明,如果我們不重新引入我們前面提到的東西:過程噪聲,那麼我們山窮水盡的狀態變換模型會讓我們陷入困境(有點不懂)。回想之前,對於單個變量系統的狀態變換的完整等式是:
x k = a x k 1 + w k x_k = a x_{k-1} + w_k

其中 w k w_k 是給定時間的過程噪聲。用線性代數將上式重寫爲:
x k = A x k 1 + w k x_k = A x_{k-1} + w_k

但是,實際上我們並沒有在預測/更新模型中考慮過程噪聲。加上過程噪聲很容易,正如我們使用 R 來代表測量噪聲 v k v_k 的協方差一樣,我們使用 Q Q 來代表過程噪聲 w k w_k 的協方差。然後,我們在預測 P k P_k 時做出一點修改,即簡單得增加這個協方差 :
P k = A P k 1 A T + Q P_k = A P_{k-1} A^T + Q

有趣的是, Q Q 矩陣的非零元素即使很小,也非常有助於保持我們的估計狀態值正常運行。下面是一個小巧的傳感器融合的Demo,允許你改變 Q Q R R 矩陣中的元素值和改變兩個傳感器的偏置(恆定的不精確性,或者噪聲均值)。正如你所看到的,當傳感器從不同方向偏置時,傳感器融合能夠提供一個比使用單個傳感器更接近真值的近似。
file


[15]我採用得這個材料來自Antonio Moran的excellent的關於卡爾曼濾波器的PPT。Matlab / Octave用戶可能想要嘗試我放在Github上的版本,這一版本包含了更爲通用的Kalman濾波器的實現。

Part 15 : 非線性

現在爲止,我們很容易看到線性代數是很棒的,它讓我們以一種簡潔的的方式表達複雜的算法,像卡爾曼濾波器。然而線性代數並不是萬能的。正如其名字所指出的,線性代數受限於表達線性關係,即以直線爲特徵。爲了明白這一點,再一次考慮矩陣乘以矢量的例子 :

[ a b c d ] [ x y ] = [ a x + b y c x + d y ] \begin{bmatrix} a & b \\ c & d \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} =\begin{bmatrix} ax + by \\ cx + dy \end{bmatrix}

下面的Demo允許你調整常量 a b c d a b c d 的值來看乘積結果矢量是如何變化的。如你所見,所有的常量調整導致了另外一條直線段(矩陣乘以矢量仍然保持線性)
file
  另一方面,出去走走,你將發現自然界中很少是線性的。你所見到的擁有直線的事物—建築、道路、電線杆等等都是人造的,然而大部分其他事物(樹木、鳥類、雲)是曲線的或者分形的。由於傳感器和電機以及其他工件都是用物理材料製成的,因此它們的行爲同樣傾向於非線性,如果超出一些限制的範圍。
在高中數學中你所學到的很多函數,像 f ( x ) = a x 2 + b x + c f(x) = ax^2 + bx+c , f ( x ) = s i n ( x ) f(x) = sin(x) , l o g ( x ) log(x) 等等,大部分是非線性的,所以,我們很容易選擇一個。下面的Demo,我根據有趣的曲線形狀挑選了幾個不同的非線性函數,將上一Demo中的藍色直線段映射爲曲線。
file

Part 16 : 處理非線性

儘管非線性可能爲任何系統引入全新世界,但我們仍然抱有希望。正如你在之前Demo中所注意的,前三個非線性函數都具有一個有用的屬性:它們是光滑曲線,這與最後一個尖銳的轉彎相反。數學家把這些光滑曲線函數稱爲可微。正如任何學過微積分的人可以證明,可微函數(光滑曲線)能夠由一系列連續的小直線段建模到任意精度。即使沒有微積分,你能看一下下面的Demo,它允許你通過調整直線段△x的大小近似函數 f x = x 2 f(x)=x^2
file
  然而,這些直線段是如何幫助處理 Kalman 濾波器中的非線性關係?考慮一個很簡單的濾波器,其傳感器符合下列線性等式 :
z k = 3 x k + 2 z_k = 3 x_k +2

這個等式告訴我們傳感器讀數一直是相應狀態值的三倍加上 2 :不管狀態值是什麼,傳感器讀數一直是三倍狀態值加上 2 .現在來考慮非線性傳感器等式 :
z k = l o g 2 ( x k ) z_k = log_2(x_k)

這個等式表明傳感器讀數是狀態值以 2 爲底的對數值:這是很典型的關係,例如,我們對音高的感覺是頻率的函數。即使你之前沒有聽說過對數,可以看一下下列近似值表格,這個表格展示了狀態 x k x_k 和傳感器讀數 z k z_k 之間的關係,這並不如之前的 Demo 直觀 :
file
  在這裏你可以看到,並沒有常量 a a z k = a x k + b z_k=ax_k+b 。然而,我們能使用上面的小線段來推導不同的 a k a_k b k b_k 集合,每個時間步長對應一個 k k ,這樣做就用很多直線段近似了非線性對數關係。如果你研究過微積分,你可能記得我們能夠直接計算很多非線性函數(log sin cos 等等)的一階導數。如果你沒有學過微積分,不要感到憂桑,一個函數的一階導數實際上是在給定點上的最佳線性近似(高數中線性主部)。稍微 Google 一下,你可以明白 l o g 2 ( x ) log_2(x) 的一階導數近似是 1 / 0.693 x 1/{0.693x} ,這是有意義的,隨着 x x 的增加, l o g 2 ( x ) log_2(x) 的值越來越大。

Part 17 : 一個非線性卡爾曼濾波器

接下來,一階導數對我們有什麼用?Well,這裏是我們的線性 Kalman 濾波器的等式集合,使用了一個沒有狀態變換和控制信號的模型,但擁有過程噪聲,單個傳感器,和單個狀態值 :
模型
x k = x k 1 + w k x_k = x_{k-1} + w_k
z k = c x k + v k z_k = c x_k + v_k

預測
x ^ k = x ^ k 1 \hat{x}_k = \hat{x}_{k-1}
p k = p k 1 + q p_k = p_{k-1} + q

更新
g k = p k c ( c p k c + r ) 1 g_k = p_k c (c p_k c + r)^{-1}
x ^ k x ^ k + g k ( z k c x ^ k ) \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c \hat{x}_k)
p k ( 1 g k c ) p k p_k \leftarrow (1 - g_k c) p_k

現在我們將會修改這些等式來反映傳感器的非線性。使用符號 h h 來代表任何非線性函數(就像我們例子中的 l o g 2 log_2 [ 16 ] ^{[16]} ,並且 c k c_k 代表它在時間步長 k k 的一階導數,我們獲得 :
模型
x k = x k 1 + w k x_k = x_{k-1} + w_k
z k = h ( x k ) + v k z_k =h(x_k)+ v_k

預測
x ^ k = x ^ k 1 \hat{x}_k = \hat{x}_{k-1}
p k = p k 1 + q p_k = p_{k-1} + q

更新
g k = p k c k ( c k p k c k + r ) 1 g_k = p_kc_k(c_kp_kc_k+r)^{-1}
x ^ k x ^ k + g k ( z k h ( x ^ k ) ) \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k-h(\hat{x}_k))
p k ( 1 g k c k ) p k p_k \leftarrow (1 - g_kc_k)p_k ck)pk

正如我們 Part14 中的傳感融合 Demo ,下面的 Demo 展示了一個時變的溫度波動,但是僅使用非線性響應並沒有恆定偏置的單一傳感器。你能夠從三個非線性傳感器函數中選擇來比較我們的非線性 Kalman 濾波器和線性 Kalman 濾波器。正如你所見,無論如何調整線性版本中 c c 參數,都不足以獲得與非線性版本相同好的擬合原始信號(真值)結果。右邊的圖顯示了非線性函數自身的形狀 :
file


[16]爲什麼不把這個函數寫作f?我們將會在很多頁面中看到爲什麼

Part 18 : 計算導數

如果你已經到這裏了,你已經佔據地利了,還差一點點天時就可以很好的理解擴展卡爾曼濾波器了。還有兩件事情需要考慮:
  1. 在並不知道顯示函數時,如何從實際信號中計算一階導數
  2. 如何推廣我們的單值非線性狀態/觀測模型到多值系統

爲了回答第一個問題,我們指出一個函數的一階導數被定義當時間步長接近零時,函數的連續值之間的差值除以時間步長的極限:
f ( x ) = lim Δ x 0 f ( x + Δ x ) f ( x ) Δ x f'(x) = \lim_{\Delta x \to 0} \frac{f(x+\Delta x) - f(x)}{\Delta x}

如果你不理解這個等式,不要憂桑:可以這樣近似一階導數
( y k + 1 y k ) t i m e s t e p \frac{(y_{k+1} - y_k)} {timestep}

實際上,如下面 Demo 展示的,這個有限差分公式通常是對一階導數的很好的近似。Demo 允許你選擇同上的三個函數。但是這次你能夠選擇是導數公式還是有限差分 :
file
  如果一個信號(像傳感器值 z k z_k )是另一個信號(狀態 x k x_k

相關文章
相關標籤/搜索