卡尔曼滤波学习笔记（附程序加详解）

原来的程序发现很多错误，所以删掉了不敢说自己掌握了卡尔曼算法。只是死磕卡尔曼一周后的一点心得。最近飞控比较火，卡尔曼顺带也热门起来。不过我没打算要做飞控，用它只是想提高电子称的精度（工作上要用,后来发现并不适合卡尔曼，改回原来的平滑滤波了）。要了解卡尔曼先要扯远一点，算数平均法是我们常用的滤波方式。多采几个样，相加，求平均数x=（a0+a1+a2）/3在这个基础上做一些改进，比如，最近采的样比较重要权重比较高x=a0*0.1+a1*0.3+a2*0.6接着呢又觉得0.1，0.3，0.6系数是固定的，比较蠢，比较木头，能不能根据实际情况自动改变于是就有了自适应滤波牛人卡尔曼（还活着现在）发明了一种“简单的”自适应滤波方法“卡尔曼滤波”卡尔曼算法有两个重点，1，是“预测”，举个例子：飞控里面，我们可以用重力加速传感器测出“现在”相对与地面的角度（重力的方向）。然后就可以“预测” 1秒后 的角度为：“一秒后角度”=“现在角度”+1秒*“旋转角速度”也就是大家说的“数据融合”，如果出现多个数据融合，用矩阵就会更直观一点。幸运的是飞控不是太复杂，不用死啃矩阵。2，“p“值更新这个现在还没搞的很清楚，就不在这里卖了

卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”，然后再用“预测值”和“实际测量值”算出最终结果。有了最终结果后，还要把“最终结果”和“预测”值做比较，衡量一下是不是“最终结果”是不是靠谱。如果相差比较小，说明“预测”是比较正确的，下次计算的时候就提高“预测值”的权重，反之就提高“实际测量值”的权重。废话不多说，应版主要求，对卡尔曼滤波的程序做详细的解析ARDUINO 代码

• float xz=0;
• float p_xz=1;
• float q_xz=0.0025;
• float k_xz=0;
• float r_xz=0.25;
• float t=0.025;
• void calculate_xz()
• {
• xz=xz+t*gyro[1];
• p_xz=p_xz+q_xz;
• k_xz=p_xz/(p_xz+r_xz);
• xz=xz+k_xz*(Axz-xz);
• p_xz=(1-k_xz)*p_xz;
• }

这个是版主提供的飞控程序的卡尔曼滤波部分，我逐条做解释1， xz=xz+t*gyro[1];第一条是“预测”xz（这次x轴的角度“预测”）=xz（上次x轴的角度）+t（时间）*gyro[1](x轴旋转角速度);2，p_xz=p_xz+q_xz;3， k_xz=p_xz/(p_xz+r_xz);第二条是防止程序飞掉，因为第三条用到除法，如果除数为0，程序就当机了，所以人为的加了个q_xz(0.0025)。不过我觉得，换成【 if(p_xz==0) {return xz}】会好点，不会人为的降低精度3， k_xz=p_xz/(p_xz+r_xz);第三条是卡尔曼系数K计算这个问题比较核心，有必要认真解释一下卡尔曼系数K示意图    06.jpg (46.34 KB, 下载次数: 20)   示意图   

                               
登录/注册后可看大图

     那么k怎么确定呢？这就要解释另一个概念“P”。p就是“计算”（不是预测）和“测量”的偏差。打个比方：我们用卡尔曼算法算出来的是5，实际测量值是7，那么p=7-5=2；p就是“上一次计算值“和“上一次测量值”的偏差。如果上一次p比较小，说明我们预测的比较准，k就比较小，结果就偏向预测值。准确的程序应该是3, k_xz=sqrt(p_xz*p_xz/(p_xz*p_xz+r_xz*r_xz));//均方差但是为了减小计算压力，近似简化为3， k_xz=p_xz/(p_xz+r_xz);//我觉得应该加一个abs（）；

希望和楼主探讨如何改进算法！