新手平衡小车的卡尔曼滤波法总结新手平衡小车的卡尔曼滤波算法总结.doc

由于做平衡小车，然后对那段滤波算法很疑惑，然后网上讲的又比较少，我看了一段时间的书。。。。。。。。。。。这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱（大二），有错的请多多包涵。 先上程序，这是抄的不知道谁的代码。。。抱歉了。。不过这程序好像都写的差不多 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)=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 Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。 float dt=0.005; 这是程序中的定义 同时 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-1)为2维列向量，B 为2维列向量，U(k) 为Gyro 二，这里是卡尔曼滤波的第二个式子 接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，（所谓噪声就是数据的方差值） P(k|k-1)=A P(k-1|k-1) A’+Q 这里的Q为向量 的协方差矩阵，即 因为漂移噪声还有角度噪声是相互独立的，则=0；=0 又由性质可知cov（x，x）=D（x）即方差，所以得到的矩阵如下 ，这里的两个方差值是开始就给出的常数 程序中的定义如下float Q_angle=0.001; float Q_gyro=0.0