[算法]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）;把两个速度加起来，得到最好用于估算位置的最终速度，整个过程是不是就是，和姿态的互补滤波一毛一样.
参考一下姿态里的陀螺仪（姿态估计程序里）

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

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

Xamarin 2017.10.9更新
Xamarin 2017.10.9更新本次更新主要解决了一些bug.Visual Studio 2017升级到15.4获得新功能.Visual Studio 2015需要工具-选项-Xamarin ...
2017.10.26 社团活动总结
这次到的人二十以内,感谢其他部门负责人捧场.上课的人是高二(15)班的罗颖,他还是要好好锻炼一下的下面总结课程内容: 想偷个懒,就讲下button控件的用法,其他看bilibili视频 1.下载安装 ...
matlab 模拟交易,股票量化投资策略模拟交易系统 [2017.03.26 更新]
本帖最后由 heawjc 于 2017-3-27 17:13 编辑本系统允许自主研发量化投资策略并进行模拟交易,从2000多只A股中自动选取并交易:策略研发完成即可对历史行情数据进行模拟调试,验证其 ...
闵梓轩大佬のnoip模拟题D1 总结 2017/10/26
背景题目概括 T1 题面分析 90分算法满分算法 T2 题面分析部分分算法满分算法满分代码 T3 题面分析代码总结背景这道题目是去年的金牌大佬闵梓轩在一年前出的一套noip模拟 ...
2017.10.26 四校联测D1
题目比以前的题不知道高明到哪里去了,虽然std有错+数据有问题 T1 地精部落原题 #include<iostream> #include<cstdio> using name ...
2017.10.26试题（NOIP DAY1难度）
T1:为爱追寻 [问题描述] 话说一年半以前,紫萱学姐展开了对杨廷学长的追求.在经历了不懈的努力之后,学姐终于成为了一名--金牌单身狗.但是这位痴情的少女并没有放弃,于是决定在保送之后继续进行这项征程 ...
2017.10.26模拟赛day1
-- T1为爱追寻问题描述历经了半年的停课之后,紫萱学姐回到了陌生又熟悉的班里,她想找到学长的位置.于是她决定采用一种高效率的寻找方法:瞎找法. 我们将学姐的班级视为一个二维平面,每个整数坐标对应 ...
2017.10.26 星际贸易思考记录
这个题非常繁琐,而且网上的题解非常不详细.. 首先第一问由于所有的点都要走到,所以就是01背包需要可持久化背包来输出方案..类似floyd的判断方式然后这些选定的点都一定是要维护的, 第二问就是 ...
【转360】KB4041678 Windows 仅安全更新(2017.10) 补丁更新后执行SQL出错! http://bbs.360.cn/thread-15201531-1-1.html...
把EXCEL20003表数据导入到MDB数据库中 sql命令语句 \"SELECT * INTO 表 FROM [Excel 8.0;DATABASE=C:\\1.xls].[Sheet1$ ...
物体抓取位姿估計算法綜述_基于深度学习的物体抓取位置估计
主讲题目:基于深度学习的物体抓取位置估计主要内容:机械臂抓取技术简介与入门方法主讲嘉宾:东北大学研究生,主要研究物体六自由度位姿估计,机械臂抓取. 知乎视频www.zhihu.com 往期干货资 ...

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

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

最新文章

热门文章