卡尔曼滤波算法代码总结

1、/*/* kalma n.c*/* 1-D Kalma n filter Algorithm, using an in cli no meter and gyro*/* Author: Rich Chi Ooi*/*/*Upload data :*/* ul file name.txt*/*/* In this versio n:*/* This is a free software; you can redistribute it and/or modify*/* it under the terms of the GNU General Public License as publishe

2、d*/* by the Free Software Foun dati on; either vers ion 2 of the Lice nse,*/* or (at your opti on) any later vers ion./*/*/ */* this code is distributed in the hope that it will be useful,*/* but WITHOUT ANY WARRANTY; without eve n the implied warra nty of*/* MERCHANTABILITY or FITNESS FOR A PARTICU

3、LAR PURPOSE. See the*/* GNU General Public License for more details./*/*/* You should have received a copy of the GNU Gen eral Public Lice nse*/* along with Autopilot; if not, write to the Free Software/* Fou ndati on, I nc., 59 Temple Place, Suite 330, Bost on,/* MA 02111-1307 USA*/*/*/*/#in clude

4、#in clude eyebot.h/* The state is updated with gyro rate measureme nt every 20ms* cha nge this value if you update at a differe nt rate./* Versio n: 1.0*/* Date:30.05.2003*/*AdaptedfromTrammelHuds on( huds on rotomoti on .com )*/*/* Compilatio n procedure:*/*/*/*/*/*Linuxgcc68 -c XXXXXX.c (to create

5、 object file) gcc68 -o XXXXXX.hex XXXXXX.o ppwa.ostatic const float dt = 0.02;/* The covarianee 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 an gle and the gyro bias.As a byproduct o

6、f computi ng* the an gle, we also have an un biased an gular rate available.These are* read-only to the user of the module.*/float an gle;float q bias;float rate;/* The R represe nts the measureme nt covaria nee no ise.R二EvvT* In this case,it is a 1x1 matrix that says that we expect* 0.1 rad jitter

7、from the in cli no meter* for a 1x1 matrix in this case v = 0.1*/ static con st float R_angle = 0.001 ;/* Q is a 2x2 matrix that represe nts the process covaria nee no ise.* In this case, it in dicates how much we trust the in cli no meter* relative to the gyros.*/static const float Q an gle = 0.001

8、;static const float Q_gyro = 0.0015;/* state_update is called every dt with a biased gyro measureme nt* by the user of the module. It updates the curre nt an gle and* rate estimate.* The pitch gyro measureme nt should be scaled into real un its, but* does not need any bias removal. The filter will t

9、rack the bias.* A = 0 -1 * 0 0 void stateUpdate(c on st float q_m)float q;float Pdot4;/* rate gyro measureme nt.* H = 1 0 * because the an gle measureme nt directly corresp onds to the an gle* estimate and the angle measurement has no relation to the gyro bias. Un bias our gyro */q = q_m - q_bias; 当

10、前角速度：测量值-估计值/* Compute the derivative of the covaria nee 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 un bias gyro estimate */rate = q;/* Update our an gle estimate* angle += ang

11、le dot * dt*+= (gyro -gyro bias) * dt*+= q * dt*/an gle += q * dt;/角速度积分累加到估计角度/* Update the covaria nee matrix */P00 += Pdot0 * dt;P01 += Pdot1 * dt;P10 += Pdot2 * dt;P11 += Pdot3 * dt; /* kalma n_update is called by a user of the module whe n a new* in cli noo meter measureme nt is available.* Thi

12、s does not need to be called every time step, but can be if* the accelerometer data are available at the same rate as thevoid kalma nU pdate(c onst float incAn gle)/ P = P - K H P Compute our measured angle and the error in our estimate */ float an gle_m = incAn gle;float an gle_err = an gle_m - an

13、gle;/1.12 zk-H*xk_dot/* h_0 shows how the state measurement directly relates to* the 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 comme nt it out.*/float h_0 = 1;/* const float h 1 = 0; */* Precom

14、pute PH as the term is used twice* Note that H0,1 = h 1 is zero, so that term is not not computed*/const float PHt_0 = h_0*P00; /* + h_1*P01 = 0*/ con st 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 an gle +(h 0 * PHt 0);/* Com

15、pute the Kalman filter gains:* (equation 21-2)* K = P H i nv(E)*/float K 0 = PHt 0 / E;float K_1 = PHt_1 / E;1/* Update covariance matrix:* (equation 21-3)* Let*/float Y 0 = PHt 0; /也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值AngleAn gle=A ngle+(Gyro - Q_bias) * dt;其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角

