平衡小車的卡爾曼濾波算法

1、角速度，加速度void Kalman_Filter(float Gyro,float Accel) (驗(yàn)估計(jì)上一刻角度加上（角速度-誤差）*時(shí)間Angle+=(Gyro - Q_bias) * dt;/協(xié)方差矩陣的預(yù)測(cè)Pdot0=Q_angle - PP1 - PP10; /Pdot1= - PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt;PP01 += Pdot1 * dt;PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;/通過卡爾曼增益進(jìn)行修整Angle_err = Accel - Angle;PCt_0

2、= 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;/更新協(xié)方差陣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ì)X(k|k

3、-1)=A X(k-1|k-1)+B U(k)P(k|k-1)=A P(k-1|k-1) A'+Q(2)/協(xié)方差矩陣的預(yù)測(cè)(3)計(jì)算卡爾曼增益Kg(k)= P(k|k-1) H ' / (H P(k|k-1) H ' + R)X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1) (4)通過卡爾曼增益進(jìn)行修正P(k|k)= (I-Kg(k) H ) P(k|k-1) (5) 跟新協(xié)方差陣卡爾曼濾波的5個(gè)方程1 .驗(yàn)估計(jì)2 .協(xié)方差矩陣預(yù)測(cè)3 .計(jì)算卡爾曼增益=4 .通過卡爾曼增益進(jìn)行修整5 .根新協(xié)方差陣 5個(gè)式子比較抽象，現(xiàn)在直接用實(shí)例來說

4、對(duì)于角度來說，我們認(rèn)為此時(shí)的角度可以近似認(rèn)為是上一時(shí)刻的角度值加上上一時(shí)刻陀螺儀測(cè)得的角加速度值乘以時(shí)間，因?yàn)閐Q =dt黑色，角度微分等于時(shí)間的微分乘以角速度。但是陀螺儀有個(gè)靜態(tài)漂移(而且還是變化的)，靜態(tài)漂移就是靜止了沒有角速度然后陀螺儀也會(huì)輸出一個(gè)值，這個(gè)值肯定是沒有意義的，計(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í)間間隔。float dt=0.005;這是程序中的定義同時(shí)Q_bia

5、s也是一個(gè)變化的量。但是就預(yù)測(cè)來說認(rèn)為現(xiàn)在的漂移跟上一時(shí)刻是相同的即Q_bias=Q_bias將兩個(gè)式子寫成矩陣的形式Angle1Q bias0Gyro-dt Angle | + dt1 Q bias 0得到上式，這個(gè)式子對(duì)應(yīng)于卡爾曼濾波的第一個(gè)式子X(k|k-1)=A X(k-1|k-1)+B U(k) . (1)/ 先驗(yàn)估計(jì)X(k|k-1)為2維列向量Angle , A為2維方陣1 出，X(k-1|k-1)為2維列向量Q bias01AngleQ biasB為2維列向量dt U(k) 0 ,為 Gyro,這里是卡爾曼濾波的第二個(gè)式子接著是預(yù)測(cè)方差陣的預(yù)測(cè)值，這里首先要給出兩個(gè)值，一個(gè)是漂移

6、的噪聲，一個(gè)是角度值的噪聲，(所謂噪聲就是 數(shù)據(jù)的方差值)cov(Angle,Angle) cov(Q_bias,Angle)cov(Angle,Q_bias) cov(Q_bias)P(k|k-1)=A P(k-1|k-1) A '+Q | 這里的Q為向量Angle Q biascov(Angle,Q_bias) =0因?yàn)槠圃肼曔€有角度噪聲是相互獨(dú)立的，則cov(Q_bias,Angle)=。又由性質(zhì)可知cov (x, x) =D (x)即方差，所以得到的矩陣如下D(Angle) 00 D(Q_bias),這里的兩個(gè)方差值是開始就給出的常數(shù)程序中的定義如下float Q_angle

7、=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 b，第一式已知 A為1 dt c d0 1則計(jì)算A P(k-1|k-1) A '+Q (就是個(gè)矩陣乘法和加法，算算吧)結(jié)果如下 .2 . .a -cxdt -bxdt +d.(dt) +D(Angle) b-d xdtc -d xdtd2d.(dt)很小為了計(jì)算簡(jiǎn)便忽略不計(jì)。于是得到Ia -cx dt -b xdt +D(Angle) b-d

8、dt c -d xdtda,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;(3)/計(jì)算卡爾曼增益三，這里是卡爾曼濾波的第三個(gè)式子Kg(k)= P(k|k-1) H ' / (H P(k|k-1) H ' + R)即計(jì)算卡爾曼增益，這是個(gè)二維向量設(shè)為k

9、0k1H =11 0為由此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;P00分子P10K0于是求出K1K_0 = PCt_0 / E;K_1 = PCt_1 / E;四，用誤差還有卡爾曼增益來修正X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1) (4)通過卡爾曼增益進(jìn)行修正這個(gè)矩陣帶進(jìn)去就行了Z (k尸

10、Accel.注意這個(gè)是加速度計(jì)算出來的角度Angle_err = Accel - Angle;對(duì)應(yīng)程序如下Angle += K_0 * Angle_err;Q_bias += K_1 * Angle_err;同時(shí)為了 PID控制還有下次的使用把角速度算出來了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.00