卡爾曼濾波算法代碼總結(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;/當前角速度：測量值-估計值/* 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;/角速度積分累加到 估計角度/* 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; /現在智能小車上用的卡爾曼濾波算法。由于做平衡小車，然后對那段濾波算法很疑惑，然后網上講的又比較少，我看了一段時間的書。這是小弟的對這段卡爾曼濾波程序的一點理解，因為基礎

17、薄弱（大二），有錯的請多多包涵。先上程序，這是抄的不知道誰的代碼。抱歉了。不過這程序好像都寫的差不多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個方程x(k|k-1)=a x(k-1|k-1

19、)+b u(k) . (1)/先驗估計p(k|k-1)=a p(k-1|k-1) a+q (2)/協(xié)方差矩陣的預測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個式子比較抽象，現在直接用實例來說，對于角度來說，我們認為此時的角度可以近似認為是上一時刻的角度值加上上一時刻陀螺儀測得的角加速度值乘以時間，因為，角度微分等于時間的微分乘以角速度。但是陀螺儀有個靜

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

21、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二，這里是卡爾曼濾波的第二個式子接著是預測方差陣的預測值，這里首先要給出兩個值，一個是漂移的噪聲，一個是角度值的噪聲，（所謂噪聲就是數據的方差值）p(k|k-1)=a p(k-1|k-1) a+q 這里的q為向量 的協(xié)方差矩陣，即因為漂移噪聲還有角度噪聲是相互獨立的，則=0；=0又由性質可知cov（x，x）=d（x）即方差，所以得到的矩陣如下，這里的兩個方差值是開始就給出的常數程序中的定義如下float q

22、_angle=0.001; float q_gyro=0.003;接著是這一部分a p(k-1|k-1) a,其中的（p（k-1）|p(k-1)）為上一時刻的預測方差陣卡爾曼濾波的目標就是要讓這個預測方差陣最小。其中p(k-1|k-1)設為,第一式已知a為則計算a p(k-1|k-1) a+q（就是個矩陣乘法和加法，算算吧）結果如下很小為了計算簡便忽略不計。于是得到a,b,c,d分別和矩陣的p00,p01,p10,p11計算過程轉化為如下程序，代換即可 pdot0=q_angle - pp01 - pp10; pdot1= - 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)/計算卡爾曼增益即計算卡爾曼增益，這是個二維向量設為，這里的 = 為由此kg=p(k|k-1