关于地磁计的数据矫正

只看该作者 · 2017-6-27 15:45

本帖最后由 genglei1003 于 2017-6-27 15:59 编辑

最近用哪个磁力计做姿态融合，发现测得的数据偏的厉害，后来上网查了一些方法，找到了AntMag这个软件。原理是每次通过串口测一些磁力计的数据，然后导入到excel中，然后软件就会自动生成一些参数，如下图所示。

一开始用感觉挺好，后来奔波于公寓和办公室之间，发现受环境的影响，磁力计的扰动特别大，每次换个地方都得重新矫正一下，步骤为：打开串口助手——采集数据——保存——导入excel——得到参数，修改程序中的参数——完毕。后来自己试着将数据矫正的方法直接写进程序中，能够省不少事。

磁力计主要收到环境中电磁场的干扰以及本身的一些因素的影响，使得原来的圆球形变成了椭圆形，

我的第一部就是要把椭圆方程求出来，然后再将其还原成圆球形。

将方程化成矩阵进行列主元高斯消去，最后能够得到

解上面的矩阵就能够得到拟合椭圆方程了,化成标准式。

下面是一些采集的原始数据

这是经过修正后的数据

下面是列主元高斯消去的代码（写的有点乱

），xyz里面放的是采集的数据。

只看该作者 · 2017-6-27 15:47

本帖最后由 genglei1003 于 2017-6-27 15:49 编辑

复制

<div class="blockcode"><blockquote>//i为采集样本数

for(i=0;i<Sample_Point_Num;i++)                                                                        //6元一次方程系数计算

        {

        a[0][0]+=xyz[i][0]*xyz[i][0]*xyz[i][0]*xyz[i][0];

        a[0][1]+=xyz[i][0]*xyz[i][0]*xyz[i][1]*xyz[i][1];

        a[0][2]+=xyz[i][0]*xyz[i][0]*xyz[i][2]*xyz[i][2];

        a[0][3]+=xyz[i][0]*xyz[i][0]*xyz[i][0];

        a[0][4]+=xyz[i][0]*xyz[i][0]*xyz[i][1];

        a[0][5]+=xyz[i][0]*xyz[i][0]*xyz[i][2];

        a[0][6]+=xyz[i][0]*xyz[i][0];

                

        a[1][0]+=xyz[i][1]*xyz[i][1]*xyz[i][0]*xyz[i][0];

        a[1][1]+=xyz[i][1]*xyz[i][1]*xyz[i][1]*xyz[i][1];

        a[1][2]+=xyz[i][1]*xyz[i][1]*xyz[i][2]*xyz[i][2];

        a[1][3]+=xyz[i][1]*xyz[i][1]*xyz[i][0];

        a[1][4]+=xyz[i][1]*xyz[i][1]*xyz[i][1];

        a[1][5]+=xyz[i][1]*xyz[i][1]*xyz[i][2];

        a[1][6]+=xyz[i][1]*xyz[i][1];

                

        a[2][0]+=xyz[i][2]*xyz[i][2]*xyz[i][0]*xyz[i][0];

        a[2][1]+=xyz[i][2]*xyz[i][2]*xyz[i][1]*xyz[i][1];

        a[2][2]+=xyz[i][2]*xyz[i][2]*xyz[i][2]*xyz[i][2];

        a[2][3]+=xyz[i][2]*xyz[i][2]*xyz[i][0];

        a[2][4]+=xyz[i][2]*xyz[i][2]*xyz[i][1];

        a[2][5]+=xyz[i][2]*xyz[i][2]*xyz[i][2];

        a[2][6]+=xyz[i][2]*xyz[i][2];

                

        a[3][0]+=xyz[i][0]*xyz[i][0]*xyz[i][0];

        a[3][1]+=xyz[i][0]*xyz[i][1]*xyz[i][1];

        a[3][2]+=xyz[i][0]*xyz[i][2]*xyz[i][2];

        a[3][3]+=xyz[i][0]*xyz[i][0];

        a[3][4]+=xyz[i][0]*xyz[i][1];

        a[3][5]+=xyz[i][0]*xyz[i][2];

        a[3][6]+=xyz[i][0];

        

        a[4][0]+=xyz[i][1]*xyz[i][0]*xyz[i][0];

        a[4][1]+=xyz[i][1]*xyz[i][1]*xyz[i][1];

        a[4][2]+=xyz[i][1]*xyz[i][2]*xyz[i][2];

        a[4][3]+=xyz[i][1]*xyz[i][0];

        a[4][4]+=xyz[i][1]*xyz[i][1];

        a[4][5]+=xyz[i][1]*xyz[i][2];

        a[4][6]+=xyz[i][1];

        

        a[5][0]+=xyz[i][2]*xyz[i][0]*xyz[i][0];

        a[5][1]+=xyz[i][2]*xyz[i][1]*xyz[i][1];

        a[5][2]+=xyz[i][2]*xyz[i][2]*xyz[i][2];

        a[5][3]+=xyz[i][2]*xyz[i][0];

        a[5][4]+=xyz[i][2]*xyz[i][1];

        a[5][5]+=xyz[i][2]*xyz[i][2];

        a[5][6]+=xyz[i][2];

        }



//进行高斯列主元消元

for(i=0;i<n;i++)

        {

                for(j=n-1;j>i;j--)

                        {

                        if(fabs(a[j][i])>fabs(a[j-1][i]))

                                {

                                        for(k=0;k<n;k++)

                                                {

                                                temp=a[j][k];

                                                a[j][k]=a[j-1][k];

                                                a[j-1][k]=temp;

                                                }

                                }        

                        }

                        

                for(j=i+1;j<n-1;j++)

                        {

                                m[j]=-a[i][i]/a[j][i];

                                        for(k=i+1;k<n;k++)

                                                {

                                                b[j][k]=a[j][k]*m[j]+a[i][k];

                                                a[j][k]=b[j][k];        

                                                }                                                        

                        }

        }

//printf("a=%f %f\n",a[5][6],a[5][5]);        

//求解系数A,B,C,D,E,F

        F=a[5][6]/a[5][5];

        E=(a[4][6]-F*a[4][5])/a[4][4];

        D=(a[3][6]-F*a[3][5]-E*a[3][4])/a[3][3];

        C=(a[2][6]-F*a[2][5]-E*a[2][4]-D*a[2][3])/a[2][2];

        B=(a[1][6]-F*a[1][5]-E*a[1][4]-D*a[1][3]-C*a[1][2])/a[1][1];

        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];

printf("A=%F B=%F C=%F D=%F E=%F F=%F\n",A,B,C,D,E,F);