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

1、/*/* kalman.c */* 1-d kalman filter algorithm, using an inclinometer and gyro */* author: rich chi ooi */* version: 1.0 */* date:30.05.2003 */* adapted from trammel hudson(hudson) */* - */* compilationprocedure: */* linux */* gcc68 -cxxxxxx.c (to create object file) */* gcc68 -oxxxxxx.hex xxxxxx.o p

2、pwa.o */*upload data : */* ul filename.txt */*/* in this version: */*/* this is a free software; you can redistribute it and/or modify */* it under the terms of the gnu general public license as published */* by the free software foundation; either version 2 of the license, */* or (at your option) a

3、ny later version. */* */* this code is distributed in the hope that it will be useful, */* but without any warranty; without even the implied warranty of */* merchantability or fitness for a particular purpose.see the */* gnu general public license for more details. */* */* you should have received

4、a copy of the gnu general public license */* along with autopilot; if not, write to the free software */* foundation, inc., 59 temple place, suite 330, boston, */* ma02111-1307usa */*/#include #include eyebot.h/* the state is updated with gyro rate measurement every 20ms* change this value if you up

5、date at a different rate.*/static const float dt = 0.02;/* the covariance matrix.this is updated at every time step to* determine how well the sensors are tracking the actual state.*/static float p22 = 1, 0 , 0, 1 ;/* our two states, the angle and the gyro bias.as a byproduct of computing* the angle

6、, we also have an unbiased angular rate available.these are* read-only to the user of the module.*/float angle;float q_bias;float rate;/* the r represents the measurement covariance noise.r=evvt* in this case,it is a 1x1 matrix that says that we expect* 0.1 rad jitter from the inclinometer.* for a 1

7、x1 matrix in this case v = 0.1*/static const float r_angle = 0.001 ;/* q is a 2x2 matrix that represents the process covariance noise.* in this case, it indicates how much we trust the inclinometer* relative to the gyros.*/static const float q_angle = 0.001;static const float q_gyro= 0.0015;/* state

8、_update is called every dt with a biased gyro measurement* by the user of the module.it updates the current angle and* rate estimate.* the pitch gyro measurement should be scaled into real units, but* does not need any bias removal.the filter will track the bias.* a = 0 -1 * 00 */void stateupdate(co

9、nst float q_m)float q;float pdot4;/* unbias our gyro */q = q_m - q_bias;/當(dāng)前角速度：測(cè)量值-估計(jì)值/* compute the derivative of the covariance matrix* (equation 22-1)* pdot = a*p + p*a + q*/pdot0 = q_angle - p01 - p10; /* 0,0 */pdot1 = - p11; /* 0,1 */pdot2 = - p11; /* 1,0 */pdot3 = q_gyro; /* 1,1 */* store our

10、unbias gyro estimate */rate = q;/* update our angle estimate* angle += angle_dot * dt* += (gyro - gyro_bias) * dt* += q * dt*/angle += q * dt;/角速度積分累加到 估計(jì)角度/* update the covariance matrix */p00 += pdot0 * dt;p01 += pdot1 * dt;p10 += pdot2 * dt;p11 += pdot3 * dt;/* kalman_update is called by a user o

11、f the module when a new* inclinoometer measurement is available.* this does not need to be called every time step, but can be if* the accelerometer data are available at the same rate as the* rate gyro measurement.* h= 1 0 * because the angle measurement directly corresponds to the angle* estimate a

12、nd the angle measurement has no relation to the gyro* bias.*/void kalmanupdate(const float incangle)/* compute our measured angle and the error in our estimate */float angle_m = incangle;float angle_err = angle_m - angle;/1.12 zk-h*xk_dot/* h_0 shows how the state measurement directly relates to* th

13、e state estimate. * * h = h_0 h_1* the h_1 shows that the state measurement does not relate* to the gyro bias estimate.we dont actually use this, so* we comment it out.*/float h_0 = 1;/* const floath_1 = 0; */* precompute ph as the term is used twice* note that h0,1 = h_1 is zero, so that term is no

14、t not computed*/const float pht_0 = h_0*p00; /* + h_1*p01 = 0*/const float pht_1 = h_0*p10; /* + h_1*p11 = 0*/* compute the error estimate:* (equation 21-1)* e = h p h + r*/float e = r_angle +(h_0 * pht_0);/* compute the kalman filter gains:* (equation 21-2)* k = p h inv(e) */float k_0 = pht_0 / e;f

