平衡小车的卡尔曼滤波算法

1、角速度，加速度void Kalman_Filter(float Gyro,float Accel) (验估计上一刻角度加上（角速度-误差）*时间Angle+=(Gyro - Q_bias) * dt;/协方差矩阵的预测Pdot0=Q_angle - PP1 - PP10; /Pdot1= - PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt;PP01 += Pdot1 * dt;PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;/通过卡尔曼增益进行修整Angle_err = Accel - Angle;PCt_0

2、= 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 -= 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个方程.(1)/先验估计X(k|k

3、-1)=A X(k-1|k-1)+B U(k)P(k|k-1)=A P(k-1|k-1) A'+Q(2)/协方差矩阵的预测(3)计算卡尔曼增益Kg(k)= P(k|k-1) H ' / (H P(k|k-1) H ' + R)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个方程1 .验估计2 .协方差矩阵预测3 .计算卡尔曼增益=4 .通过卡尔曼增益进行修整5 .根新协方差阵 5个式子比较抽象，现在直接用实例来说

4、对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为dQ =dt黑色，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的)，静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值AngleAngle=Angle+(Gyro - Q_bias) * dt;其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。float dt=0.005;这是程序中的定义同时Q_bia

5、s也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_bias=Q_bias将两个式子写成矩阵的形式Angle1Q bias0Gyro-dt Angle | + dt1 Q bias 0得到上式，这个式子对应于卡尔曼滤波的第一个式子X(k|k-1)=A X(k-1|k-1)+B U(k) . (1)/ 先验估计X(k|k-1)为2维列向量Angle , A为2维方阵1 出，X(k-1|k-1)为2维列向量Q bias01AngleQ biasB为2维列向量dt U(k) 0 ,为 Gyro,这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移

6、的噪声，一个是角度值的噪声，(所谓噪声就是 数据的方差值)cov(Angle,Angle) cov(Q_bias,Angle)cov(Angle,Q_bias) cov(Q_bias)P(k|k-1)=A P(k-1|k-1) A '+Q | 这里的Q为向量Angle Q biascov(Angle,Q_bias) =0因为漂移噪声还有角度噪声是相互独立的，则cov(Q_bias,Angle)=。又由性质可知cov (x, x) =D (x)即方差，所以得到的矩阵如下D(Angle) 00 D(Q_bias),这里的两个方差值是开始就给出的常数程序中的定义如下float Q_angle

7、=0.001;float Q_gyro=0.003;接着是这一部分 A P(k-1|k-1) A '，其中的(P (k-1) |P(k-1)为上一时刻的预测方差阵 卡尔曼滤波的目标就是要让这个预测方差阵最小。其中P(k-1|k-1)设为a b，第一式已知 A为1 dt c d0 1则计算A P(k-1|k-1) A '+Q (就是个矩阵乘法和加法，算算吧)结果如下 .2 . .a -cxdt -bxdt +d.(dt) +D(Angle) b-d xdtc -d xdtd2d.(dt)很小为了计算简便忽略不计。于是得到Ia -cx dt -b xdt +D(Angle) b-d

8、dt c -d xdtda,b,c,d 分别和矩阵的 P00,P01,P10,P11计算过程转化为如下程序，代换即可Pdot0=Q_angle - PP01 - PP10;Pdot1= - PP11;Pdot2= - PP11;/Pdot3=Q_gyro;PP00 += Pdot0 * dt;PP01 += Pdot1 * dt;PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;(3)/计算卡尔曼增益三，这里是卡尔曼滤波的第三个式子Kg(k)= P(k|k-1) H ' / (H P(k|k-1) H ' + R)即计算卡尔曼增益，这是个二维向量设为k

9、0k1H =11 0为由此kg=P(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 * PCt_0;P00分子P10K0于是求出K1K_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尸

10、Accel.注意这个是加速度计算出来的角度Angle_err = Accel - Angle;对应程序如下Angle += K_0 * Angle_err;Q_bias += K_1 * Angle_err;同时为了 PID控制还有下次的使用把角速度算出来了Gyro_x = Gyro - Q_bias;五，最后一步对矩阵 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.00