研发埠

标题: 卡尔曼滤波学习笔记(附程序加详解) [打印本页]

作者: 明德烈    时间: 2015-1-12 11:37
标题: 卡尔曼滤波学习笔记(附程序加详解)
原来的程序发现很多错误,所以删掉了不敢说自己掌握了卡尔曼算法。只是死磕卡尔曼一周后的一点心得。最近飞控比较火,卡尔曼顺带也热门起来。不过我没打算要做飞控,用它只是想提高电子称的精度(工作上要用,后来发现并不适合卡尔曼,改回原来的平滑滤波了)。要了解卡尔曼先要扯远一点,算数平均法是我们常用的滤波方式。多采几个样,相加,求平均数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 代码这个是版主提供的飞控程序的卡尔曼滤波部分,我逐条做解释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();
作者: 杨希祥    时间: 2015-1-14 13:40
希望和楼主探讨如何改进算法!




欢迎光临 研发埠 (http://bbs.yanfabu.com/) Powered by Discuz! X3.2