搜索
bottom↓
回复: 55

使用橢圓擬合做電子羅盤校正

  [复制链接]

出0入0汤圆

发表于 2013-4-8 00:24:33 | 显示全部楼层 |阅读模式
本帖最后由 john800422 于 2013-4-8 00:27 编辑

自己做的, 分享下

主要是參考
"橢圓直接擬合算法研究"


以Mathematica做實現
附有C及推導


流程:
最小二乘法→解方程組→代推導結果→校正電子羅盤數據→使用atan2求解航向角

=========== 推導 =======================================




=========== Mathematica實現 ===============================







=========== C語言實現 ===================================
void EllipseFitting( float* Ans, short* MegDataX, short* MegDataY, unsigned char Num )
{
  char i, j, k;
  float temp, temp1, temp2;
  float tempArrX[8] = {0}, tempArrY[8] = {0};

  float MEG_X1Y0 = 0.0f;
  float MEG_X2Y0 = 0.0f;
  float MEG_X3Y0 = 0.0f;
  float MEG_X0Y1 = 0.0f;
  float MEG_X0Y2 = 0.0f;
  float MEG_X0Y3 = 0.0f;
  float MEG_X0Y4 = 0.0f;
  float MEG_X1Y1 = 0.0f;
  float MEG_X2Y1 = 0.0f;
  float MEG_X1Y2 = 0.0f;
  float MEG_X1Y3 = 0.0f;
  float MEG_X2Y2 = 0.0f;
  float MEG_X3Y1 = 0.0f;

  float MegArr[5][6] = {0};

  /* 因為累加數值會太大, 故除1000 */
  for(i=0; i<Num; i++) {
    tempArrX = (float)MegDataX/1000.0f;
    tempArrY = (float)MegDataY/1000.0f;
  }

  for(i=0; i<Num; i++) {
     MEG_X1Y0 += tempArrX;
     MEG_X2Y0 += tempArrX*tempArrX;
     MEG_X3Y0 += tempArrX*tempArrX*tempArrX;
     MEG_X0Y1 += tempArrY;
     MEG_X0Y2 += tempArrY*tempArrY;
     MEG_X0Y3 += tempArrY*tempArrY*tempArrY;
     MEG_X0Y4 += tempArrY*tempArrY*tempArrY*tempArrY;
     MEG_X1Y1 += tempArrX*tempArrY;
     MEG_X2Y1 += tempArrX*tempArrX*tempArrY;
     MEG_X1Y2 += tempArrX*tempArrY*tempArrY;
     MEG_X1Y3 += tempArrX*tempArrY*tempArrY*tempArrY;
     MEG_X2Y2 += tempArrX*tempArrX*tempArrY*tempArrY;
     MEG_X3Y1 += tempArrX*tempArrX*tempArrX*tempArrY;
  }

  MegArr[0][0] = MEG_X2Y2;
  MegArr[0][1] = MEG_X1Y3;
  MegArr[0][2] = MEG_X2Y1;
  MegArr[0][3] = MEG_X1Y2;
  MegArr[0][4] = MEG_X1Y1;
  MegArr[0][5] = -MEG_X3Y1;

  MegArr[1][0] = MEG_X1Y3;
  MegArr[1][1] = MEG_X0Y4;
  MegArr[1][2] = MEG_X1Y2;
  MegArr[1][3] = MEG_X0Y3;
  MegArr[1][4] = MEG_X0Y2;
  MegArr[1][5] = -MEG_X2Y2;

  MegArr[2][0] = MEG_X2Y1;
  MegArr[2][1] = MEG_X1Y2;
  MegArr[2][2] = MEG_X2Y0;
  MegArr[2][3] = MEG_X1Y1;
  MegArr[2][4] = MEG_X1Y0;
  MegArr[2][5] = -MEG_X3Y0;

  MegArr[3][0] = MEG_X1Y2;
  MegArr[3][1] = MEG_X0Y3;
  MegArr[3][2] = MEG_X1Y1;
  MegArr[3][3] = MEG_X0Y2;
  MegArr[3][4] = MEG_X0Y1;
  MegArr[3][5] = -MEG_X2Y1;

  MegArr[4][0] = MEG_X1Y1;
  MegArr[4][1] = MEG_X0Y2;
  MegArr[4][2] = MEG_X1Y0;
  MegArr[4][3] = MEG_X0Y1;
  MegArr[4][4] = Num;
  MegArr[4][5] = -MEG_X2Y0;

  /* 解方程組 */
  for(i=0; i<5; i++)
    for(j=i+1; j<6; j++)
      for(k=5; k>i-1; k--)
        MegArr[j][k] = MegArr[j][k] - MegArr[j]/MegArr*MegArr[k];
  for(i=0; i<5; i++) {
    temp = MegArr;
    for(j=i; j<6; j++)
    MegArr[j] = MegArr[j]/temp;
  }
  for(j=4; j>0; j--)
    for(i=0; i<j; i++)
      MegArr[5] = MegArr[5] - MegArr[j]*MegArr[j][5];

  /* 解算橢圓參數 */
  temp = (1-MegArr[1][5])/MegArr[0][5];
  temp1 = temp + sqrtf(temp*temp+1.0f);
  Ans[0] = atanf(temp1);        /* Theta */

  temp = MegArr[0][5]*MegArr[0][5]-4*MegArr[1][5];
  Ans[1] = (2.0f*MegArr[1][5]*MegArr[2][5]-MegArr[0][5]*MegArr[3][5])/temp;        /* X0 */
  Ans[2] = (2.0f*MegArr[3][5]-MegArr[0][5]*MegArr[2][5])/temp;        /* Y0 */

  temp  = (Ans[1]*Ans[1]+MegArr[0][5]*Ans[1]*Ans[2]+MegArr[1][5]*Ans[2]*Ans[2]-MegArr[4][5])*(temp1*temp1*temp1*temp1-1.0f);
  temp2 = cosf(Ans[0]);
  Ans[3] = temp2/sqrtf((MegArr[1][5]*temp1*temp1-1)/temp);        /* a */
  Ans[4] = temp2/sqrtf((temp1*temp1-MegArr[1][5])/temp);       /* b */

  /* 將之前除的1000補回來 */
  Ans[1] = Ans[1]*1000.0f;
  Ans[2] = Ans[2]*1000.0f;
  Ans[3] = Ans[3]*1000.0f;
  Ans[4] = Ans[4]*1000.0f;
}

