使用橢圓擬合做電子羅盤校正 (amobbs.com 阿莫电子技术论坛) -

john800422 发表于 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 = {0}, tempArrY = {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 = {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 = MEG_X2Y2;
MegArr = MEG_X1Y3;
MegArr = MEG_X2Y1;
MegArr = MEG_X1Y2;
MegArr = MEG_X1Y1;
MegArr = -MEG_X3Y1;

MegArr = MEG_X1Y3;
MegArr = MEG_X0Y4;
MegArr = MEG_X1Y2;
MegArr = MEG_X0Y3;
MegArr = MEG_X0Y2;
MegArr = -MEG_X2Y2;

MegArr = MEG_X2Y1;
MegArr = MEG_X1Y2;
MegArr = MEG_X2Y0;
MegArr = MEG_X1Y1;
MegArr = MEG_X1Y0;
MegArr = -MEG_X3Y0;

MegArr = MEG_X1Y2;
MegArr = MEG_X0Y3;
MegArr = MEG_X1Y1;
MegArr = MEG_X0Y2;
MegArr = MEG_X0Y1;
MegArr = -MEG_X2Y1;

MegArr = MEG_X1Y1;
MegArr = MEG_X0Y2;
MegArr = MEG_X1Y0;
MegArr = MEG_X0Y1;
MegArr = Num;
MegArr = -MEG_X2Y0;

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

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

temp = MegArr*MegArr-4*MegArr;
Ans = (2.0f*MegArr*MegArr-MegArr*MegArr)/temp; /* X0 */
Ans = (2.0f*MegArr-MegArr*MegArr)/temp; /* Y0 */

temp= (Ans*Ans+MegArr*Ans*Ans+MegArr*Ans*Ans-MegArr)*(temp1*temp1*temp1*temp1-1.0f);
temp2 = cosf(Ans);
Ans = temp2/sqrtf((MegArr*temp1*temp1-1)/temp); /* a */
Ans = temp2/sqrtf((temp1*temp1-MegArr)/temp);    /* b */

/* 將之前除的1000補回來 */
Ans = Ans*1000.0f;
Ans = Ans*1000.0f;
Ans = Ans*1000.0f;
Ans = Ans*1000.0f;
}

zywei_09 发表于 2013-4-8 09:44:33

顶一个，论坛上之前有用matlab拟合的，我是用那个matlab拟合校正的，效果很好。

john800422 发表于 2013-4-8 10:41:27

zywei_09 发表于 2013-4-8 09:44 static/image/common/back.gif
顶一个，论坛上之前有用matlab拟合的，我是用那个matlab拟合校正的，效果很好。 ...

不知可貼個連結不?

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

zywei_09 发表于 2013-4-9 08:30:14

john800422 发表于 2013-4-8 10:41 static/image/common/back.gif
不知可貼個連結不?

最近也在學Matlab

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

pipi516 发表于 2013-4-9 08:36:35

记号一下,

john800422 发表于 2013-4-9 12:19:40

zywei_09 发表于 2013-4-9 08:30 static/image/common/back.gif
http://www.amobbs.com/forum.php?mod=viewthread&tid=5488742&highlight=5883
《分享一种校准HMC5883的 ...

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

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

zywei_09 发表于 2013-4-9 18:00:12

john800422 发表于 2013-4-9 12:19 static/image/common/back.gif
感謝提供連結
又學到了一些~

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

__@ 发表于 2013-4-10 12:10:07

指南针是轴三的数据，楼主只要稍微改一下变成椭球拟合就可以修正三轴罗盘了

john800422 发表于 2013-4-10 14:34:14

zywei_09 发表于 2013-4-9 18:00 static/image/common/back.gif
还好，因为不是每次都要校准，校准一次基本就可以了。除非环境变化比较大。其实真正的校准是应该是把一般 ...

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

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

john800422 发表于 2013-4-10 14:35:45

__@ 发表于 2013-4-10 12:10 static/image/common/back.gif
指南针是轴三的数据，楼主只要稍微改一下变成椭球拟合就可以修正三轴罗盘了 ...

似乎會複雜許多

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

asha 发表于 2013-4-11 08:24:41

这个是椭圆到圆的平面矫正，只要倾角大了还是不行，用椭球到圆球的约束矫正会得到更好的效果。

kmani 发表于 2013-10-8 09:58:57

楼主，台湾教的是mathematica吗？
国内学校教的是matlab。

john800422 发表于 2013-10-8 11:31:09

kmani 发表于 2013-10-8 09:58 static/image/common/back.gif
楼主，台湾教的是mathematica吗？
国内学校教的是matlab。

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

lukefan2008 发表于 2013-11-14 21:56:37

之前因为考试一直没有往下做，这两天来看，台湾兄都进展这么深入了

analoglamb 发表于 2013-11-16 12:48:55

谢谢分享。。。。。。。

zsjalive@126 发表于 2013-11-18 09:45:25

这个必须顶起

silence2455 发表于 2014-4-4 19:20:15

楼主大人，我想问下求出椭圆的五个参数之后怎样把它校准为圆啊？

金牛AKI 发表于 2014-4-4 20:55:09

支持了不错哦

john800422 发表于 2014-4-4 21:27:55

silence2455 发表于 2014-4-4 19:20
楼主大人，我想问下求出椭圆的五个参数之后怎样把它校准为圆啊？

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

silence2455 发表于 2014-4-10 14:56:28

楼主大哥，请问一下，你这里面得到的a和b应该是长短半轴吧？那么进行缩放的时候怎么知道是X方向的轴更长还是Y方向的轴更长呢？

john800422 发表于 2014-4-10 15:58:42

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

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

衍云发表于 2014-4-11 02:34:49

大神膜拜，留个记号以后肯定要用到

lkm_unication 发表于 2014-4-11 09:34:33

这个必须mark，谢谢LZ的分享。{:handshake:}

bxcfc 发表于 2014-5-5 13:41:38

膜拜大神，学到很多

lujianfeng2001 发表于 2014-5-18 12:32:39

我想问5883的程序流程图是怎样的？

silence2455 发表于 2014-5-18 15:11:15

楼主大人，我用你的这个函数EllipseFitting进行程序校准，发现一加上这个函数就会导致程序复位，是不是因为太费时了？请问你有没有遇到这个问题啊？

孤单片机器人 发表于 2014-8-21 13:55:13

楼主我问一下采集数据时旋转罗盘时一定要在水平面内吗

john800422 发表于 2014-8-21 17:02:23

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

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

papa0305 发表于 2014-8-21 20:39:00

mark

yang9769 发表于 2014-8-22 17:08:10

LZ，去年我也用过椭圆拟合的方法校正磁场，发现软磁（Theta ）有问题，用此方法校正之后有可能会出现角度的静差，请问你有什么办法解决么？拜谢~

john800422 发表于 2014-8-22 22:32:34

yang9769 发表于 2014-8-22 17:08
LZ，去年我也用过椭圆拟合的方法校正磁场，发现软磁（Theta ）有问题，用此方法校正之后有可能会出现角度的 ...

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

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

PANGKUN 发表于 2014-8-30 16:37:31

john800422 发表于 2014-8-22 22:32
橢圓擬合僅是將已知數據擬合出橢圓形，並假設之後讀取到的數據依然會分布在該橢圓上，
所以若是環境磁場 ...

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

knightlyj 发表于 2014-8-30 18:06:34

其实原理很简单的,最小二乘问题求解在微积分和线性代数都有提到,各位可以多看看书

zkl1097 发表于 2014-10-8 20:08:35

这个收藏一下{:lol:}

iseafish 发表于 2014-12-6 12:11:15

感谢分享{:smile:}

xydang 发表于 2014-12-6 12:54:40

不错，谢谢分享

默默七 发表于 2014-12-6 15:56:12

这个用来校准软磁可能还不错，关键看运算效率

随影发表于 2015-1-26 07:18:03

谢谢楼主~最近在搞四轴~

john800422 发表于 2015-1-26 17:29:46

随影发表于 2015-1-26 07:18
谢谢楼主~最近在搞四轴~

回答你的問題 http://www.amobbs.com/forum.php?mod=redirect&goto=findpost&ptid=5605744&pid=8384377

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

   case n:
      MagDataX = (s16)MoveAve_WMA(Mag.X, MAG_FIFO, MagCorrectionAve);
      MagDataY = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO, MagCorrectionAve);
      break;

   ...

   default:
      Correction_Time = 0;
      EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
      Mag.OffsetX = Ellipse;
      Mag.OffsetY = Ellipse;
      SensorMode = Mode_Algorithm;// 切換至運算模式
      break;
}

