卡尔曼滤波算法总结

1、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;PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0

2、;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 个方程：1）先验估计2）协方差矩阵的预测X(k |k 1) AX(k 1|k 1) Bu(k)P(k |k 1) AP(k 1|k 1)A Q(3) 计算卡尔曼增益

3、(4) 进行修正(5) 更新协方差阵Kg(k) P(k|k 1)H/HP(k|k 1)H R)X(k|k) X(k|k 1) Kg (k)(Z(k) HX (k|k 1)P(k|k) (I Kg(k) H) P(k |k 1)5个式子比较抽象，现在直接用实例来说:一、卡尔曼滤波第一个式子对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上 上一时刻陀螺仪测得的角加速度值乘以时间，因为 d dt ，角度微分等于时 间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的)，静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的， 计算时要把它减去。由此我们得

4、到了当前角度的预测值 An gleAn gle=A ngle+(Gyro - Q_bias) * dt;其中等号左边Angle为此时的角度，等号右边 Angle为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔，我们的运行周期是 4ms或者6ms同时Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的，即Q_bias=Q_bias将上面两个式子写成矩阵的形式An gledt AngleQ_bias1 Q_biasdtGyro0得到上式，这个式子对应于卡尔曼滤波的第一个式子X(k |k 1) AX(k 1| k 1) Bu(k)X k|k 1

5、为2维列向量列向量AngleQ_biasAn gle Q_biasB为2维列向量卡尔曼滤波第二个式子，A为2维方阵dt0u k 为 GyrodtX k-1|k 1 为 2 维接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声,一个是角度值的噪声，(所谓噪声就是数据的方差值)P(k|k 1) AP(k 1| k 1)A Q这里的Q为向量An gle的协方差矩阵，即Q_biascov(Angle,Angle) cov(Q_bias,Angle) cov(A ngle,Q_bias)cov(Q_bias,Q_biaS)因为漂移噪声和角度噪声是相互独立的，则cov(A ngle,Q_bi

6、as) 0 又由性质可知cov(x,x) D(x)即方差，所以得到的矩阵如下D(A ngle)00D(Q_bias)，这里的两个方差值是开始就给出的常数程序中的定义如下float Q_an gle=;float Q_gyro=;接着是这一部分 A P(k-1|k- 1) A ,其中的(P( k-1)|P(k-1)为上一时刻的 预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中 P(k-1|k-1)设为1式已知A为0dt1则计算A P(k-1|k- 1) A +Q (就是个矩阵乘法和加法，算算吧)结果如下D(Angle) b d dtd2a c dt b dt d.(dt)c d dt2

7、d.(dt)很小为了计算简便忽略不计。于是得到a c dt b dt D(Angle) b d dtc d dtda,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)k

8、o：H 1k1P00P10KoKi注意这个是加速度计算出来的角度An gle_err = Accel - An gle;对应程序如下An gle += K_0 * An gle_err;Q_bias += K_1 * An gle_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)/跟预测方差阵这个很简单，矩阵带进去算就行了六，总结卡尔曼滤波一共只需要给很少的初始值量，float Q_an gle=;float Q_gyro=;还有