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

1、由于做平衡小車，然后對那段濾波算法很疑惑，然后網(wǎng)上講的又比較少，我看了一段時間的書。這是小弟的對這段卡爾曼濾波程序的一點理解，因為基礎(chǔ)薄弱（大二），有錯的請多多包涵。先上程序，這是抄的不知道誰的代碼。抱歉了。不過這程序好像都寫的差不多void Kalman_Filter(float Gyro,float AccelAngle+=(Gyro - Q_bias * dt; Pdot0=Q_angle - PP01 - PP10; /Pdot1= - PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP

2、10 += 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 *

3、 Angle_err; Gyro_x = Gyro - Q_bias; 首先是卡爾曼濾波的5個方程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/協(xié)方差矩陣的預(yù)測Kg(k= P(k|k-1 H / (H P(k|k-1 H + R (3/計算卡爾曼增益X(k|k= X(k|k-1+Kg(k (Z(k - H X(k|k-1 (4通過卡爾曼增益進行修正P(k|k=（I-Kg(k H）P(k|k-1 (5/跟新協(xié)方差陣5個式子比較抽象，現(xiàn)在直接用實例來說，對于角度來說，我們認為此時的角度可以近似認為是上一時刻的角度值加上上

4、一時刻陀螺儀測得的角加速度值乘以時間，因為，角度微分等于時間的微分乘以角速度。但是陀螺儀有個靜態(tài)漂移（而且還是變化的），靜態(tài)漂移就是靜止了沒有角速度然后陀螺儀也會輸出一個值，這個值肯定是沒有意義的，計算時要把它減去。由此我們得到了當前角度的預(yù)測值 AngleAngle=Angle+(Gyro - Q_bias * dt; 其中等號左邊Angle為此時的角度，等號右邊Angle為上一時刻的角度，Gyro 為陀螺儀測的角速度的值，dt是兩次濾波之間的時間間隔。float dt=0.005; 這是程序中的定義同時 Q_bias也是一個變化的量。但是就預(yù)測來說認為現(xiàn)在的漂移跟上一時刻是相同的即Q_bi

5、as=Q_bias將兩個式子寫成矩陣的形式得到上式，這個式子對應(yīng)于卡爾曼濾波的第一個式子X(k|k-1=A X(k-1|k-1+B U(k . (1/先驗估計X(k|k-1為2維列向量，A為2維方陣，X(k-1|k-1為2維列向量，B 為2維列向量，U(k 為Gyro二，這里是卡爾曼濾波的第二個式子接著是預(yù)測方差陣的預(yù)測值，這里首先要給出兩個值，一個是漂移的噪聲，一個是角度值的噪聲，（所謂噪聲就是數(shù)據(jù)的方差值）P(k|k-1=A P(k-1|k-1 A+Q 這里的Q為向量 的協(xié)方差矩陣，即因為漂移噪聲還有角度噪聲是相互獨立的，則=0；=0又由性質(zhì)可知cov（x，x）=D（x）即方差，所以得到的

6、矩陣如下，這里的兩個方差值是開始就給出的常數(shù)程序中的定義如下float Q_angle=0.001; float Q_gyro=0.003;接著是這一部分A P(k-1|k-1 A,其中的（P（k-1）|P(k-1）為上一時刻的預(yù)測方差陣卡爾曼濾波的目標就是要讓這個預(yù)測方差陣最小。其中P(k-1|k-1設(shè)為,第一式已知A為 則計算A P(k-1|k-1 A+Q（就是個矩陣乘法和加法，算算吧）結(jié)果如下很小為了計算簡便忽略不計。于是得到a,b,c,d分別和矩陣的P00,P01,P10,P11計算過程轉(zhuǎn)化為如下程序，代換即可Pdot0=Q_angle - PP01 - PP10; Pdot1= -

7、PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt;PP11 += Pdot3 * dt; 三，這里是卡爾曼濾波的第三個式子Kg(k= P(k|k-1 H / (H P(k|k-1 H + R (3/計算卡爾曼增益即計算卡爾曼增益，這是個二維向量設(shè)為，這里的 = 為由此kg=P(K|K-1+R，這里又有一個常數(shù)R，程序中的定義如下float R_angle=0.5;這個指的是角度測量噪聲值，則式子的分母=P00+R_angle即程序中的PCt_0 = C_0 *

8、PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0;分子 于是求出K_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通過卡爾曼增益進行修正這個矩陣帶進去就行了Z（k=Accel.注意這個是加速度計算出來的角度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=0.001; float Q_g