(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 筆磁力計資料（取完平均值的），將這些資料放入橢圓擬合計算橢圓參數。

随影发表于 2015-1-28 15:27:28

本帖最后由随影于 2015-1-28 15:28 编辑

john800422 发表于 2015-1-26 17:29
回答你的問題 http://www.amobbs.com/forum.php?mod=redirect&goto=findpost&ptid=5605744&pid=8384377

...

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

john800422 发表于 2015-1-29 22:20:14

本帖最后由 john800422 于 2015-1-30 13:20 编辑

随影发表于 2015-1-28 15:27
恩恩，谢谢楼主大神，我都懂你的思路了。我的还有一个疑问在于。。这两个数组为什么要这样子用呢？我 ...

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

wszyjsw2 发表于 2015-1-29 22:42:58

mark Mathematica

zhenan421731 发表于 2015-2-11 10:03:21

好东西真心有帮助

注释、青春 发表于 2015-3-16 12:11:38

mark 先标记一下

PANGKUN 发表于 2015-5-4 12:04:37

请问椭圆拟合后解算出来的角度Θ怎么利用

john800422 发表于 2015-5-4 12:22:40

PANGKUN 发表于 2015-5-4 12:04
请问椭圆拟合后解算出来的角度Θ怎么利用

旋轉回水平

PANGKUN 发表于 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);是这样吗

