由于做平衡小车，然后对那段滤波算法很疑惑，然后网上讲的又比较少，我看了一段时间的书。。。。。。。。。。。这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱（大二），有错的请多多包涵。先上程序，这是抄的不知道谁的代码。。。抱歉了。。不过这程序好像都写的差不多voidKalma
_FilterfloatGyrofloatAccelA
gleGyroQ_biasdtPdot0Q_a
glePP01PP10Pdot1PP11Pdot2PP11Pdot3Q_gyroPP00Pdot0dtPP01Pdot1dtPP10Pdot2dtPP11Pdot3dtA
gle_errAccelA
glePCt_0C_0PP00PCt_1C_0PP10ER_a
gleC_0PCt_0K_0PCt_0EK_1PCt_1Et_0PCt_0t_1C_0PP01PP00K_0t_0PP01K_0t_1PP10K_1t_0PP11K_1t_1A
gleK_0A
gle_errQ_biasK_1A
gle_errGyro_xGyroQ_bias首先是卡尔曼滤波的5个方程Xkk1AXk1k1BUk………1先验估计Pkk1APk1k1A’Q………2协方差矩阵的预测
fKgkPkk1H’HPkk1H’R………3计算卡尔曼增益XkkXkk1KgkZkHXkk1………4通过卡尔曼增益进行修正Pkk（IKgkH）Pkk1………5跟新协方差阵5个式子比较抽象，现在直接用实例来说，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为ddt，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值A
gleA
gleA
gleGyroQ_biasdt其中等号左边A
gle为此时的角度，等号右边A
gle为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。floatdt0005这是程序中的定义同时Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_biasQ_bias将两个式子写成矩阵的形式
A
gleQ_bias

1dtA
gle01Q_bias

dt0
Gyro
得到上式，这个式子对应于卡尔曼滤波的第一个式子Xkk1AXk1k1BUk………1先验估计Xkk1为2维列向量
A
gle1dt，A为2维方阵，Xk1k1为2维列向量Q_bias01dtUk为Gyro0，
A
gleQ_bias
，B为2维列向量
二，这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，（所谓噪声就是数据r