卡爾曼濾波算法總結(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 個方程：1 ）先驗估計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

3、)計算卡爾曼增益(4)進行修正(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個式子比較抽象，現(xiàn)在直接用實例來說:一、卡爾曼濾波第一個式子對于角度來說，我們認為此時的角度可以近似認為是上一時刻的角度值加上 上一時刻陀螺儀測得的角加速度值乘以時間，因為 d dt ,角度微分等于時 間的微分乘以角速度。但是陀螺儀有個靜態(tài)漂移(而且還是變化的)，靜態(tài)漂移就是靜止了沒有角速度然后陀螺儀也會輸出一個值，這個值肯定是沒有意義的，

4、計算時要把它減去。由此我們得到了當(dāng)前角度的預(yù)測值 Angle Angle=Angle+(Gyro - Q_bias) * dt;其中等號左邊Angle為此時的角度，等號右邊 Angle為上一時刻的角度，Gyro 為陀螺儀測的角速度的值，dt是兩次濾波之間的時間問隔，我們的運行周期是 4ms或者6ms同時Q_bias也是一個變化的量。但是就預(yù)測來說認為現(xiàn)在的漂移跟上一時刻是相同的，即Q_bias=Q_bias將上面兩個式子寫成矩陣的形式AngleQ biasdt Angle1 Q biasdtGyro 0得到上式，這個式子對應(yīng)于卡爾曼濾波的第一個式子X(k|k 1) AX(k 11k 1) Bu

5、(k)X k|k 1為2維列向量Angle , A為2維方陣1 dt , X k-1|k 1為2維Q bias01列向量Angle , B為2維列向量dt u k為GyroQ_bias0 ,二、卡爾曼濾波第二個式子接著是預(yù)測方差陣的預(yù)測值，這里首先要給出兩個值，一個是漂移的噪聲,一個是角度值的噪聲，(所謂噪聲就是數(shù)據(jù)的方差值)P(k|k 1) AP(k 11k 1)A' Q這里的Q為向量AngleQ bias的協(xié)方差矩陣，即cov(Angle,Angle) cov(Q_bias,Angle) cov(Angle,Q_bias)cov(Q_bias,Q_biaS)因為漂移噪聲和角度噪聲是

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

7、 dt b dt d.(dt) D(Angle) b d dtc d dtd2小(出)很小為了計算簡便忽略不計。于是得到a c dt b dt D(Angle) b d dt c d dtda,b,c,d 分別和矩陣的 P00,P01,P10,P11計算過程轉(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;三，這里是卡爾曼濾波的第

8、三個式子Kg(k尸P(k|k- 1)H'/ (H P(k|k-1)H'+ R) ko H 1 0 P00 K0k1P10 K1 .注意這個是加速度計算出來的角度Angle_err = Accel - Angle;對應(yīng)程序如下Angle += K_0 * Angle_err;Q_bias += K_1 * Angle_err;同時為了 PID控制還有下次的使用把角速度算出來了Gyro_x = Gyro - Q_bias;五，最后一步對矩陣P進行更新，因為下一次濾波時要用到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ù)測方差陣這個很簡單，矩陣帶進去算就行了六，總結(jié)卡爾曼濾波一共只需要給很少的初始值量，float Q_angle=;float Q_gy