本帖子中包含更多资源

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

x

阿莫论坛20周年了!感谢大家的支持与爱护!!

一只鸟敢站在脆弱的枝条上歇脚,它依仗的不是枝条不会断,而是自己有翅膀,会飞。

出0入0汤圆

发表于 2013-4-8 09:44:33 | 显示全部楼层
顶一个,论坛上之前有用matlab拟合的,我是用那个matlab拟合校正的,效果很好。

出0入0汤圆

 楼主| 发表于 2013-4-8 10:41:27 | 显示全部楼层
zywei_09 发表于 2013-4-8 09:44
顶一个,论坛上之前有用matlab拟合的,我是用那个matlab拟合校正的,效果很好。 ...

不知可貼個連結不?

最近也在學Matlab
想說用Matlab寫個接收STM32的示波器看看

出0入0汤圆

发表于 2013-4-9 08:30:14 | 显示全部楼层
john800422 发表于 2013-4-8 10:41
不知可貼個連結不?

最近也在學Matlab

http://www.amobbs.com/forum.php? ... &highlight=5883
《分享一种校准HMC5883的方法,此法也可以校验其他双轴... 》
我是把5883的数据读回来输入到Matlab里拟合的。

出0入17汤圆

发表于 2013-4-9 08:36:35 | 显示全部楼层
记号一下,

出0入0汤圆

 楼主| 发表于 2013-4-9 12:19:40 | 显示全部楼层
zywei_09 发表于 2013-4-9 08:30
http://www.amobbs.com/forum.php?mod=viewthread&tid=5488742&highlight=5883
《分享一种校准HMC5883的 ...

感謝提供連結
又學到了一些~

不過因為這需要用在四軸上的
送回Matlab的方法貌似不方便

出0入0汤圆

发表于 2013-4-9 18:00:12 | 显示全部楼层
john800422 发表于 2013-4-9 12:19
感謝提供連結
又學到了一些~

还好,因为不是每次都要校准,校准一次基本就可以了。除非环境变化比较大。其实真正的校准是应该是把一般椭球体校准成球体。。。。

出0入0汤圆

发表于 2013-4-10 12:10:07 | 显示全部楼层
指南针是轴三的数据,楼主只要稍微改一下变成椭球拟合就可以修正三轴罗盘了