wenming 发表于 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 = MegArr - MegArr/MegArr*MegArr;
for(i=0; i<5; i++) {
temp = MegArr;
for(j=i; j<6; j++)
MegArr = MegArr/temp;
}
for(j=4; j>0; j--)
for(i=0; i<j; i++)
   MegArr = MegArr - MegArr*MegArr;

tangqh 发表于 2015-5-8 09:17:07

看起来就很牛逼的样子，实际上更牛逼！

wei4350 发表于 2015-5-26 14:45:47

好贴必须顶起！！！！

常山赵子龙 发表于 2015-9-26 18:17:03

mark下，，学习了~~

westloveohyeah 发表于 2015-10-28 22:23:53

curve fitting 思路是正确的，但是用的时候依然会有问题，不是数学上的问题，而是工程上的问题。
来自国内外的朋友，我很想和你继续讨论这个问题。

john800422 发表于 2015-10-28 23:04:50

westloveohyeah 发表于 2015-10-28 22:23
curve fitting 思路是正确的，但是用的时候依然会有问题，不是数学上的问题，而是工程上的问题。
来自国内 ...

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

實際的四軸磁力計校正實現可以參考下面，使用 MPU9150，一圈取 8 的點做採樣
https://github.com/QCopter/QCopterFlightControl/blob/d79ff6a8ab07dd373d9e41f98af74c1594ed61a0/Software/QCopterFC_FlightControl/Program_System/QCopterFC_it.c

westloveohyeah 发表于 2015-10-29 08:34:15

john800422 发表于 2015-10-28 23:04
有問題可以直接在這裡提出，或許其他人有會有類似問題。

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

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

arkey 发表于 2018-3-17 22:52:19

罗盘校准mark一下下

s1j2h3 发表于 2018-3-28 09:23:59

这个我也做出来了，不过现在需要实时修正，不知有没有思路？

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

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