卡爾曼濾波：以陀螺儀測量的角速度作為**值的控制量，加速度感測器測量的角度作為觀測值。下面程式中angle_m為測量角度，gyro_m為測量角速度，gyro_m*dt為控制量。
以下程式是按卡爾曼濾波的五個公式來編寫的。
x(k|k-1)=a x(k-1|k-1)+b u(k) ……….. (1)
p(k|k-1)=a p(k-1|k-1) a』+q ……… (2)
x(k|k)= x(k|k-1)+kg(k) (z(k)-h x(k|k-1)) ……… (3)
kg(k)= p(k|k-1) h』 / (h p(k|k-1) h』 + r) ……… (4)
p(k|k)=（
i-kg(k) h
）p(k|k-1) ……… (5)
對於單輸入單輸出系統，a、b
、h、i
不為矩陣且值都為1。
卡爾曼濾波引數的調整：其引數有三個，p0
是初始化最優角度估計的協方差（初始化最優角度估計可設為零），它是乙個初值。
q是**值的協方差，
r是測量值的協方差。對q和
r的設定只需記住，
q/(q+r)
的值就是卡爾曼增益的收斂值，比如其值為
0.2，那麼卡爾曼增益會向
0.2收斂（對於
0.2的含**釋一下，比如**角度值是
5度，角度測量值是
10度，那麼最優化角度為：
5+0.2*
（10-5）=6
。從這裡可以看出，卡爾曼增益越小，說明**值越可靠，最優化角度越接近**值；相反的，卡爾曼增益越大，說明測量值越可靠，最優化角度越接近測量值）。p0/(q+r)
反映收斂的快慢程度，該值設定越小，收斂越快，該值越大，收斂越慢（這裡的p0
是指初始最優角度值的協方差），因為卡爾曼增益收斂總的來說是很快的，所以該值設定大一點或小一點都沒什麼關係。
注：以下程式只用於說明演算法，存在語法錯誤，初始的引數也是隨意給定的。
x=0; /* 最優角度初值*/
p=1; /* 最優角度對應協方差初值*/
dt=0.02;
q=0.0025；
r=0.25；
void kalman_filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
從上面的程式可以看出，卡爾曼濾波是乙個遞推過程，初始的最優角度值可設為x=0
，初始最優角度值的協方差
p一定不能設為零，
dt是取樣週期，q 與
r可共同決定卡爾曼增益收斂的大小。
通俗解釋卡爾曼濾波
在學習卡爾曼濾波器之前，首先看看為什麼叫 卡爾曼 跟其他著名的理論 例如傅利葉變換，泰勒級數等等 一樣，卡爾曼也是乙個人的名字，而跟他們不同的是，他是個現代人！簡單來說，卡爾曼濾波器是乙個 optimal recursive data processing algorithm 最優化自回歸資料處理演...
通俗解釋卡爾曼濾波
假設你有兩個感測器，測的是同乙個訊號。可是它們每次的讀數都不太一樣，怎麼辦？取平均。再假設你知道其中貴的那個感測器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？加權平均。怎麼加權？假設兩個感測器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，此處省略若干數學公式 ...
卡爾曼濾波程式matlab 解釋
clear n 200 取200個數 w 1 0 w randn 1,n 產生乙個1 n的行向量,第乙個數為0,w為過程雜訊 其和後邊的v在卡爾曼理論裡均為高斯白雜訊 x 1 0 狀態x初始值 a 1 a為狀態轉移陣,此程式簡單起見取1 for k 2 n x k a x k 1 w k 1 系統狀...