出0入0汤圆

 楼主| 发表于 2013-4-10 14:34:14 | 显示全部楼层
zywei_09 发表于 2013-4-9 18:00
还好,因为不是每次都要校准,校准一次基本就可以了。除非环境变化比较大。其实真正的校准是应该是把一般 ...

每次換環境
圓心似乎都會偏移
所以才每次啟動都先校正

等有空再來測試環境對航向角的誤差影響

出0入0汤圆

 楼主| 发表于 2013-4-10 14:35:45 | 显示全部楼层
__@ 发表于 2013-4-10 12:10
指南针是轴三的数据,楼主只要稍微改一下变成椭球拟合就可以修正三轴罗盘了 ...

似乎會複雜許多

尤其是推導橢圓參數的部分

出0入0汤圆

发表于 2013-4-11 08:24:41 | 显示全部楼层
这个是椭圆到圆的平面矫正,只要倾角大了还是不行,用椭球到圆球的约束矫正会得到更好的效果。

出0入0汤圆

发表于 2013-10-8 09:58:57 | 显示全部楼层
楼主,台湾教的是mathematica吗?
国内学校教的是matlab。

出0入0汤圆

 楼主| 发表于 2013-10-8 11:31:09 | 显示全部楼层
kmani 发表于 2013-10-8 09:58
楼主,台湾教的是mathematica吗?
国内学校教的是matlab。

工程科系幾乎都是教 matlab
mathematica 是自學的

出0入0汤圆

发表于 2013-11-14 21:56:37 | 显示全部楼层
之前因为考试一直没有往下做,这两天来看,台湾兄都进展这么深入了

出0入0汤圆

发表于 2013-11-16 12:48:55 | 显示全部楼层
谢谢分享。。。。。。。

出0入0汤圆

发表于 2013-11-18 09:45:25 | 显示全部楼层
这个必须顶起

出0入0汤圆

发表于 2014-4-4 19:20:15 | 显示全部楼层
楼主大人,我想问下求出椭圆的五个参数之后怎样把它校准为圆啊?

出0入0汤圆

发表于 2014-4-4 20:55:09 | 显示全部楼层
支持了 不错哦

出0入0汤圆

 楼主| 发表于 2014-4-4 21:27:55 | 显示全部楼层
silence2455 发表于 2014-4-4 19:20
楼主大人,我想问下求出椭圆的五个参数之后怎样把它校准为圆啊?

長軸除以長軸,短軸除以短軸,就會變成單位圓了

出0入0汤圆

发表于 2014-4-10 14:56:28 | 显示全部楼层
楼主大哥,请问一下,你这里面得到的a和b应该是长短半轴吧?那么进行缩放的时候怎么知道是X方向的轴更长还是Y方向的轴更长呢?

出0入0汤圆

 楼主| 发表于 2014-4-10 15:58:42 | 显示全部楼层
silence2455 发表于 2014-4-10 14:56
楼主大哥,请问一下,你这里面得到的a和b应该是长短半轴吧?那么进行缩放的时候怎么知道是X方向的轴更长还 ...

可以通過角度或是取樣數據得知。

出0入0汤圆

发表于 2014-4-11 02:34:49 | 显示全部楼层
大神膜拜,留个记号以后肯定要用到

出0入0汤圆

发表于 2014-4-11 09:34:33 | 显示全部楼层
这个必须mark,谢谢LZ的分享。

出0入0汤圆

发表于 2014-5-5 13:41:38 | 显示全部楼层
膜拜大神,学到很多

出0入0汤圆

发表于 2014-5-18 12:32:39 | 显示全部楼层
我想问5883的程序流程图是怎样的?

出0入0汤圆

发表于 2014-5-18 15:11:15 | 显示全部楼层
楼主大人,我用你的这个函数EllipseFitting进行程序校准,发现一加上这个函数就会导致程序复位,是不是因为太费时了?请问你有没有遇到这个问题啊?

出0入0汤圆

发表于 2014-8-21 13:55:13 | 显示全部楼层
楼主  我问一下  采集数据时旋转罗盘时  一定要在水平面内吗

出0入0汤圆

 楼主| 发表于 2014-8-21 17:02:23 | 显示全部楼层
孤单片机器人 发表于 2014-8-21 13:55
楼主  我问一下  采集数据时旋转罗盘时  一定要在水平面内吗

