卡尔曼滤波算法的程序实现和推导过程

卡尔曼滤波算法的程序实现和推导过程---蒋海林（QQ：280586940）---卡尔曼滤波算法由匈牙利裔美国数学家鲁道夫·卡尔曼（RudolfEmilKalman）创立，这个数学家特么牛逼，1930年出生，现在还能走能跳，吃啥啥麻麻香，但他的卡尔曼滤波算法已经广泛应用在航空航天，导弹发射，卫星在轨运行等很多高大上的应用中。让我们一边膜拜一边上菜吧，下面就是卡尔曼滤波算法的经典程序，说是经典，因为能正常运行的程序都长得差不多，在此向原作者致敬。看得懂的，帮我纠正文中的错误；不太懂的，也不要急，让我慢慢道来。最后希望广大朋友转载时，能够保留我的联系方式，一则方便后续讨论共同进步，二则支持奉献支持正能量。voidKalman_Filter(floatGyro,floatAccel)///角速度，加速度{///陀螺仪积分角度（先验估计）Angle_Final=Angle_Final+(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_Final;///卡尔曼增益计算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_Final+=K_0*Angle_err;///后验估计最优角度值Q_bias+=K_1*Angle_err;///更新最优估计值的偏差Gyro_Final=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个方程比较抽象，下面我们就来把这5个方程和上面的程序对应起来。—，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角速度乘以时间，因为ddt，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值Angle_Final:Angle_Final=Angle_Final+(Gyro-Q_bias)*dt;其中等号左边Angle_Final为此时的角度，等号右边Angle_Final为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。#definedt0.01这是程序中的定义，同时零漂Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_bias=Q_bias将两个式子写成矩阵的形式&&1_010_ AngledtAngledtQbiasQbiasGyro这个式子对应于卡尔曼滤波的第一个式子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)dt因为漂移噪声和角度噪声是相互独立的，则cov(Angle,Q_bias)=0；cov(Q_bias,Angle)=0；又由性质可知cov（x，x）=D（x）即方差，所以得到的矩阵如下：D(Angle)00D(Q_bias)Qdt这里的两个方差值是开始就给定的常数，程序中定义如下：#defineQ_angle0.001////角度过程噪声的协方差#defineQ_gyro0.003////角速度过程噪声的协方差接着是这一部分AP(k-1|k-1)A’,其中的P（k-1|k-1)为上一时刻的预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中P(k-1|k-1)设为abcd,第一式已知A为101dt,则A’为101dt则计算AP(k-1|k-1)A’+Q（就是个矩阵乘法和加法，算算吧）结果如下:2(dt)(Angle)(_)acdtbdtdDdtbddtcddtdDQbiasdt由于2(dt)d很小为了计算简便忽略不计。于是得到(Angle)(_)acdtbdtDdtbddtcddtdDQbiasdt把整个式子完整的写出来：####(Angle)(_)(Angle)(_)acdtbdtDdtbddtabcddtdDQbiasdtcdabcdtbdtDdtddtcdddtDQbiasdt又由于abcd是####abcd的上一状态的方差阵，所以可以写成程序可以实现的形式：(_)_abQanglecbdtddtcdddtQgyrodt此过程转化为如下程序：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叫做观测模型，是用来把真实的状态映射到观测空间中。真实的状态是不能被观测到的。因为测量值只有加速度计的测量值，所以H定义如下：10H另外这里有一个常数R，程序中的定义如下#defineR_angle0.5////测量噪声的协方差（即是测量偏差）如果你把测量噪声方差设置的太高，滤波器将反应较慢，因为它会更加不相信新的测量值。但是如果设置过小，该值可能过冲，并且带来更多的噪声，因为更加相信加速计的测量值。则第三个式子可写成:000101011000111011001000(|1)()(|1)10110_0_TgTPkkHkkHPkkHRPPPPKPPPPPPPPKRanglePPPPPPPPPPRangle分母即程序中的PCt_0=C_0*PP[0][0];PCt_1=C_0*PP[1][0];分子即程序中的E=R_angle+C_0*PCt_0;于是求出01kkK_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(k|k)=（I-Kg(k)H）P(k|k-1)………(5)//更新预测方差阵I被称为单位矩阵，其定义为：1001I推导过程很简单，矩阵代进去算就行了:##&&000010001##&&110111011&&&&000010001&&&&110111011&&0&0001000&&1101110(10)01101001KPPPPPPPPKPPPPPPPPKPPPPPPPPKPPPPPPPPKPPPPPPPPKPPPP&1&&&&0001000001&&&&1011110111PPPPKPPKPPPPPPKPPKPP又由于&&0001&&1011PPPPPPPP是##0001##1011PPPPPPPP的上一状态的方差阵,所以可以写成如下比较接近程序实现方式的形式:00010000011011110111PPPPKPPKPPPPPPKPPKPP转化成如下程序: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;六，初始值卡尔曼滤波算法的推导过程已经完成了,如果再回头去看一遍程序,相信大家会有一个全新的认识。卡尔曼算法从理论上讲是迭代的算法，也是最优算法，需要给很少的初始值：#defineQ_angle0.001////角度过程噪声的协方差#defineQ_gyro0.003////角速度过程噪声的协方差#defineR_angle0.5////测量噪声的协方差（即是测量偏差）#definedt0.01////卡尔曼滤波采样频率#defineC_01voidKalman_Filter_Init(void){Q_bias=0;PP[0][0]=1;PP[0][1]=0;PP[1][0]=0;PP[1][1]=1;Angle_Final=0;Gyro_Final=0;}七，展望如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着测量值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。针对卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。