查看: 462|回复: 1
收起左侧

[四轴技术交流] 关于地磁计的数据矫正

[复制链接]
     

2

主题

5

帖子

16

积分

实习生

 楼主| 发表于 2017-6-27 15:45 | 显示全部楼层 |返回版面||阅读模式
本帖最后由 genglei1003 于 2017-6-27 15:59 编辑

最近用哪个磁力计做姿态融合,发现测得的数据偏的厉害,后来上网查了一些方法,找到了AntMag这个软件。原理是每次通过串口测一些磁力计的数据,然后导入到excel中,然后软件就会自动生成一些参数,如下图所示。
一开始用感觉挺好,后来奔波于公寓和办公室之间,发现受环境的影响,磁力计的扰动特别大,每次换个地方都得重新矫正一下,步骤为:打开串口助手——采集数据——保存——导入excel——得到参数,修改程序中的参数——完毕。后来自己试着将数据矫正的方法直接写进程序中,能够省不少事。
磁力计主要收到环境中电磁场的干扰以及本身的一些因素的影响,使得原来的圆球形变成了椭圆形,
我的第一部就是要把椭圆方程求出来,然后再将其还原成圆球形。
将方程化成矩阵进行列主元高斯消去,最后能够得到
解上面的矩阵就能够得到拟合椭圆方程了,化成标准式。
下面是一些采集的原始数据

这是经过修正后的数据
下面是列主元高斯消去的代码(写的有点乱),xyz里面放的是采集的数据。

本帖子中包含更多资源

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

x
     

2

主题

5

帖子

16

积分

实习生

 楼主| 发表于 2017-6-27 15:47 | 显示全部楼层 |返回版面