目前橢圓擬合僅校正旋轉的平面,
無法校正三維空間

出0入0汤圆

发表于 2014-8-21 20:39:00 | 显示全部楼层
mark         

出0入0汤圆

发表于 2014-8-22 17:08:10 | 显示全部楼层
LZ,去年我也用过椭圆拟合的方法校正磁场,发现软磁(Theta )有问题,用此方法校正之后有可能会出现角度的静差,请问你有什么办法解决么?拜谢~

出0入0汤圆

 楼主| 发表于 2014-8-22 22:32:34 | 显示全部楼层
yang9769 发表于 2014-8-22 17:08
LZ,去年我也用过椭圆拟合的方法校正磁场,发现软磁(Theta )有问题,用此方法校正之后有可能会出现角度的 ...

橢圓擬合僅是將已知數據擬合出橢圓形,並假設之後讀取到的數據依然會分布在該橢圓上,
所以若是環境磁場變化,讀到的數據就不一定會分布在橢圓上,轉換出來的角度就會不準確。

假設環境磁場變化不大的話,可以使用陀螺儀作為主要測量方位的裝置,定期透過電子羅盤來校正陀螺儀。

出0入0汤圆

发表于 2014-8-30 16:37:31 | 显示全部楼层
john800422 发表于 2014-8-22 22:32
橢圓擬合僅是將已知數據擬合出橢圓形,並假設之後讀取到的數據依然會分布在該橢圓上,
所以若是環境磁場 ...

计算这个对单片机的要求挺高吧

出0入0汤圆

发表于 2014-8-30 18:06:34 | 显示全部楼层
其实原理很简单的,最小二乘问题求解在微积分和线性代数都有提到,各位可以多看看书

出0入0汤圆

发表于 2014-10-8 20:08:35 | 显示全部楼层
这个收藏一下

出0入0汤圆

发表于 2014-12-6 12:11:15 | 显示全部楼层
感谢分享

出0入0汤圆

发表于 2014-12-6 12:54:40 | 显示全部楼层
不错,谢谢分享

出0入0汤圆

发表于 2014-12-6 15:56:12 | 显示全部楼层
这个用来校准软磁可能还不错,关键看运算效率

出0入0汤圆

发表于 2015-1-26 07:18:03 | 显示全部楼层
谢谢楼主~最近在搞四轴~

出0入0汤圆

 楼主| 发表于 2015-1-26 17:29:46 | 显示全部楼层
随影 发表于 2015-1-26 07:18
谢谢楼主~最近在搞四轴~


回答你的問題 http://www.amobbs.com/forum.php? ... 744&pid=8384377

  1. switch((u16)(Correction_Time/600)) {

  2.         case n:
  3.           MagDataX[n] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrectionAve);  
  4.           MagDataY[n] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrectionAve);
  5.           break;

  6.         ...

  7.         default:
  8.           Correction_Time = 0;
  9.           EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
  10.           Mag.OffsetX = Ellipse[1];
  11.           Mag.OffsetY = Ellipse[2];
  12.           SensorMode = Mode_Algorithm;  // 切換至運算模式
  13.           break;
  14. }
复制代码


(Correction_Time/600) 決定每一個 case 會執行的時間與次數。

case 的 n = 0 ~ 7,表示 360 度取樣 8 個點,分別為 0(360), 45, 90, 135, 180, 225, 270, 315
每一個取樣點裡面紀錄 X, Y 軸的磁力大小,MoveAve_WMA 只是求多筆資料的平均值。

所以跑完 case0 ~ case7 會有 8*2 筆磁力計資料(取完平均值的),將這些資料放入橢圓擬合計算橢圓參數。

出0入0汤圆

发表于 2015-1-28 15:27:28 | 显示全部楼层
本帖最后由 随影 于 2015-1-28 15:28 编辑


恩恩,谢谢楼主大神,我都懂你的思路了。我的还有一个疑问在于。。这两个数组为什么要这样子用呢?   我个人的思维想法会做两个不同的数组。而楼主的处理在于用了一个数组。仅仅是头部相差1?这中间有什么精妙的算法吗?

本帖子中包含更多资源

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

x

出0入0汤圆

 楼主| 发表于 2015-1-29 22:20:14 | 显示全部楼层
本帖最后由 john800422 于 2015-1-30 13:20 编辑
随影 发表于 2015-1-28 15:27
恩恩,谢谢楼主大神,我都懂你的思路了。我的还有一个疑问在于。。这两个数组为什么要这样子用呢?   我 ...


