找回密码
 立即注册

QQ登录

只需一步,快速开始

扫一扫,访问微社区

查看: 4965|回复: 169

超详细讲解:罗盘和加速度计校正方法(附C源代码)

  [复制链接]
发表于 2016-5-25 12:58:14 | 显示全部楼层 |阅读模式

加入社区,轻松玩转无人机!

您需要 登录 才可以下载或查看,没有帐号?立即注册

x
本帖最后由 zoujiachi 于 2016-7-14 17:46 编辑

请尊重作者劳动成果,转载请注明出处,谢谢!
作者:Elor
交流QQ:529380360


1.为什么要校正

       我们都知道,罗盘是测量周围的磁场强度,若不存在外在磁场的干扰,只存在地磁的话,理论上罗盘旋转测得的磁场是一个圆球。
可是现实空间中,除了地磁场外,还存在其他的磁场干扰,这里我索性将它分为两大类。


第一类:地球空间中的磁场,这类磁场有个特点,就是随着罗盘坐标系的转动,磁场方向不变,类似地磁场。
第二类:罗盘坐标空间中的磁场,这类磁场源一般是固定在飞机上的,所以随着罗盘坐标系的转动,磁场方向也跟着转动。但是对于罗盘坐标系来说,却是一个恒定值。

       对于第一类的磁场,目前我了解到的还没有什么好的方式可以进行校正(如果哪位大神知道,还请告知)。而我后面要介绍的校正方法,即是滤除第二类磁场的干扰,校正的思想即是基于最小二乘法的椭球拟合算法。

注:这个只能校正磁场强度固定不变的磁场,而对于电机这种变化的磁场,我没有测验过,不知道电机产生磁场的强度大小跟电机转速的关系怎样,如果谁有研究过的,还请告知,谢谢

2.椭球拟合校正理论推导
       网络上有许多关于椭球拟合校正的论文,我都没有细看,因为那些公式都写得晦涩难懂,没有那个耐心,我这里尽量用最简洁的语言介绍校正方法的理论基础。


首先建立数值模型,设测量值为: 1.PNG
,校正后的值为: 2.PNG
,平移参数为: 3.PNG
,缩放参数为:
4.PNG

他们之间的关系如下所示:
5.PNG



我们校正后的目标就是使得校正值近似分布在一个圆球上,而圆球的公式大家都知道:x2+y2+z2=R2,故我们将校正后的值带入圆球公式,与理论的圆球半径平方做差,构建误差u:
6.PNG



将校正值用测量值替换,变为:
7.PNG

可以看到,这分明就是个椭球公式嘛~


记:
8.PNG



则我们的误差u可以写为如下形式:
9.PNG



       下面就是校正的核心思想了:假设我们有许多组数据,我们要求得一组参数,使得所有数据的误差和最小,即 11.PNG
最小,但是由于u有正有负,所以符号相反的误差有可能相互抵消。那么加绝对值呢?这个也不可取,因为对绝对值函数求极小值十分复杂。那么我们自然就想到对u求平方和,即:

10.PNG



我们把u看成一个未知数,这个函数是一个二次函数,其有极小值点。而为了求得这个极小值点,我们对其做偏导即可:
12.PNG



记:
13.PNG



则我们可以将偏导写成如下形式:
14.PNG



B是已知的,P是我们待求的参数矩阵,故可以通过求齐次线性方程组,来求得P的各个参数的解


齐次线性方程组求解的过程我这里就不详细解释了,我算法中使用的方法是经典的高斯消元法,有兴趣的可以仔细看看


当我们求得P的各个未知数a,b,c,d,e,f,g后,需要通过这几个参数反求出我们的偏移量(ox,oy,oz)和缩放量(gx,gy,gz)。




在反求解之前,我们先回到上一个式子,BxP=0。其实满足这个式子的解P有无数组,我们可以将式子改写成BxCP=0,C是一个任意常数,即
15.PNG



我们通过解线性方程组求得的只是这个解系中的一个基本解,所以我们首先要求出这个基本解的C


其算式经推导如下,带入a,b,c,d,e,f,g即可求解:


C=(d2/a + e2/b + f2/c - 4g)/4R2  (R为理论圆球半径)

ox=d/2a

oy=e/2b

oz=f/2c

gx=sqrt(a/C)

gy=sqrt(b/C)

gz=sqrt(c/C)



最后,将这六个参数回调到之前的式子中去,即完成校正:


3.校正程序源码(C语言)
游客,如果您要查看本帖隐藏内容请回复



回复

使用道具 举报

发表于 2016-5-25 13:30:03 | 显示全部楼层
最近正在看相关的东西,有帮助
回复 支持 反对

使用道具 举报

发表于 2016-5-25 13:45:54 | 显示全部楼层
回复一下,学习学习
回复 支持 反对

使用道具 举报

发表于 2016-5-25 14:00:20 | 显示全部楼层
学习一下
回复

使用道具 举报

发表于 2016-5-25 14:34:37 | 显示全部楼层
好啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊啊
回复 支持 反对

使用道具 举报

发表于 2016-5-25 14:46:49 | 显示全部楼层

最近正在看相关的东西,有帮助
回复 支持 反对

使用道具 举报

发表于 2016-5-25 14:59:27 | 显示全部楼层
还挺不错的,感谢楼主
回复 支持 反对

使用道具 举报

发表于 2016-5-25 15:26:43 | 显示全部楼层
看看!!!!!!!!!!!!
回复

使用道具 举报

发表于 2016-5-25 16:03:07 | 显示全部楼层
请问对于磁场校准,G是多少啊啊啊
回复 支持 反对

使用道具 举报

 楼主| 发表于 2016-5-25 16:04:41 | 显示全部楼层
fengheqi 发表于 2016-5-25 16:03
请问对于磁场校准,G是多少啊啊啊

G就是理论磁场圆球的半径,可以取任意值
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

快速回复 返回顶部 返回列表