本帖最后由 genglei1003 于 2017-6-27 15:49 编辑
  1. <div class="blockcode"><blockquote>//i为采集样本数
  2. for(i=0;i<Sample_Point_Num;i++)                                                                        //6元一次方程系数计算
  3.         {
  4.         a[0][0]+=xyz[i][0]*xyz[i][0]*xyz[i][0]*xyz[i][0];
  5.         a[0][1]+=xyz[i][0]*xyz[i][0]*xyz[i][1]*xyz[i][1];
  6.         a[0][2]+=xyz[i][0]*xyz[i][0]*xyz[i][2]*xyz[i][2];
  7.         a[0][3]+=xyz[i][0]*xyz[i][0]*xyz[i][0];
  8.         a[0][4]+=xyz[i][0]*xyz[i][0]*xyz[i][1];
  9.         a[0][5]+=xyz[i][0]*xyz[i][0]*xyz[i][2];
  10.         a[0][6]+=xyz[i][0]*xyz[i][0];
  11.                
  12.         a[1][0]+=xyz[i][1]*xyz[i][1]*xyz[i][0]*xyz[i][0];
  13.         a[1][1]+=xyz[i][1]*xyz[i][1]*xyz[i][1]*xyz[i][1];
  14.         a[1][2]+=xyz[i][1]*xyz[i][1]*xyz[i][2]*xyz[i][2];
  15.         a[1][3]+=xyz[i][1]*xyz[i][1]*xyz[i][0];
  16.         a[1][4]+=xyz[i][1]*xyz[i][1]*xyz[i][1];
  17.         a[1][5]+=xyz[i][1]*xyz[i][1]*xyz[i][2];
  18.         a[1][6]+=xyz[i][1]*xyz[i][1];
  19.                
  20.         a[2][0]+=xyz[i][2]*xyz[i][2]*xyz[i][0]*xyz[i][0];
  21.         a[2][1]+=xyz[i][2]*xyz[i][2]*xyz[i][1]*xyz[i][1];
  22.         a[2][2]+=xyz[i][2]*xyz[i][2]*xyz[i][2]*xyz[i][2];
  23.         a[2][3]+=xyz[i][2]*xyz[i][2]*xyz[i][0];
  24.         a[2][4]+=xyz[i][2]*xyz[i][2]*xyz[i][1];
  25.         a[2][5]+=xyz[i][2]*xyz[i][2]*xyz[i][2];
  26.         a[2][6]+=xyz[i][2]*xyz[i][2];
  27.                
  28.         a[3][0]+=xyz[i][0]*xyz[i][0]*xyz[i][0];
  29.         a[3][1]+=xyz[i][0]*xyz[i][1]*xyz[i][1];
  30.         a[3][2]+=xyz[i][0]*xyz[i][2]*xyz[i][2];
  31.         a[3][3]+=xyz[i][0]*xyz[i][0];
  32.         a[3][4]+=xyz[i][0]*xyz[i][1];
  33.         a[3][5]+=xyz[i][0]*xyz[i][2];
  34.         a[3][6]+=xyz[i][0];
  35.        
  36.         a[4][0]+=xyz[i][1]*xyz[i][0]*xyz[i][0];
  37.         a[4][1]+=xyz[i][1]*xyz[i][1]*xyz[i][1];
  38.         a[4][2]+=xyz[i][1]*xyz[i][2]*xyz[i][2];
  39.         a[4][3]+=xyz[i][1]*xyz[i][0];
  40.         a[4][4]+=xyz[i][1]*xyz[i][1];
  41.         a[4][5]+=xyz[i][1]*xyz[i][2];
  42.         a[4][6]+=xyz[i][1];
  43.        
  44.         a[5][0]+=xyz[i][2]*xyz[i][0]*xyz[i][0];
  45.         a[5][1]+=xyz[i][2]*xyz[i][1]*xyz[i][1];
  46.         a[5][2]+=xyz[i][2]*xyz[i][2]*xyz[i][2];
  47.         a[5][3]+=xyz[i][2]*xyz[i][0];
  48.         a[5][4]+=xyz[i][2]*xyz[i][1];
  49.         a[5][5]+=xyz[i][2]*xyz[i][2];
  50.         a[5][6]+=xyz[i][2];
  51.         }

  52. //进行高斯列主元消元
  53. for(i=0;i<n;i++)
  54.         {
  55.                 for(j=n-1;j>i;j--)
  56.                         {
  57.                         if(fabs(a[j][i])>fabs(a[j-1][i]))
  58.                                 {
  59.                                         for(k=0;k<n;k++)
  60.                                                 {
  61.                                                 temp=a[j][k];
  62.                                                 a[j][k]=a[j-1][k];
  63.                                                 a[j-1][k]=temp;
  64.                                                 }
  65.                                 }       
  66.                         }
  67.                        
  68.                 for(j=i+1;j<n-1;j++)
  69.                         {
  70.                                 m[j]=-a[i][i]/a[j][i];
  71.                                         for(k=i+1;k<n;k++)
  72.                                                 {
  73.                                                 b[j][k]=a[j][k]*m[j]+a[i][k];
  74.                                                 a[j][k]=b[j][k];       
  75.                                                 }                                                       
  76.                         }
  77.         }
  78. //printf("a=%f %f\n",a[5][6],a[5][5]);       
  79. //求解系数A,B,C,D,E,F
  80.         F=a[5][6]/a[5][5];
  81.         E=(a[4][6]-F*a[4][5])/a[4][4];
  82.         D=(a[3][6]-F*a[3][5]-E*a[3][4])/a[3][3];
  83.         C=(a[2][6]-F*a[2][5]-E*a[2][4]-D*a[2][3])/a[2][2];
  84.         B=(a[1][6]-F*a[1][5]-E*a[1][4]-D*a[1][3]-C*a[1][2])/a[1][1];
  85.         A=(a[0][6]-F*a[0][5]-E*a[0][4]-D*a[0][3]-C*a[0][2]-B*a[0][1])/a[0][0];
  86. printf("A=%F B=%F C=%F D=%F E=%F F=%F\n",A,B,C,D,E,F);
复制代码



您需要登录后才可以回帖 登录 | 注册 手机登录

本版积分规则

关闭

热门推荐上一条 /2 下一条

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