摘要： 標量Kalman濾波的過程分析和證明及C實現，希望能夠幫助入門的小白，同時得到各位高手的指教。並不涉及其他Kalman濾波方法。
本文主要參考自《A Introduction to the Kalman》 (需要的同學可以自行百度，也可以找到中文版的)
注：遞歸思想，高斯分布獨立性的應用，數據融合的應用
一，什么是Kalman 濾波（已經了解的同學可以跳過這里）
卡爾曼在博士期間發表了用遞歸方法解決離散數據線性濾波 問題的論文（關於Kalman 濾波的真正第一人還是有待探討的，有興趣的同學自行查找）。從此以后，Kalman濾波器得到了廣泛的應用，比如自主導航的姿態解算（四軸飛行器也有部分開發商使用了Kalman濾波，但是人家用到不是今天講的標量濾波，有時間可以探討下）
其實Kalman濾波器由5條遞歸的數學公式表示。5條公式提供十分有效的計算方法，描述了系統狀態的變化（其實就是觀測量的變化，后續涉及系統狀態請自行對應），能得到與真實值最小誤差的估計（這個估計可以當作真實值使用）。這5條公式，可以做到估計過去，現在，甚至未來的系統狀態，而不用當心系統狀態的變化模型。
同時Kalman也是提出結合狀態空間和測量空間的濾波方法。而Wiener濾波則是僅在測量空間上濾波。Wiener濾波需要知道所有[0,n-1]時刻協方差的先驗知識,Kalman直接可以通過上一時刻即n-1時刻的狀態信息和均方誤差信息就可遞推得到n時刻的估計，其實也是利用了[0,n-1]時刻的先驗知識。
二，什么樣的數據適合於Kalman 濾波
可以先明確的指出的是：
- 噪聲符合高斯正態分布且與狀態量相互獨立（在后續的證明可以看出）；
- 系統的狀態量不是突變的物理量，比如加速度啊，電容的充電電流啊（這個例子不是很恰當~）
三，Kalman 濾波的數學推導及物理意義：
3.1背景（這里舉經典的例子）：
假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的， 也就是現在這一分鍾的溫度等於過去一分鍾的溫度（假設我們用一分鍾來做時間單位）（先驗估計） 。假設你對你的經驗不是 100% 的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲，也就是這些偏差跟前后時間是沒有關系的而且符合高斯分布。
另外，我們在房間里放一個溫度計，但是這個溫度計也不准確的， 測量值和實際值有偏差。我們也把這些偏差看成是高斯白噪聲。
好了， 現在對於某一分鍾我們有兩個有關於該房間的溫度值： 你根據經驗的預測值 （系統的 預測值）和溫度計的值（測量值）。
Kalman要解決的問題是如何使用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算 k 時刻的是實際溫度值。 首先你要根據 k-1 時刻的溫度值， 來預測 k 時刻的 溫度。因為你假定溫度是恆定的（就算事實不是恆定的也沒有關系，因為你不相信你這個假定），所以你會得到 k 時刻的溫度預測值是跟 k-1 時刻一樣的， 假設是 23 度，同時假設該估計值的高斯噪聲的標准差是 5 度（這里為何是5，首先假設預測噪聲標准差是4，上次最優估計誤差為3，求平方和即得。至於為何是平方和，可以看做兩個高斯過程相加[上次最優估計結果是個高斯過程，這次預測也是高斯過程]，所得的也是高斯過程，方差為原先兩者的方差之和）。
然后，你從溫度計那里得到了 k 時刻的溫度值，假設是 25 度，同時該值的噪聲標准差是 4 度。 由於我們用於估算 k 時刻的實際溫度有兩個溫度值， 分別是 23 度和 25 度。 究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點， 我們可以上次的估計值的噪聲方差及上次的最優估計方差總和之比判斷。算出比例因子Kg: Kg^2=5^2/(5^2+4^2) ，所以 Kg=0.78 ，我們可以估算出 k 時刻的實際溫度值（最優估計）是： 23+0.78*(25-23)=24.56 度[估計值+Kg*(測量值-估計值)]。可以看出， 所以估算出的最優溫度值偏向溫度計的值。 現在我們已經得到 k 時刻的最優溫度值了，下一步就是要進入 k+1 時刻，進行新的最優估算。到現在為止，好像還沒看到什么遞歸的東西出現。對了，在進入 k+1 時刻之前，我 們還要算出 k 時刻那個最優值（ 24.56 度）的噪聲標准差。算法如下：
((1-Kg)*5^2)^0.5=2.35 。這里 的 5 就是上面的k 時刻你預測的那個 23 度溫度值的標准差，得出的 2.35 就是進入 k+1 時刻以 后 k 時刻估算出的最優溫度值的標准（對應於上面的 3 ） 。 就是這樣，卡爾曼濾波器就不斷遞歸，從而估算出最優的溫度值。他運行的 很快， 而且它只保留了上一時刻的 最優估計誤差標准差。 上面的Kg ， 就是卡爾曼增益 （ Kalman Gain ） 。 他可以隨不同的時刻而改變他自己的值，是不是很神奇！
三，Kalman 濾波的過程分析
這里引用了《An Introduction to Kalman》
這里稍微解釋下最優估計的協方差（誤差協方差）：為何要稱作協方差呢，一開始認為協方差明明是兩個變量間的。可以留意預測公式2和校正公式3（公式3目前可能看不出來），根據實例中講到的兩個高斯分布相乘，可以理解為這是上一次最優估計和當前預測值(從時間上看是兩個變量)的協方差。
四．Kalman 的證明及物理意義
上面圖片看不懂沒關系，明白各個物理量就可以了，說實話一維的應用會在上面再修改一下。
證明：
- 首先確定幾個符號（十分重要，一開始我也不習慣,大概讀懂下面符號的物理意義再結合讀公式，應該能明白的~）：
幾個假設和模型
-
- N時刻狀態量： X[n]=A*X[n-1|n-1]+B*U(n)+W[n]
即等於上個時刻的“真實值”+控制量+誤差（W[n]~N(0,Q)）。
因此，給出先驗估計X[n|n-1]= A*X[n-1|n-1]+B*U(n);
- N時刻的觀測值：Z[n]=H*X[n]+V[n]
即等於真實值得數乘+誤差（V[n]~N(0,R)）
- 我們出發點是利用預測值和測量值的殘差來修正估計：
給出采用下面的修正公式：X[n|n]=X[n|n-1]+K[n]*Residul[n]。
所以問題只剩下如何找到合適的K[n]使得估計最優，即后驗誤差協方差P[n|n]最小(越小說明越接近真實值X[n])
即在連續時間下，后驗誤差協方差Pt 滿足：
這里推廣到聯系時間下證明，只是為了方便求極值，結論可以在離散時間適應。
- 推導過程(人生苦短，我就是不撞南牆不回頭，價值觀不同的大概可以跳過這的)【這里我給出截圖吧，本來在WORD編輯的】：
Kalman 過程詳解：
(1) 預測：做出先驗估計x[n|n-1]=A*x[n-1|n-1];
【對於一維的情況，A可以看成一個常數使用，經常取1，同時對於B經常取零(---可能有人會有疑問：取0沒事嗎，可以放心的告訴你，問題不大。反過來想想，這只是一個估計，可以在估計噪聲方差得到修正)】
(2) 向前推算協方差：做出預測后的新的概率分布的方差（預測上次的最優估計為當前時刻的先驗估計這個過程可以當成一個符合預測過程噪聲分布的和另一個(上一次的最優估計可以看做高斯分布的)也符合高斯分布的相加。預測結果也是符合高斯噪聲分布的，方差是兩個相互獨立的方差之和）。
【對於一維的情況,P[n|n-1]=P[n-1|n-1]+Q。 Q為預測方差，代表對預測的不信任程度，工程上根據實際調節以改善濾波器的性能:動態效果和去噪效果】
(3) 計算卡爾曼增益：
【對於一維的情況，K[n]=H*P[n|n-1]/{H^2*P[n|n-1]+R}。其中H是對觀測的響應倍數，通常取1，R為測量的方差，工程上一般都可以直接獲得】
(4) 更估計值：做出后驗估計，修正后的估計值，更接近真實值。
【對於一維的情況，最優估計由下式給出：
x[n|n]=x[n|n-1]+K[n]*{z[n]-x[n|n-1]}。其中z[n]為觀測值】
(5) 更新誤差協方差：得到最優估計的概率分布的方差。
【對於一維的情況，新的誤差協方差由下式給出：
P[n|n]=(1-K[n]*H)*P[n|n-1]】
#include "Kalman.h" /** *@function: - 卡爾曼濾波器初始化 *@kalmanFilter：卡爾曼濾波器結構體 *@init_x：待測量的初始值 *@init_p：后驗狀態估計值誤差的方差的初始值 */ void kalmanFilter_init(KalmanStructTypedef *kalmanFilter, float init_x, float init_p,float predict_q,float newMeasured_q) { kalmanFilter->x = init_x;//待測量的初始值，如有中值一般設成中值 kalmanFilter->p = init_p;//后驗狀態估計值誤差的方差的初始值（不要為0問題不大） kalmanFilter->A = 1; kalmanFilter->H = 1; kalmanFilter->q = predict_q;//預測（過程）噪聲方差 影響收斂速率，可以根據實際需求給出 kalmanFilter->r = newMeasured_q;//測量（觀測）噪聲方差 可以通過實驗手段獲得 } /** *@function: - 卡爾曼濾波器 *@kalmanFilter:卡爾曼結構體 *@newMeasured；測量值 *返回濾波后的值 */ float kalmanFilter_filter(KalmanStructTypedef *kalmanFilter, float newMeasured) { /* Predict */ kalmanFilter->x = kalmanFilter->A * kalmanFilter->x;//%x的先驗估計由上一個時間點的后驗估計值和輸入信息給出 kalmanFilter->p = kalmanFilter->A * kalmanFilter->A * kalmanFilter->p + kalmanFilter->q; /*計算先驗均方差 p(n|n-1)=A^2*p(n-1|n-1)+q */ /* Correct */ kalmanFilter->gain = kalmanFilter->p * kalmanFilter->H / (kalmanFilter->p * kalmanFilter->H * kalmanFilter->H + kalmanFilter->r); kalmanFilter->x = kalmanFilter->x + kalmanFilter->gain * (newMeasured - kalmanFilter->H * kalmanFilter->x);//利用殘余的信息改善對x(t)的估計，給出后驗估計，這個值也就是輸出 kalmanFilter->p = (1 - kalmanFilter->gain * kalmanFilter->H) * kalmanFilter->p;//%計算后驗均方差 return kalmanFilter->x; }
#ifndef _Kalman_H_ #define _Kalman_H_ //標量卡爾曼濾波 typedef struct { float x; // 系統的狀態量 float A; // x(n)=A*x(n-1)+u(n),u(n)~N(0,q) float H; // z(n)=H*x(n)+w(n),w(n)~N(0,r) float q; // 預測過程噪聲協方差 float r; // 測量過程噪聲協方差 float p; // 估計誤差協方差 float gain;//卡爾曼增益 }KalmanStructTypedef; void kalmanFilter_init(KalmanStructTypedef *kalmanFilter, float init_x, float init_p,float predict_q,float newMeasured_q); float kalmanFilter_filter(KalmanStructTypedef *kalmanFilter, float newMeasured); #endif