卡尔曼滤波算法总结

《卡尔曼滤波算法总结》由会员分享，可在线阅读，更多相关《卡尔曼滤波算法总结（6页珍藏版）》请在装配图网上搜索。

1、void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Acc

2、el - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; 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 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1

3、 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程: (1) 先验估计 (2) 协方差矩阵的预测 (3) 计算卡尔曼增益 (4) 进行修正 (5) 更新协方差阵 X (k I k -1) = AX (k -11 k -1) + Bu(k) P(k I k -1) = AP(k - 1I k - 1)A'+ Q Kg (k) = P(k I k - 1)H 7 HP(k I k - 1)H'+ R) X (k I k) = X (k I k -1) + Kg (k)(Z(k) - HX (k I k -1)) P(

4、k I k) = (I - Kg(k) H) P(k I k -1) 5个式子比较抽象，现在直接用实例来说: 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上 上一时刻陀螺仪测得的角加速度值乘以时间，因为dO =dtx①，角度微分等于时 间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的)，静态漂移 就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的， 计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle二Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle为此时的角

5、度，等号右边Angle为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔，我们的运行周期是 4ms 或者 6ms。 同时Q_bias也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 Angle Q _ bias -dt Angle Q _ bias dt 0 Gyro 得到上式，这个式子对应于卡尔曼滤波的第一个式子 X (k I k -1) = AX (k - 1I k -1) + Bu (k) X (kI k-1)为2维列向量Angle ，入

6、为2维方阵1 一出，x (k-1I k-1)为2维 (k ) 为 Gyro Q _ bias 0 1 列向量Angle Q _ bias B为2维列向量dt 0 ， 二、卡尔曼滤波第二个式子 接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声， 一个是角度值的噪声，(所谓噪声就是数据的方差值) P(k I k -1) = AP(k -11 k - 1)A'+ Q 这里的q为向量岫 的协方差矩阵，即cov(Angie，Angie)cov(Q-bias，Angie， Q - bias cov(Angle,Q-bias)cov(Q_bias,Q_bias) 因

7、为漂移噪声和角度噪声是相互独立的，则cov(Angle,Q_bias) = 0。 又由性质可知cov(x, x) = D(x)即方差，所以得到的矩阵如下 D(Angle) 0 0 D(Q-bias)，这里的两个方差值是开始就给出的常数 程序中的定义如下 float Q_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为0， 则计算A P(k-1|k-1) A’

8、+Q (就是个矩阵乘法和加法，算算吧)结果如下 a 一 c x dt - b x dt + d.(dt)2 + D(Angle) b 一 d x dt c - d x dt d d .(dt)2很小为了计算简便忽略不计。 于是得到 a - c x dt - b x dt + D (Angle) b - d x dt c - d x dt d a,b,c,d 分别和矩阵的 P[0][0],P[0][1],P[1][0],P[1][1] 计算过程转化为如下程序，代换即可 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1]

9、[1]; Pdot[2]= - PP[1][1];/ Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; 三，这里是卡尔曼滤波的第三个式子 Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (3)//计算卡尔曼增益 即计算卡尔曼增益，这是个二维向量设为k H = |1 0|为由此kg 气，这里的 P(K|K-1)+R，这里又有一个常数R，程序中的定义如

10、下 float R_angle=0.5; 这个指的是角度测量噪声值，则式子的分母=P[0][0]+R_angle 即程序中的 PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; 分子 P[0][0] P[1][0] 于是求出 K 0 K 1 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)通过卡尔曼增益进

11、 行修正 这个矩阵带进去就行了 Z(k)二Accel.....注意这个是加速度计算出来的角度 Angle_err = Accel - Angle; 对应程序如下 Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; 同时为了 PID控制还有下次的使用把角速度算出来了 Gyro_x = Gyro - Q_bias; 五，最后一步对矩阵P进行更新，因为下一次滤波时要用到 PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= 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 (数组) 理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。滤波估值将与初始值 的选取无关。 但是实际上并不是如此，比如测量方差值一直在变化。 精品