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

1、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

2、;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è)方程：1）先驗(yàn)估計(jì)2）協(xié)方差矩陣的預(yù)測X(k |k 1) AX(k 1|k 1) Bu(k)P(k |k 1) AP(k 1|k 1)A Q(3) 計(jì)算卡爾曼增益

3、(4) 進(jìn)行修正(5) 更新協(xié)方差陣Kg(k) P(k|k 1)H/HP(k|k 1)H R)X(k|k) X(k|k 1) Kg (k)(Z(k) HX (k|k 1)P(k|k) (I Kg(k) H) P(k |k 1)5個(gè)式子比較抽象，現(xiàn)在直接用實(shí)例來說:一、卡爾曼濾波第一個(gè)式子對于角度來說，我們認(rèn)為此時(shí)的角度可以近似認(rèn)為是上一時(shí)刻的角度值加上 上一時(shí)刻陀螺儀測得的角加速度值乘以時(shí)間，因?yàn)?d dt ，角度微分等于時(shí) 間的微分乘以角速度。但是陀螺儀有個(gè)靜態(tài)漂移(而且還是變化的)，靜態(tài)漂移就是靜止了沒有角速度然后陀螺儀也會(huì)輸出一個(gè)值，這個(gè)值肯定是沒有意義的， 計(jì)算時(shí)要把它減去。由此我們得

4、到了當(dāng)前角度的預(yù)測值 An gleAn gle=A ngle+(Gyro - Q_bias) * dt;其中等號左邊Angle為此時(shí)的角度，等號右邊 Angle為上一時(shí)刻的角度，Gyro 為陀螺儀測的角速度的值，dt是兩次濾波之間的時(shí)間間隔，我們的運(yùn)行周期是 4ms或者6ms同時(shí)Q_bias也是一個(gè)變化的量。但是就預(yù)測來說認(rèn)為現(xiàn)在的漂移跟上一時(shí)刻是相同的，即Q_bias=Q_bias將上面兩個(gè)式子寫成矩陣的形式An gledt AngleQ_bias1 Q_biasdtGyro0得到上式，這個(gè)式子對應(yīng)于卡爾曼濾波的第一個(gè)式子X(k |k 1) AX(k 1| k 1) Bu(k)X k|k 1

5、為2維列向量列向量AngleQ_biasAn gle Q_biasB為2維列向量卡爾曼濾波第二個(gè)式子，A為2維方陣dt0u k 為 GyrodtX k-1|k 1 為 2 維接著是預(yù)測方差陣的預(yù)測值，這里首先要給出兩個(gè)值，一個(gè)是漂移的噪聲,一個(gè)是角度值的噪聲，(所謂噪聲就是數(shù)據(jù)的方差值)P(k|k 1) AP(k 1| k 1)A Q這里的Q為向量An gle的協(xié)方差矩陣，即Q_biascov(Angle,Angle) cov(Q_bias,Angle) cov(A ngle,Q_bias)cov(Q_bias,Q_biaS)因?yàn)槠圃肼暫徒嵌仍肼暿窍嗷オ?dú)立的，則cov(A ngle,Q_bi

6、as) 0 又由性質(zhì)可知cov(x,x) D(x)即方差，所以得到的矩陣如下D(A ngle)00D(Q_bias)，這里的兩個(gè)方差值是開始就給出的常數(shù)程序中的定義如下float Q_an gle=;float Q_gyro=;接著是這一部分 A P(k-1|k- 1) A ,其中的(P( k-1)|P(k-1)為上一時(shí)刻的 預(yù)測方差陣卡爾曼濾波的目標(biāo)就是要讓這個(gè)預(yù)測方差陣最小。其中 P(k-1|k-1)設(shè)為1式已知A為0dt1則計(jì)算A P(k-1|k- 1) A +Q (就是個(gè)矩陣乘法和加法，算算吧)結(jié)果如下D(Angle) b d dtd2a c dt b dt d.(dt)c d dt2

7、d.(dt)很小為了計(jì)算簡便忽略不計(jì)。于是得到a c dt b dt D(Angle) b d dtc d dtda,b,c,d分別和矩陣的 P00,P01,P10,P11計(jì)算過程轉(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)k

8、o：H 1k1P00P10KoKi注意這個(gè)是加速度計(jì)算出來的角度An gle_err = Accel - An gle;對應(yīng)程序如下An gle += K_0 * An gle_err;Q_bias += K_1 * An gle_err;同時(shí)為了 PID控制還有下次的使用把角速度算出來了Gyro_x = Gyro - Q_bias;五，最后一步對矩陣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ù)測方差陣這個(gè)很簡單，矩陣帶進(jìn)去算就行了六，總結(jié)卡爾曼濾波一共只需要給很少的初始值量，float Q_an gle=;float Q_gyro=;還有