只是指針與數組的用法而已

出0入0汤圆

发表于 2015-1-29 22:42:58 | 显示全部楼层
mark       Mathematica

出0入0汤圆

发表于 2015-2-11 10:03:21 | 显示全部楼层
好东西 真心有帮助

出0入0汤圆

发表于 2015-3-16 12:11:38 | 显示全部楼层
mark 先标记一下

出0入0汤圆

发表于 2015-5-4 12:04:37 | 显示全部楼层
请问椭圆拟合后解算出来的角度Θ怎么利用

出0入0汤圆

 楼主| 发表于 2015-5-4 12:22:40 | 显示全部楼层
PANGKUN 发表于 2015-5-4 12:04
请问椭圆拟合后解算出来的角度Θ怎么利用


旋轉回水平

出0入0汤圆

发表于 2015-5-4 14:03:54 | 显示全部楼层
xj=cos(-thetarad)/a*(X-x0)-sin(-thetarad)/a*(Y-y0);
yj=sin(-thetarad)/b*(X-x0)+cos(-thetarad)/b*(Y-y0);是这样吗

出0入20汤圆

发表于 2015-5-4 14:07:27 | 显示全部楼层
高手啊,这个方程算起来挺复杂的。
for(i=0; i<5; i++)
     for(j=i+1; j<6; j++)
       for(k=5; k>i-1; k--)
         MegArr[j][k] = MegArr[j][k] - MegArr[j]/MegArr*MegArr[k];
   for(i=0; i<5; i++) {
     temp = MegArr;
     for(j=i; j<6; j++)
     MegArr[j] = MegArr[j]/temp;
   }
   for(j=4; j>0; j--)
     for(i=0; i<j; i++)
       MegArr[5] = MegArr[5] - MegArr[j]*MegArr[j][5];

出0入0汤圆

发表于 2015-5-8 09:17:07 | 显示全部楼层
看起来就很牛逼的样子,实际上更牛逼!

出0入0汤圆

发表于 2015-5-26 14:45:47 | 显示全部楼层
好贴必须顶起!!!!

出0入0汤圆

发表于 2015-9-26 18:17:03 | 显示全部楼层
mark下,,学习了~~

出0入0汤圆

发表于 2015-10-28 22:23:53 | 显示全部楼层
curve fitting 思路是正确的,但是用的时候依然会有问题,不是数学上的问题,而是工程上的问题。
来自国内外的朋友,我很想和你继续讨论这个问题。

出0入0汤圆

 楼主| 发表于 2015-10-28 23:04:50 | 显示全部楼层
westloveohyeah 发表于 2015-10-28 22:23
curve fitting 思路是正确的,但是用的时候依然会有问题,不是数学上的问题,而是工程上的问题。
来自国内 ...

有問題可以直接在這裡提出,或許其他人有會有類似問題。

實際的四軸磁力計校正實現可以參考下面,使用 MPU9150,一圈取 8 的點做採樣
https://github.com/QCopter/QCopt ... stem/QCopterFC_it.c

出0入0汤圆

发表于 2015-10-29 08:34:15 | 显示全部楼层
john800422 发表于 2015-10-28 23:04
有問題可以直接在這裡提出,或許其他人有會有類似問題。

實際的四軸磁力計校正實現可以參考下面,使用 M ...

等我的论文发表出来,投稿在IEEE trans

出0入0汤圆

发表于 2018-3-17 22:52:19 来自手机 | 显示全部楼层
罗盘校准mark一下下

出0入0汤圆

发表于 2018-3-28 09:23:59 | 显示全部楼层
这个我也做出来了,不过现在需要实时修正,不知有没有思路?
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。【本论坛不允许直接上传手机拍摄图片,浪费大家下载带宽和论坛服务器空间,请压缩后(图片小于1兆)才上传。压缩方法可以在微信里面发给自己(不要勾选“原图),然后下载,就能得到压缩后的图片】。另外,手机版只能上传图片,要上传附件需要切换到电脑版(不需要使用电脑,手机上切换到电脑版就行,页面底部)。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子技术论坛 ( 粤ICP备2022115958号, 版权所有:东莞阿莫电子贸易商行 创办于2004年 (公安交互式论坛备案:44190002001997 ) )

GMT+8, 2024-3-29 19:19

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

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