[算法]px4位置估计-inav （2017/10/26更新）

技术交流：zinghd@163.com,757012902@qq.com
转载标明出处，欢迎转载，因为都是自己的想法，不一定都是对的，欢迎讨论，哪有问题欢迎指点。

2017/10/26更新 之前写的有点小问题，赶紧修改一下
之前的问题是控制模式无法切换，现在可以切换，现在的问题是效果不好，定点的飘移范围有点大，推开之后无法回到原点。

经过分析，原因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;
        }
    }
}

预测函数其实是建立一个模型进行估计
通过用物理模型得到当前的预测值
x[0] += x[1] * dt + acc * dt * dt / 2.0f; (v=v0*t+a*t^2)
x[1] += acc * dt;(v=加速度的积分)
校正函数其实是通过测量值来校正预测值
跟卡尔曼滤波的思路一样

就是把实测值（光流传感器出来的速度）和估计值（通过公式得来的速度）比较一下，如果估计值比测量值小，那就把估计值加上他们之间的偏差作为新的估计值，当然前面要加个系数，就是我们前面说的加权系数，这个地方我要写个公式，因为很简单就能说明白。
比如我们的观查值是Z，估计值是X， 那么新的估计值就应该是 Xnew = X + K (
Z-X），从这个公式可以看到，如果X估计小了，那么新的估计值会加上一个量K ( Z-X）,
如果估计值大了,大过Z了，那么新的估计值就会减去一个量K (
Z-X），这就保证新的估计值一定比现在的准确，一次一次递归下去就会越来越准却了，当然这里面很有作用的也是这个K，也就是我们前面说的权值，书上都把他叫卡尔曼增益。。。(Xnew
= X + K ( Z-X） = X ×（1-K) + KZ ，也就是说估计值X的权值是1-k，而观察值Z的权值是k，究竟k 取多大，全看估计值和观察值以前的表现，也就是他们的方差情况了。
http://blog.csdn.net/mangzuo/article/details/71171137

函数原型
x[i] += ewdt;（预测函数带入的i=1）
x[1]+=（flow_v[0]-x_est[1]）*w(权重)*dt
x[1]=x[1]+(flow_v[0]-x_est[1])* K(系数)
x[1]得到的就是数度的估计值x_est[1]（t+1）

x_est[1]（t+1）=x_est[1]（t）+K*flow_v[0](t)-K*x_est[1](t)
x_est[1]（t+1）=(1-k)*x_est[1]（t）+K*flow_v[0](t)
当前估计值=上次的估计值*（1-k）+当前测量值*k

这个加权思想 和卡尔曼一样 (知乎的大神们经常说互补滤波是固定增益的卡尔曼滤波)
http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies

是个系数不变的加权，确实还是互补滤波的模式

（
这段之前写的不太对请乎略
原来是互补滤波，为什么是互补滤波？
先看珞石的博客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）;把两个速度加起来，得到最好用于估算位置的最终速度，整个过程是不是就是，和姿态的互补滤波一毛一样.
参考一下姿态里的陀螺仪（姿态估计程序里）

这段之前写的不太对请乎略）