卡爾曼濾波與擴展卡爾曼濾波

1. 背景介紹
   卡爾曼濾波（Kalman filtering）一種線性系統狀態方程，對輸入輸出的觀測數據進行最優估計的算法。由於觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。故求解最優估計的算法就稱之爲卡爾曼濾波算法。目前在機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等應用較廣。近年來更被應用於計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測，計算機視覺等等。

2. 卡爾曼濾波算法
   a. 上述所說，濾波器在有噪聲的線性系統中，跟蹤目標值狀態的遞歸濾波器。它基於貝葉斯(Bayes)概率，預先假定一個模型，並使用這個模型從以前的狀態預測（Prediction）當前狀態。然後這個預測值與由外部測量儀器獲得的實際測量值之間的誤差來執行一個補償（update）過程，這個過程利用誤差值推定更準確的狀態。
   
   通俗講解–（可借用上述模型）
   Step 1: 假設在t−1時刻我們預測房間的溫度爲23度，預測的誤差爲3度。假設它是服從高斯分佈，我們將3度視爲溫度標準差。（Pk-1=23, Xk-1=3）
   　　Step 2: 根據我們的經驗，在沒有外界干擾的情況下房間的溫度將會恆定不變。於是我們預測在t時刻房間的溫度爲23度。但是預測本身也會存在誤差。我們把這個誤差看做是服從高斯分佈。假設其標準差爲4度。值得一提的是對於4度和3度，兩個高斯分佈是相互獨立的。綜合這兩個高斯分佈，通過我們的經驗可以得到t時刻的氣溫爲23度，其標準差爲5度。(利用物理模型，預測階段)
   　　Step 3: 爲了更精準的測量室內溫度，我們會使用溫度計去測量溫度。在t時刻溫度計的讀數爲25度。溫度計的誤差爲4度，其服從高斯分佈。（當前測定值）
   　　Step 4: 此時我們對t時刻的氣溫值有兩個估計，一個是人爲經驗的預測，另一個是溫度計測量讀數。它們的誤差都服從高斯分佈且相互獨立。
   　　我們將其視爲兩個傳感器然後進行加權求得我們最終的估計值。通過一系列計算，我們最終算的房間溫度爲24.56度，誤差爲2.35度。（測定值與預測值，補償階段）
   　　Step 5: 將Step 4中算出的結果作爲t時刻的最終結果，重複Step 1至Step 4便可以算出以後任意時刻房間的氣溫。（進入下一時刻）
   　 b. 卡爾曼濾波的數學形式
   卡爾曼濾波的整個過程可以被描述爲5個公式。
   （1）狀態的一步預測方程：X̂ t,t−1=At,t−1X̂ t−1（A作爲矩陣，對X進行線性變換）
   （2）均方誤差的一步預測：Pt,t−1=At,t−1Pt−1ATt,t−1+Q（對新誤差進行標準差更新）
   （3）濾波增益方程（權重）：Ht=Pt,t−1CTt[CtPt,t−1CTt+R]−1（計算權重，決定實際測量值和預估值的計算權重）
   （4）濾波估計方程（T時刻的最優值）：X̂ t=X̂ t,t−1+Ht[Zk−CkX̂ t,t−1]（計算最優值）
   （5）濾波均方誤差更新矩陣（T時刻的最優均方誤差）：Pt=[I−HtCt]pt,t−1（計算新的方差誤差）

3. 擴展卡爾曼濾波算法（EKF）
   上述已經說過，卡爾曼濾波算法是線性算法，而對許多應用是值得改進的。擴展卡爾曼濾波就出現了，這裏使用所有時刻採集到的數據進行狀態估計，一般對圖像而言，相同的地點像素和明暗度是基本不變的，所以數據估計反而更優，在視覺slam中是主流方法。
   a. SLAM中的運動方程和觀測方程通常是非線性系統，需要使用相機內參模型及李代數表示的位姿，更不可能是線性系統。經典的KF算法，裏面線性結果，則符合高斯分佈。而非線性變換，高斯分佈也就不再是高斯分佈。這時KF需要擴展。
   b. 擴展方式
   通常的做法是，在某個點附近考慮運動方程及觀測方程展開成一階泰勒，只保留一階項，即線性的部分，然後按線性系統進行推導。
   5步驟中，依次對幾個過程進行泰勒轉換取其中的線性部分。