一.引言
1.卡爾曼濾波中的真實值,測量值,預測值,估計值怎麼區分?
他的5條公式是其核心內容，結合現代的計算機,其實卡爾曼的程序相當的簡單,只要你理解了他的那5條公式.
用一個簡單的小例子：假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的， 也就是現在這一分鐘的溫度等於過去一分鐘的溫度（假設我們用一分鐘來做時間單位）（先驗估計） 。假設你對你的經驗不是 100% 的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲，也就是這些偏差跟前後時間是沒有關係的而且符合高斯分佈。另外，我們在房間裏放一個溫度計，但是這個溫度計也不準確的， 測量值和實際值有偏差，我們也把這些偏差看成是高斯白噪聲。好了， 現在對於某一分鐘我們有兩個有關於該房間的溫度值： 你根據經驗的預測值 （系統的 預測值）和溫度計的值（測量值）。
Kalman要解決的問題是如何使用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算k時刻的實際溫度值.首先你要根據k-1時刻的溫度值,來預測k時刻的溫度.因爲你相信溫度是恆定的,所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的,假定是23度,同時該值的高斯噪聲的偏差是5度[5是這樣得到的：如果k-1時刻最優估計誤差爲3,你對自己預測噪聲標準差是4度,他們平方和再開方,就是5）.至於爲何是平方和，可以看做兩個高斯過程相加[上次最優估計結果是個高斯過程，這次預測也是高斯過程]，所得的也是高斯過程，方差爲原先兩者的方差之和）]
然後，你從溫度計那裏得到了 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 ） 。 他可以隨不同的時刻而改變他自己的值，是不是很神奇！
2.卡爾曼濾波遞歸過程
卡爾曼濾波器涉及一些基本的概念知識,包括概率（Probability）,隨機變量（Random Variable）,高斯或正態分配（Gaussian Distribution）等.
首先,要引入一個離散控制過程的系統：該系統可用一個線性隨機微分方程來描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系統的測量值：Z(k)=H X(k)+V(k)上兩式子中,X(k)是k時刻的系統狀態,U(k)是k時刻對系統的控制量.A和B是系統參數,對於多模型系統,他們爲矩陣.Z(k)是k時刻的測量值,H是測量系統的參數,對於多測量系統,H爲矩陣.[常見的：A爲狀態轉移矩陣;H爲觀測矩陣].W(k)和V(k)分別表示過程和測量的噪聲.他們被假設成高斯白噪聲,他們的協方差（covariance）分別是Q,R（這裏我們假設他們不隨系統狀態變化而變化）.
過程：對於滿足上面的條件(線性隨機微分系統,過程和測量都是高斯白噪聲),卡爾曼濾波器是最優的信息處理器.首先我們要利用系統的過程模型,來預測下一狀態的系統.假設現在的系統狀態是k,根據系統的模型,可以基於系統的上一狀態而預測出現在狀態：X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中,X(k|k-1)是利用上一狀態預測的結果,X(k-1|k-1)是上一狀態最優的結果,U(k)爲現在狀態的控制量,如果沒有控制量,它可以爲0.到現在爲止,我們的系統結果已經更新了,可是,對應於X(k|k-1)的covariance還沒更新.我們用P表示covariance：P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中,P(k|k-1)是X(k|k-1)對應的covariance,P(k-1|k-1)是X(k-1|k-1)對應的covariance,A’表示A的轉置矩陣,Q是系統過程的covariance.式子1,2就是卡爾曼濾波器5個公式當中的前兩個,也就是對系統的預測.現在我們有了現在狀態的預測結果,然後我們再收集現在狀態的測量值.結合預測值和測量值,我們可以得到現在狀態(k)的最優化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg爲卡爾曼增益(Kalman Gain)：Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
到現在爲止,我們已經得到了k狀態下最優的估算值X(k|k).但是爲了要另卡爾曼濾波器不斷的運行下去直到系統過程結束,我們還要更新k狀態下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1)………(5)
其中I爲1的矩陣,對於單模型單測量,I=1.當系統進入k+1狀態時,P(k|k)就是式子(2)的P(k-1|k-1).這樣,算法就可以自迴歸的運算下去.
卡爾曼濾波器的原理基本描述了,根據這5個公式,可以很容易的實現計算機的程序.
最優估計的協方差（誤差協方差）：爲何要稱作協方差呢？協方差是兩個變量間的總體誤差，P(k|k-1)可以理解爲這是上一次最優估計和當前預測值(從時間上看是兩個變量)的協方差。
3.卡爾曼濾波器的工作過程：
把房間看成一個系統,然後對這個系統建模:
運動方程：X(k)=A X(k-1)+B U(k)+W(k)
觀測方程：Z(k)=H X(k)+V(k)
房間的溫度是跟前一時刻的溫度相同的,所以A=1.沒有控制量,所以U(k)=0.因此得出：X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因爲測量的值是溫度計的,跟溫度直接對應,所以H=1.式子3,4,5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ………
P(k|k)=（1-Kg(k)）P(k|k-1)………(10)
現在模擬一組測量值作爲輸入.假設房間的真實溫度爲25度,模擬200個測量值,這些測量值的平均值爲25度,但是加入了標準偏差爲幾度的高斯白噪聲.爲了令卡爾曼濾波器開始工作,需要告訴卡爾曼兩個零時刻的初始值,是X(0|0)和P(0|0).他們的值不用太在意,隨便給一個就可以了,因爲隨着卡爾曼的工作,X會逐漸的收斂.但是對於P,一般不要取0,因爲這樣可能會令卡爾曼完全相信你給定的X(0|0)是系統最優的,從而使算法不能收斂.選取X(0|0)=1度,P(0|0)=10.
二.Kalman 的物理意義
1.首先確定幾個符號（十分重要）：
2.幾個假設和模型
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])
3.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;//測量（觀測）噪聲方差R，可以通過實驗手段獲得
}
/**
*@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