廢話
在學長們不厭其煩地教導后，我想我大概也許可能。。。知道卡爾曼濾波是個什么了，，，，，，
我覺得對於我們初學菜鳥入門級別的，可能淺顯粗俗的話更容易理解一些。所以，本貼不包含原理以及公式推導，僅是自己的一點心得——關於Kalman濾波的應用（所以寫論文的朋友千萬不要直接Copy）。如有錯誤，不吝指正！
首先說一下Kalman濾波與非線性優化。Kalman濾波是對問題進行線性處理（一次一階泰勒展開），非線性優化自然是非線性咯（多次一階或二階泰勒展開）。
下面就開始Kalman吧，為了方便理解，先自定義一些概念：
預測值（X_），即通過狀態估計方程計算得到的；觀測值（Z），觀測得到的值，無需計算，直接得到；終值（X），即最后我們要取的值。
它的大概思想是這樣：我們最后要取的值，既不是預測值，也不是觀測值，而是通過二者合並計算后的值。為什么呢？因為Kalman認為，真實的理論值是不可能被正確觀測到的，總會有一定誤差。舉個不恰當的例子，理論上4/3是存在的，但在現實生活中是不可能有這個的，你拿尺子去量任何確定的實物都會得到一個長度，無限小數是不存在的！
那么怎么根據預測值和觀測值得到終值呢？如下：
X = X_ + ◊
很簡單對吧，就是讓預測值再加上一個東西 ◊，這個 ◊ 是通過 K、X_ 和 Z 計算得到的，所以核心工作就是求 X_、K。
思路
先看一下Kalman的兩個方程（第一個是用來計算的，第二個僅是理論表示）：
狀態方程： X(k) = AX(k-1) + BU(k) + W(k)
觀測方程： Z(k) = HX(k) + V(k)
其中狀態方程表示的是如何用上一時刻的狀態計算得到這一時刻的狀態，A、B 為系數矩陣，U(k) 為控制輸入（可以為0），W(k) 為噪聲（通常假設為高斯白噪聲，方差為 Q ）；
其中觀測方程表示的是當前時刻狀態被噪聲干擾然后被觀測得到的過程，H 為系數矩陣， V(k) 為噪聲（通常假設為高斯白噪聲，方差為 R ）。
在控制問題中通常有控制輸入，而另外某些問題中狀態被觀測時會有一定變化，所以這兩個方程看起來很復雜。實際上很多問題中，沒有控制，觀測也很簡單，所以 A、H 可以取 1（或單位矩陣），U(k) 為 0，甚至 W(k) 也可以為 0。
所以最簡單的形式為：
X(k) = X(k-1)
Z(k) = X(k) + V(k)
這表示，很短時間內，現在時刻的值（終值）可以近似認為等於上一時刻的值（很多慣性系統，如溫度）；而觀測值僅僅是終值基礎上加了一個噪聲。
計算
下面說一下Kalman的迭代計算過程（核心是計算 X_、K，同時定義了中間變量 P_和P ）
（首先，X(k-1)、P(k-1)已知，初始時刻這兩個值可以隨便給，最終會收斂；一般X(k-1)取觀測值，P(k-1)可以取單位矩陣等實數矩陣 ）
首先明確我們可以用的參數有：A、B、H、Q、R。 上一時刻的值：X(k-1)、P(k-1)。 當前時刻控制輸入：U(k)。 當前時刻觀測值：Z(k)
然后開始迭代，迭代可以分為兩大部分。
第一部分（計算參數：中間變量 P_、 預測值 X_、 卡爾曼增益 K ）（矩陣相乘的乘號我就省了，為了看清一點特意加了空格）：
P_(k) = AP(k-1)AT + Q
X_(k) = AX(k-1) + BU(k) ；可以發現這個式子就是原來的狀態方程計算的式子（所以前面說狀態方程是用來參與計算的）
K(k) = P_(k) HT ( H P_(k) HT + R )-1 ；用中間變量 P_ 來計算 K
第二部分（用計算得到的參數計算終值與中間變量 P ）
X(k) = X_(k) + K(k) ( Z(k) - HX_(k) ) ；用卡爾曼增益 K 和觀測值來更新 X，可以看到觀測值 Z 直接就用了，而不是通過計算得到的（所以前面說觀測方程僅供理論分析，計算時不是直接用）
P(k) = ( I - K(k) H ) P_(k) ；用卡爾曼增益 K 和 P_ 來更新 P
然后用 X(k) 和 P(k) 進行下一輪迭代。
續
前面是完整的兩個方程進行迭代的過程，對於簡單系統：
X(k) = X(k-1)
Z(k) = X(k) + V(k)
計算過程就相當簡單：
上一時刻 X(k-1)、P(k-1) 已知，可直接計算 Kalman 增益 K，
K(k) = P(k-1)/ (P(k-1) + R)
然后可計算終值及 P，
X(k) = X(k-1) + K(k) ( Z(k) - X(k-1) )
P(k) = ( I - K(k) ) P(k-1)
可以看到，Kalman本質上是一個利用上一時刻狀態估計當前時刻狀態的一階線性系統。當前值為上一時刻值加上一個偏差，而這個偏差又可以通過當前時刻的觀測值進行修正。