卡爾曼濾波算法--核心公式推導導論
寫在最前面：這是我第一篇專欄文章，感謝知乎提供這么一個平台，讓自己能和大家分享知識。本人會不定期的開始更新文章，文章的內容應該集中在汽車動力學控制，整車軟件架構，控制器等方面。作為一名在校碩士，很多理解都可能不全面，不正確，大家有不同意見歡迎討論。 謝謝！
---------------------------------------------------------------------------------------------------------------------------
卡爾曼濾波算法的應用很廣泛，省略很多字。
卡爾曼濾波算法的優點很多，省略很多字。
卡爾曼濾波算法很牛逼，因為有一堆公式，有一堆符號，看起來就很牛逼啊，乍一看不懂的都很牛逼啊！
本文針對卡爾曼濾波算法的核心公式進行推導，不讓大家被它華麗的外表嚇到。（之后計划寫關於針對非線性情況的EKF和UKF，對卡爾曼濾波算法做一個全面一點的應用介紹。感興趣的可以關注專欄。）
--------------------------------------------------------------------------------------------------------------------------
Okay，進入正題。這篇文章假設讀者已經對卡爾曼濾波算法有初步的了解，知道它能做什么，知道它的優點，知道它很牛逼，並且你已經對它產生興趣，但不知道如何下手。
首先給出一個控制理論中公式，別急着翻控制理論的書，沒那么復雜：
兩個基本問題：
1.卡爾曼濾波算法要做什么？
對狀態進行估計。
2. 卡爾曼濾波算法怎么對狀態進行估計？
利用狀態過程噪聲和測量噪聲對狀態進行估計。
一個狀態在一個時刻點k的狀態進入下一個時刻點k+1狀態，會有很多外界因素的干擾，我們把干擾就叫做過程噪聲，（這個詞一看就是硬翻譯過來的，別在意為什么叫噪聲）用w表示。任何一個測量儀器，都會有誤差，我們把這個誤差叫做量測噪聲，用v表示。
回到上面那個公式，狀態方程表示狀態在不斷的更新，從一個時刻點進入下一個時刻點，這個很好理解。關鍵是量測方程，它表示，我們不斷更新的狀態有幾個能用測量儀器測出來，比如，汽車運動狀態參數有很多，比如速度，輪速，滑移率等，但是我們只能測量出輪速，因此量測方程要做的就是把狀態參數中能量測的狀態拿出來。
我們始終要記得我們要做的事：我們要得到的是優化的狀態量Xk。
理解了上面之后就可以開始推導公式了。
1.首先不考慮過程噪聲對狀態進行更新，很簡單：
舉個例子，v(k)=v(k-1)+at，勻加速運動咯。
2.不考慮測量噪聲取出能測量的狀態，也很簡單：
3.用測量儀器測量出來的狀態值（大家可以考慮到：測量的值就是被各種噪聲干擾后的真實值）減去上面不考慮噪聲得到的測量值：
這個值在數學上是一個定義值，叫做新息，有很多有趣的性質，感興趣的可以自己谷歌。
我們對步驟暫且停一停。這個叫新息的值有什么用？由上面的過程我們可以明顯看到，它反映了過程噪聲和測量噪聲綜合對測量狀態值的影響，也就是它包含了w和v的情況。
回到數學層面，（不要害怕，很簡單的數學應用和思考啦！）一個數值c由兩部分內容a和b組成，那么怎樣用數學表達式來表達？
一般有兩種做法：
I.直接相加：c=a+b;
II. 用比例的方法：a=n*c,b=(1-n)*c
卡爾曼采用了方法II，用比例的方法來做（其實這也是為什么叫做濾波的原因，因為濾波就是給權值之類的操作）。也就是說，過程噪聲w=新息*一個比例。這樣得到的過程噪聲加上原來（第一步）不考慮過程噪聲的狀態值不就是優化值了嗎？ 也就是：
Okay，都寫到這里了，有必要做一下前提假設：
a. 什么高斯噪聲，均值為零一堆；
b.Ak,Ck,wk的協方差Q,vk的協方差R，系統協方差初始值P0,狀態初始值X0，都已知。為什么已知，你實際做項目就知道了。不過不懂的可以留言或者私信。
那么到目前為止我們的思路就是清楚了，找到一個合適的Hk值（卡爾曼增益），那么我們就能得到狀態的最優值。（卡爾曼說的，不是我說的，所以你問為什么，你要問他，這么深層次的理論留給博士和學者們去做就好，我們就現學現用就行，哈哈哈，站在巨人的肩膀！）
問題來了：怎么得到合適的Hk？似乎不是隨便一個參數。
這是誤差協方差矩陣。
思路：使得誤差協方差矩陣Pk最小的Hk。
為什么？這里我從感觀的角度說明自己的理解，歡迎討論。
協方差表示什么，協方差表示兩者之間的聯系或者關系，關系越大，協方差越大。誤差協方差越小說明過程噪聲和量測噪聲的關系越小。關系越小能做什么，這要回到我們第3步討論的我們用比例的方法分開了w和v。用比例分開，到底多少屬於w，多少是v，如果關系越小，分開的越精確，比如一堆白砂糖和鹽，如果兩種混合的很均勻，我們說它關系很大，也就越難用比例的方法將其分開。
4.求的誤差協方差矩陣Pk
自然是把里面的Xk先得到，然后公式運算，通過上面的步驟我們也容易得到：
然后復雜的數學計算，和之前假設的高斯噪聲，新息的性質之類（至於過程，個人覺得你如果只做應用，不研究算法，就沒必要深入去看了），就能得到下面的卡爾曼濾波遞推公式：
通過上面的解釋，我們也就不難知道這些公式都在干嘛，知道干嘛就可以了。在知道A,C,P0,Q,R的情況下，整個公式的運算流程也都很清晰了。
————————————————————————————————————————
歡迎評論！轉載請注明出處！謝謝大家！