15、loat k_1 = pht_1 / e;/* update covariance matrix:* (equation 21-3)* p = p - k h p * let* y = h p*/float y_0 = pht_0;/*h_0 * p00*/float y_1 = h_0 * p01;p00 -= k_0 * y_0;p01 -= k_0 * y_1;p10 -= k_1 * y_0;p11 -= k_1 * y_1;/* update our state estimate:* xnew = x + k * error* err is a measurement of the

16、difference in the measured state* and the estimate state.in our case, it is just the difference* between the inclinometer measured angle and the estimated angle.*/angle += k_0 * angle_err;q_bias += k_1 * angle_err; /現(xiàn)在智能小車上用的卡爾曼濾波算法。由于做平衡小車，然后對(duì)那段濾波算法很疑惑，然后網(wǎng)上講的又比較少，我看了一段時(shí)間的書。這是小弟的對(duì)這段卡爾曼濾波程序的一點(diǎn)理解，因?yàn)榛A(chǔ)

17、薄弱（大二），有錯(cuò)的請(qǐng)多多包涵。先上程序，這是抄的不知道誰的代碼。抱歉了。不過這程序好像都寫的差不多void 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;pc

18、t_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è)方程x(k|k-1)=a x(k-1|k-1

19、)+b u(k) . (1)/先驗(yàn)估計(jì)p(k|k-1)=a p(k-1|k-1) a+q (2)/協(xié)方差矩陣的預(yù)測(cè)kg(k)= p(k|k-1) h / (h p(k|k-1) h + r) (3)/計(jì)算卡爾曼增益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è)式子比較抽象，現(xiàn)在直接用實(shí)例來說，對(duì)于角度來說，我們認(rèn)為此時(shí)的角度可以近似認(rèn)為是上一時(shí)刻的角度值加上上一時(shí)刻陀螺儀測(cè)得的角加速度值乘以時(shí)間，因?yàn)?，角度微分等于時(shí)間的微分乘以角速度。但是陀螺儀有個(gè)靜

20、態(tài)漂移（而且還是變化的），靜態(tài)漂移就是靜止了沒有角速度然后陀螺儀也會(huì)輸出一個(gè)值，這個(gè)值肯定是沒有意義的，計(jì)算時(shí)要把它減去。由此我們得到了當(dāng)前角度的預(yù)測(cè)值 angleangle=angle+(gyro - q_bias) * dt; 其中等號(hào)左邊angle為此時(shí)的角度，等號(hào)右邊angle為上一時(shí)刻的角度，gyro 為陀螺儀測(cè)的角速度的值，dt是兩次濾波之間的時(shí)間間隔。float dt=0.005; 這是程序中的定義同時(shí) q_bias也是一個(gè)變化的量。但是就預(yù)測(cè)來說認(rèn)為現(xiàn)在的漂移跟上一時(shí)刻是相同的即q_bias=q_bias將兩個(gè)式子寫成矩陣的形式得到上式，這個(gè)式子對(duì)應(yīng)于卡爾曼濾波的第一個(gè)式子x(

21、k|k-1)=a x(k-1|k-1)+b u(k) . (1)/先驗(yàn)估計(jì)x(k|k-1)為2維列向量，a為2維方陣，x(k-1|k-1)為2維列向量，b 為2維列向量，u(k) 為gyro二，這里是卡爾曼濾波的第二個(gè)式子接著是預(yù)測(cè)方差陣的預(yù)測(cè)值，這里首先要給出兩個(gè)值，一個(gè)是漂移的噪聲，一個(gè)是角度值的噪聲，（所謂噪聲就是數(shù)據(jù)的方差值）p(k|k-1)=a p(k-1|k-1) a+q 這里的q為向量 的協(xié)方差矩陣，即因?yàn)槠圃肼曔€有角度噪聲是相互獨(dú)立的，則=0；=0又由性質(zhì)可知cov（x，x）=d（x）即方差，所以得到的矩陣如下，這里的兩個(gè)方差值是開始就給出的常數(shù)程序中的定義如下float q

22、_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ì)算過程轉(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