卡尔曼滤波算法平衡小车

先上程序，这是抄的不知道谁的代码。。。抱歉了。。不过这程序好像都写的差不多voidKalman_Filter(floatGyro,floatAccel){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)=AX(k-1|k-1)+BU(k)………..(1)//先验估计P(k|k-1)=AP(k-1|k-1)A’+Q………(2)//协方差矩阵的预测Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(3)//计算卡尔曼增益X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(4)通过卡尔曼增益进行修正P(k|k)=（I-Kg(k)H）P(k|k-1)………(5)//跟新协方差阵5个式子比较抽象，现在直接用实例来说—，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为ddt，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值AngleAngle=Angle+(Gyro-Q_bias)*dt;其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。floatdt=0.005;这是程序中的定义同时Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_bias=Q_bias将两个式子写成矩阵的形式1_01_0 AngledtAngledtQbiasQbiaosGyr得到上式，这个式子对应于卡尔曼滤波的第一个式子X(k|k-1)=AX(k-1|k-1)+BU(k)………..(1)//先验估计X(k|k-1)为2维列向量_AngleQbias，A为2维方阵101dt，X(k-1|k-1)为2维列向量_AngleQbias，B为2维列向量0dt，U(k)为Gyro二，这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，（所谓噪声就是数据的方差值）P(k|k-1)=AP(k-1|k-1)A’+Q这里的Q为向量_AngleQbias的协方差矩阵，即cov(Angle,Angle)cov(Q_bias,Angle)cov(Angle,Q_bias)cov(Q_bias)因为漂移噪声还有角度噪声是相互独立的，则cov(Angle,Q_bias)=0；cov(Q_bias,Angle)=0又由性质可知cov（x，x）=D（x）即方差，所以得到的矩阵如下D(Angle)00D(Q_bias)，这里的两个方差值是开始就给出的常数程序中的定义如下floatQ_angle=0.001;floatQ_gyro=0.003;接着是这一部分AP(k-1|k-1)A’,其中的（P（k-1）|P(k-1)）为上一时刻的预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中P(k-1|k-1)设为abcd,第一式已知A为101dt则计算AP(k-1|k-1)A’+Q（就是个矩阵乘法和加法，算算吧）结果如下2.(dt)(Angle)acdtbdtdDbddtcddtd2.(dt)d很小为了计算简便忽略不计。于是得到(Angle)acdtbdtDbddtcddtda,b,c,d分别和矩阵的P[0][0],P[0][1],P[1][0],P[1][1]计算过程转化为如下程序，代换即可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;三，这里是卡尔曼滤波的第三个式子Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(3)//计算卡尔曼增益即计算卡尔曼增益，这是个二维向量设为01kk，这里的H=10为由此kg=P(K|K-1)+R，这里又有一个常数R，程序中的定义如下floatR_angle=0.5;这个指的是角度测量噪声值，则式子的分母=P[0][0]+R_angle即程序中的PCt_0=C_0*PP[0][0];PCt_1=C_0*PP[1][0];E=R_angle+C_0*PCt_0;分子[0][0][1][0]PP于是求出01KKK_0=PCt_0/E;K_1=PCt_1/E;四，用误差还有卡尔曼增益来修正X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(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;五，最后一步对矩阵P进行更新，因为下一次滤波时要用到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;P(k|k)=（I-Kg(k)H）P(k|k-1)………(5)//跟预测方差阵这个很简单，矩阵带进去算就行了六，总结卡尔曼滤波一共只需要给很少的初始值量，floatQ_angle=0.001;floatQ_gyro=0.003;还有floatR_angle=0.5;以及系统的初始量angle还有Q_bias还有预测误差矩阵P，程序里给的是0（数组）理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。滤波估值将与初始值的选取无关。但是实际上并不是如此，比如测量方差值一直在变化。