卡尔曼滤波算法的程序实现和推导过程.docVIP

卡尔曼滤波算法的程序实现和推导过程 ---蒋海林（QQ：280586940）--- 卡尔曼滤波算法由匈牙利裔美国数学家鲁道夫·卡尔曼（Rudolf Emil Kalman）创立，这个数学家特么牛逼，1930年出生，现在还能走能跳，吃啥啥麻麻香，但他的卡尔曼滤波算法已经广泛应用在航空航天，导弹发射，卫星在轨运行等很多高大上的应用中。 让我们一边膜拜一边上菜吧，下面就是卡尔曼滤波算法的经典程序， 说是经典，因为能正常运行的程序都长得差不多，在此向原作者致敬。看得懂的，帮我纠正文中的错误；不太懂的，也不要急，让我慢慢道来。最后希望广大朋友转载时，能够保留我的联系方式，一则方便后续讨论共同进步，二则支持奉献支持正能量。 void Kalman_Filter(float Gyro,float Accel)///角速度，加速度 { ///陀螺仪积分角度（先验估计） Angle_Final = Angle_Final + (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_Final; ///卡尔曼增益计算 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_Final += K_0 * Angle_err; ///后验估计最优角度值 Q_bias += K_1 * Angle_err; ///更新最优估计值的偏差 Gyro_Final = Gyro - Q_bias; ///更新最优角速度值 } 我们先把卡尔曼滤波的5个方程贴上来： X(k|k-1)=A X(k-1|k-1)+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个方程比较抽象，下面我们就来把这5个方程和上面的程序对应起来。 —，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角速度乘以时间，因为，角度微分等于时间的微分乘以角速度。 但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值Angle_Final: Angle_Final = Angle_Final + (Gyro - Q_bias) * dt; 其中等号左边Angle_Final为此时的角度，等号右边Angle_Final为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。 #define dt 0.01 这是程序中的定义，同时零漂Q_bias也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的即 Q_bias=Q_bias 将两个式子写成矩阵的形式 这个式子对应于卡尔曼滤波的第一个式子 X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)//先验估计 X(k|k-1)为2维列向量，A为2维方阵，X(k-1|k