16、度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。float dt=0.005;这是程序中的定义同时Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即h 0 * P00*/* Xnew = X + K * error* err is a measureme nt of the differe nee in the measured state* and the estimate state. In our ease, it is just the differe nee* betwee n the in eli no meter measured a

17、n gle and the estimated an gle.*/ an gle += K_0 * an gle_err;q_bias += K_1 * an gle_err; 厂 一 一http:/www.doei /现在智能小车上用的卡尔曼滤波算法。段时间的，有由于做平衡小车，然后对那段滤波算法很疑惑， 然后网上讲的又比较少，我看了 书o oooooooooo这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱(大二) 错的请多多包涵。先上程序，这是抄的不知道谁的代码。抱歉了。不过这程序好像都写的差不多void Kalman_Filter(float Gyro,float Aeeel)A

18、n gle+=(Gyro - Q_bias) * dt;Pdot0=Q_a ngle - PP01 - PP10; /Pdot1= - PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt;PP01 += Pdot1 * dt;PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;An gle_err = Accel - An gle;PCt_O = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_an gle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PCt_1 /

19、E;t_0 = PCt_0;t_1 = C_0 1“ PP01;PP00-=K_0 * t_0;PP01-=K_0 * t_1;PP10-=K_1 * t_0;PP11-=K_1 * t_1;An gle+= K_0 * An gle_err;Q_bias += K_1 * An gle_err;Gyro_x=Gyro - Q_bias;首先是卡尔曼滤波的 5个方程X(k|k-1)=A X(k-1|k-1)+B U(k).先验估计P(k|k-1)=A P(k-1|k-1) A Q -(2)/协方差矩阵的预测Kg(k)= P(k|k-1) H (H P(k|k-1) H+ R) (3)/计算卡尔

20、曼增益X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1) 通过卡尔曼增益进行修正P(k|k)= (I-Kg(k) H ) P(k|k-1) (5)/跟新协方差阵5个式子比较抽象，现在直接用实例来说,对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀 螺仪测得的角加速度值乘以时间，因为- dt - ，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的)，静态漂移就是静止了没有角速度然后陀螺仪Q_bias=Q_bias将两个式子写成矩阵的形式An gleQ_bias_dt1An gleQ biasdtGyro0得到上式

21、，这个式子对应于卡尔曼滤波的第一个式子X(k|k-1)=A X(k-1|k-1)+B U(k) 先验估计X(k|k-1)为2维列向量An gleQ_biasAn gleQ_bias，B为2维列向量A为2维方阵dt0 U(k)为 Gyro-dt1，X(k-1|k-1)为2维列向量，这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值， 一个是漂移的噪声， 一个是角度值的噪声，(所谓噪声就是数据的方差值)P(k|k-1)=A P(k-1|k-1) A Q这里的Q为向量An glecov(A ngle,A ngle)cov(Q_bias,A ngle)Q_bias的协方差矩阵，

22、即cov(A ngle,Q_bias)cov(Q_bias) |cov(A ngle,Q_bias) =Q 因为漂移噪声还有角度噪声是相互独立的，则cov(Q_bias,A ngle) =Q又由性质可知cov (x，x)=D (x)即方差，所以得到的矩阵如下D(A ngle)0D(Q_bias)，这里的两个方差值是开始就给出的常数程序中的定义如下 float Q_an gle=0.001;float Q_gyro=0.003;其中P(k-1|k-1)设为，第一式已知A为dt1接着是这一部分 A P(k-1|k-1) A ,其中的(P( k-1)|P(k-1)为上一时刻的预测方差阵卡尔曼滤波的目

23、标就是要让这个预测方差阵最小。则计算A P(k-1|k-1) A Q (就是个矩阵乘法和加法，算算吧)结果如下2a _c汇dt b汇dt+d.(dt) +D(Angle)bd 汽dtc d xdtdd .(dt) $很小为了计算简便忽略不计。于是得到ac汇 dtb 汇 dt+D(Angle)bd 汉 dtc -d xdtda,b,c,d 分别和矩阵的 P00,P01,P10,P11计算过程转化为如下程序，代换即可Pdot0=Q_angle - PP01 - PP10;Pdot1= - PP11;Pdot2= - PP11;Pdot3=Q_gyro;PPOO += Pdot0 * dt;PP01 += Pdot1 * dt;PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;三，这里是卡尔曼滤波的第三个式子(3)/计算卡尔曼增益Kg(k)= P(k|k-1) H (H P(k|k-1) H + R)即计算卡尔曼增益，这是个二维向量设为0ki，这里的P(K|K-1)+R，这里又有一个常数 R，程序