卡尔曼滤波算法总结.pdf

2015.12.12 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 = Accel - 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 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的 5 个方程： X (k | k 1) AX ( k 1| k 1) Bu( k) （1）先验估计 P( k | k 1) AP( k 1| k 1)A Q （2 ）协方差矩阵的预测 Kg( k) P(k | k 1)H / HP(k | k 1)H R) （3 ）计算卡尔曼增益 X (k | k) X (k | k 1) Kg (k )(Z (k ) HX ( k | k 1)) （4 ）进行修正 P( k | k ) （I Kg（k）H ）P(k | k 1) （5 ）更新协方差阵 5 个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说， 我们认为此时的角度可以近似认为是上一时刻的角度值加上 上一时刻陀螺仪测得的角加速度值乘以时间，因为 d dt ，角度微分等于时 间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的） ，静态漂移 就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的， 计算时要把它减去。 由此我们得到了当前角度的预测值 Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边 Angle 为此时的角度，等号右边 Angle 为上一时刻的角度， Gyro 为陀螺仪测的角速度的值， dt 是两次滤波之间的时间间隔，我们的运行周期是 4ms 或者 6ms。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 Angle 1 dt Angle dt Gyro Q _ bias 0 1 Q _ bias 0 得到上式，这个式子对应于卡尔曼滤波的第一个式子 X (k | k 1) AX ( k 1| k 1) Bu(k) Angle