卡爾曼濾波算法總結(jié)

2015.12.12void Kalman_Filter(float Gyro,float Accel)Angle+=(Gyro - Q_bias) * dt; Pdot0=Q_angle - PP01 - PP10; Pdot1= - PP11;Pdot2= - PP11;Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt;PP11 += Pdot3 * dt; Angle_err = Accel - Angle;PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * PP01;PP00 -= K_0 * t_0;PP01 -= K_0 * t_1;PP10 -= K_1 * t_0;PP11 -= K_1 * t_1;Angle+= K_0 * Angle_err;Q_bias+= K_1 * Angle_err;Gyro_x = Gyro - Q_bias;首先是卡爾曼濾波的5個(gè)方程： 5個(gè)式子比較抽象，現(xiàn)在直接用實(shí)例來(lái)說(shuō)：一、卡爾曼濾波第一個(gè)式子對(duì)于角度來(lái)說(shuō)，我們認(rèn)為此時(shí)的角度可以近似認(rèn)為是上一時(shí)刻的角度值加上上一時(shí)刻陀螺儀測(cè)得的角加速度值乘以時(shí)間，因?yàn)?，角度微分等于時(shí)間的微分乘以角速度。但是陀螺儀有個(gè)靜態(tài)漂移（而且還是變化的），靜態(tài)漂移就是靜止了沒(méi)有角速度然后陀螺儀也會(huì)輸出一個(gè)值，這個(gè)值肯定是沒(méi)有意義的，計(jì)算時(shí)要把它減去。由此我們得到了當(dāng)前角度的預(yù)測(cè)值A(chǔ)ngleAngle=Angle+(Gyro - Q_bias) * dt; 其中等號(hào)左邊Angle為此時(shí)的角度，等號(hào)右邊Angle為上一時(shí)刻的角度，Gyro 為陀螺儀測(cè)的角速度的值，dt是兩次濾波之間的時(shí)間間隔，我們的運(yùn)行周期是4ms或者6ms。同時(shí) Q_bias也是一個(gè)變化的量。但是就預(yù)測(cè)來(lái)說(shuō)認(rèn)為現(xiàn)在的漂移跟上一時(shí)刻是相同的，即Q_bias=Q_bias將上面兩個(gè)式子寫(xiě)成矩陣的形式得到上式，這個(gè)式子對(duì)應(yīng)于卡爾曼濾波的第一個(gè)式子 為2維列向量，A為2維方陣，為2維列向量，B 為2維列向量，為二、卡爾曼濾波第二個(gè)式子接著是預(yù)測(cè)方差陣的預(yù)測(cè)值，這里首先要給出兩個(gè)值，一個(gè)是漂移的噪聲，一個(gè)是角度值的噪聲，（所謂噪聲就是數(shù)據(jù)的方差值）這里的Q為向量的協(xié)方差矩陣，即，因?yàn)槠圃肼暫徒嵌仍肼暿窍嗷オ?dú)立的，則。又由性質(zhì)可知即方差，所以得到的矩陣如下，這里的兩個(gè)方差值是開(kāi)始就給出的常數(shù)程序中的定義如下float Q_angle=0.001; float Q_gyro=0.003;接著是這一部分A P(k-1|k-1) A,其中的（P（k-1）|P(k-1)）為上一時(shí)刻的預(yù)測(cè)方差陣卡爾曼濾波的目標(biāo)就是要讓這個(gè)預(yù)測(cè)方差陣最小。其中P(k-1|k-1)設(shè)為,第一式已知A為則計(jì)算A P(k-1|k-1) A+Q（就是個(gè)矩陣乘法和加法，算算吧）結(jié)果如下很小為了計(jì)算簡(jiǎn)便忽略不計(jì)。于是得到a,b,c,d分別和矩陣的P00,P01,P10,P11計(jì)算過(guò)程轉(zhuǎn)化為如下程序，代換即可 Pdot0=Q_angle - PP01 - PP10; Pdot1= - PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt;PP11 += Pdot3 * dt; 三，這里是卡爾曼濾波的第三個(gè)式子 Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (3)/計(jì)算卡爾曼增益即計(jì)算卡爾曼增益，這是個(gè)二維向量設(shè)為，這里的 = 為由此kg=P(K|K-1)+R，這里又有一個(gè)常數(shù)R，程序中的定義如下float R_angle=0.5;這個(gè)指的是角度測(cè)量噪聲值，則式子的分母=P00+R_angle即程序中的PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0;分子于是求出K_0 = PCt_0 / E;K_1 = PCt_1 / E;四，用誤差還有卡爾曼增益來(lái)修正X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1) (4)通過(guò)卡爾曼增益進(jìn)行修正這個(gè)矩陣帶進(jìn)去就行了Z（k)=Accel.注意這個(gè)是加速度計(jì)算出來(lái)的角度Angle_err = Accel - Angle;對(duì)應(yīng)程序如下Angle+= K_0 * Angle_err;Q_bias+= K_1 * Angle_err;同時(shí)為了PID控制還有下次的使用把角速度算出來(lái)了Gyro_x = Gyro - Q_bias;五，最后一步對(duì)矩陣P進(jìn)行更新，因?yàn)橄乱淮螢V波時(shí)要用到PP00 -= K_0 * t_0;PP01 -= K_0 * t_1;PP10 -= K_1 * t_0;PP11 -= K_1 * t_1;P(k|k)=（I-Kg(k) H）P(k|k-1) (5)/跟預(yù)測(cè)方差陣這個(gè)很簡(jiǎn)單，矩陣帶進(jìn)去算就行了六，總結(jié)卡爾曼濾波一共只需要給很少的初始值量，float Q_angle=0.001; float Q_gyro=0.003;還有float R