《新手平衡小车的卡尔曼滤波算法总结》由会员分享，可在线阅读，更多相关《新手平衡小车的卡尔曼滤波算法总结（5页珍藏版）》请在金锄头文库上搜索。
1、由于做平衡小车，然后对那段滤波算法很疑惑，然后网上讲的又比较少，我看了一段时间 的书。 。 。 。 。 。 。 。 。 。 。这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱（大二） ， 有错的请多多包涵。 先上程序，这是抄的不知道谁的代码。 。 。抱歉了。 。不过这程序好像都写的差不多 void Kalman_Filter(float Gyro,float Accel)/角速度，加速度 /验估计 Angle+=(Gyro - Q_bias) * dt; /上一刻角度加上（角速度-误差）*时间 /协方差矩阵的预测 Pdot0=Q_angle - PP01 - PP10; / Pdot1=
2、 - PP11; Pdot2= - PP11;/ Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt; PP11 += Pdot3 * dt; / 通过卡尔曼增益进行修整 Angle_err = Accel - Angle;PCt_0 = C_0 * PP00; PCt_1 = C_0 * PP10;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 * PP01; /更新协方差阵 PP00
3、-= K_0 * t_0; PP01 -= K_0 * t_1; PP10 -= K_1 * t_0; PP11 -= 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(
4、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 个方程方程 1.验估计 2.协方差矩阵预测 3.计算卡尔曼增益 4.通过卡尔曼增益进行修整 5.根新协方差阵5 个式子比较抽象，现在直接用实例来说 ，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为，角度微分等于时间的微分乘以角速ddt 度。但是陀螺仪有个静态漂移（而且还是变化的） ，静态漂移就是静止了没有角速度然后陀螺仪 也会输出一个值，这个值肯定
5、是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值 AngleAngle=Angle+(Gyro - Q_bias) * dt; 其中等号左边 Angle 为此时的角度，等号右边 Angle 为上一时刻的角度，Gyro 为陀螺仪测 的角速度的值，dt 是两次滤波之间的时间间隔。 float dt=0.005; 这是程序中的定义 同时 Q_bias 也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即 Q_bias=Q_bias 将两个式子写成矩阵的形式1 _01_0Angledt Angledt QbiasQbiaosGyr得到上式，这个式子对应于卡尔曼滤波的第一个
6、式子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 维列向量_AngleQbias101dt，B 为 2 维列向量 ，U(k) 为 Gyro_AngleQbias0dt二，这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值 的噪声， （所谓噪声就是数据的方差值数据的方差值） P(k|k-1)=A P(k-1|k-1) A+Q 这里的 Q 为向量的协方差矩阵，即_AngleQbiascov(Angle,Angle)cov(Q_bi
7、as,Angle)cov(Angle,Q_bias)cov(Q_bias)因为漂移噪声还有角度噪声是相互独立的，则=0；cov(Angle,Q_bias)=0cov(Q_bias,Angle)又由性质可知 cov（x，x）=D（x）即方差，所以得到的矩阵如下，这里的两个方差值是开始就给出的常数D(Angle)00D(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
8、-1)设为,第一式已知 A 为abcd101dt则计算 A P(k-1|k-1) A+Q（就是个矩阵乘法和加法，算算吧）结果如下2.(dt)(Angle)acdtb dtdDbddt cddtd 很小为了计算简便忽略不计。2.(dt)d于是得到(Angle)acdtb dtDbddtcddtd a,b,c,d 分别和矩阵的 P00,P01,P10,P11 计算过程转化为如下程序，代换即可Pdot0=Q_angle - PP01 - PP10; Pdot1= - PP11; Pdot2= - PP11;/ Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot
9、1 * dt; PP10 += Pdot2 * dt; PP11 += Pdot3 * dt; 三，这里是卡尔曼滤波的第三个式子Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (3)/计算卡尔曼增益即计算卡尔曼增益，这是个二维向量设为，这里的 = 为由此 kg=01kkH10P(K|K-1)+R，这里又有一个常数 R，程序中的定义如下float R_angle=0.5; 这个指的是角度测量噪声值，则式子的分母=P00+R_angle 即程序中的PCt_0 = C_0 * PP00; PCt_1 = C_0 * PP10; E = R_angle + C_0 * P
10、Ct_0;分子0010PP于是求出01KKK_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)通过卡尔曼增益进行修正这个矩阵带进去就行了 Z（k)=Accel.注意这个是加速度计算出来的角度Angle_err = Accel - Angle; 对应程序如下Angle+= K_0 * Angle_err; Q_bias+= K_1 * Angle_err;同时为了 PID 控制还有下次的使用把角速度算出来了Gyro_x = Gyro - Q_bias;五，最后一步
11、对矩阵 P 进行更新，因为下一次滤波时要用到PP00 -= K_0 * t_0; PP01 -= K_0 * t_1; PP10 -= K_1 * t_0; PP11 -= 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（数组） 理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。滤波估值将与初始值的选取无 关。 但是实际上并不是如此，比如测量方差值一直在变化。其实卡尔曼的程序相当的简单，其实卡尔曼的程序相当的简单， 只要你理解了他的那只要你理解了他的那 5 条公式。条公式。
