卡爾曼濾波的理解以及推導過程
針對的系統為：
狀態方程 X(k)=AX(k-1)+Bu(k-1)+W(k-1)
測量方程 Z(k)=HX(k)+V(k) 0
W是狀態預測的噪聲 符合正態分佈N(0,Q)
V 是測量的噪聲 符合正態分佈N(0,R)
理解：
一般測量值Z(k)為外界提供，大概是採用一種不同的方式去獲取同一個目標值（值不一定相等），用來矯正預測的。於是我們得到了三個值，測量值Z(k),上一次綜合預測值和測量值得到的最優值X(k-1|k-1)，對上一次最優值通過狀態方程預測的下一個結果
X(k|k-1)=AX(k-1|k-1)+Bu(k)。卡爾曼的濾波的目的是綜合測量值和預測值得到一個最優值，這裡採用的是分配權重的方式來綜合結果X(k|k)=X(k|k-1)+K(k)(Z(k)-HX(k|k-1))。到此已經得到了最優的解，但是權重怎麼計算？這裡的權重K就是卡爾曼增益（Kalman gain）。K的確定是通過求使得估計值X(k|k)與真實值Xk的均方誤差最小的K。得到K之後自然就是想辦法使得卡爾曼濾波可以無限迭代下去，這好像是卡爾曼最牛逼的地方。
推導過程：
要看懂推導過程需要一些矩陣的計算知識以及統計學知識。
預測方程
綜合預測值和測量值的最優解
要求使得估計值與預測值的均方差最小，自然要先求均方差，這裡是求協方差矩陣，協方差矩陣的對角線就是方差，只要求出了協方差矩陣就可以求該矩陣的痕跡即tr(矩陣的跡便是對角線上的元素累加起來)，再對跡求導，使得求導後等於0，解方程即可得到K。
估計值X(k|k)和真實值X(k)之間的協方差矩陣
預測值X(k|k-1)和真實值X(k)之間的協方差矩陣(別問為什麼要求這個，這個是迭代的需要，具體我也不知道，接下來會用到)
將0式帶入2式，再將計算結果帶入3式可以得到
將5展開後將4式帶入可以得到
求6式的跡可以得到
對7式求K(k)的偏導，並使之等於0可得到
對8式移項可得
將9式帶入Pk可以得到
到此還差P(k|k-1)未知
將4式展開計算即可以得到
至此全部計算完畢。
標記黃色的即為卡爾曼的核心五條公式
使用：
使用步驟：
預測公式：
更新公式：
參考連線：
卡爾曼推導過程 http://blog.csdn.net/heyijia0327/article/details/17487467