卡尔曼滤波算法代码总结(0311).doc

《卡尔曼滤波算法代码总结(0311).doc》由会员分享，可在线阅读，更多相关《卡尔曼滤波算法代码总结(0311).doc（9页珍藏版）》请在三一办公上搜索。

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)/协方差矩阵的预测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)/跟新协方差阵5个式子比较抽象，现在直接用实例来说，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静

20、态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值 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为向量 的协方差矩阵，即因为漂移噪声还有角度噪声是相互独立的，则=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;P

23、P00 += 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)+R，这里又有一个常数R，程序中的定义如下float R_angle=0.5;这个指的是角度测量噪声值，则式子的分母=P00+R_angle即程序中的PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E =

24、 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;对应程序如下Angle+= K_0 * Angle_err;Q_bias+= K_1 * Angle_err;同时为了PID控制还有下次的使用把角速度算出来了Gyro_x = Gyro - Q_bias;五，最后

25、一步对矩阵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)/跟预测方差阵这个很简单，矩阵带进去算就行了六，总结卡尔曼滤波一共只需要给很少的初始值量，float Q_angle=0.001; float Q_gyro=0.003;还有float R_angle=0.5;以及系统的初始量angle还有Q_bias还有预测误差矩阵P，程序里给的是0（数组）理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。滤波估值将与初始值的选取无关。但是实际上并不是如此，比如测量方差值一直在变化。