px4位置估计-inav 算法解读

技术交流：[e-mail]
转载标明出处，欢迎转载，因为都是自己的想法，不一定都是对的，欢迎讨论，哪有问题欢迎指点。

之前的问题是控制模式无法切换，现在可以切换，现在的问题是效果不好，定点的飘移范围有点大，推开之后无法回到原点。

经过分析，原因1：光流的位置估计不够精确
原因2：位置控制算法中速度内环有PID，外环只有I（目前把外环加上I效果也不好，参数需要调节）

到了不得不看算法了，于是找到了包神的博客http://blog.sina.com.cn/s/blog_8fe4f2f40102wo50.html
估计高度的过程和估计x,y轴的思路是一模一样的，位置用的是光流和加速度计的数据。整个过程包神解读的很清楚。
还有陈哥的博客http://blog.csdn.net/czyv587/article/details/51884052
但是没有人详解算法吗？
包神高冷的结尾：预测函数很好理解，但是校正函数是根据什么理论推导出来的？目前尚不清楚。
没办法 我只能拿起了纸和笔
原程序的两个算法

void inertial_filter_predict(float dt, float x[2], float acc){    if (isfinite(dt))         {           if (!isfinite(acc)) {acc = 0.0f;}        x[0] += x[1] * dt + acc * dt * dt / 2.0f;        x[1] += acc * dt;    }}void inertial_filter_correct(float e, float dt, float x[2], int i, float w){    if (isfinite(e) && isfinite(w) && isfinite(dt)) {        float ewdt = e * w * dt;        x[i] += ewdt;        if (i == 0) {            x[1] += w * ewdt;        }    }}

原来是互补滤波，为什么是互补滤波？
先看珞石的博客http://blog.csdn.net/luoshi006/article/details/51513580
http://blog.csdn.net/luoshi006/article/details/51459884

姿态控制里的互补滤波大家比较清楚了，来类比一下

在姿态里，加速度计用重力加速度作为标准，通过PI来去除误差，因为加速度计的干扰是高频的，所以PI的作用相当于是低通滤波器。
在位置估计里，用的就是这个处理好的加速度，来得到速度。
在看位置估计的，第二的函数，用光流数据得到速度，
第一句微分（float ewdt = e * w * dt）;就是高通滤波，
为什么呢？大家参考电路里微分电路，就是高通滤波器。
然后第二句（x_set[1] += ewdt）;把两个速度加起来，得到最好用于估算位置的最终速度，整个过程是不是就是，和姿态的互补滤波一毛一样.
参考一下姿态里的陀螺仪（姿态估计程序里）

因为家中有事，在家休息了一个月，期间收到不少邮件催更，说明写点东西还是有用的，利人利己，感谢大家的肯定，一个月之后写这个博客，感觉很多都忘了，以后会计及时写下来。感谢催更同学们的鼓励