onev 发表于 2014-5-1 19:01:26

电子罗盘校准 椭球拟合

本帖最后由 onev 于 2014-5-1 22:25 编辑

/**********************************************************************************************
说明:本程序是在VS2012环境下写就。虽然是在C++环境下写就但实际还是C编程,故构造函数、析构函数均略去。
   鉴于C++的流输入和流输出的使用更为方便简洁,因而使用cin、cout代替printf()、scanf()等函数。
描述:
作者:onev
时间:2014
***********************************************************************************************/
#include <iostream>
#include <string>
#include <cmath>
//#include <cstdio>


using namespace std;

#define NUM 6 //线性方程组大小


//http://zh.wikipedia.org/wiki/%E7%BA%BF%E6%80%A7%E6%96%B9%E7%A8%8B%E7%BB%84
//double matrix_A = {{5, -1, -1, -1},{-1, 10, -1, -1},{-1, -1, 5, -1},{-1, -1, -1, 10}};//矩阵A
//double matrix_b = {-4, 12, 8, 34};//矩阵b
//double matrix_x = {0, 0, 0, 0};//解



float matrix_Q = {{-135, 54, -152},{92, -93, -164},{72, 86, -165},{66, 151, 66},{-87,153,-108},{-82,-112,-167}};//6组电子罗盘数据,顺序X、Y、Z

double matrix_A = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};//A矩阵由后面的PP函数产生
double matrix_b = {0,0,0,0,0,0};//同上
double matrix_x = {0, 0, 0, 0, 0, 0};

double matrix_M = {0};      


void PP(void)
{
      double matrix_temp1 = {0};

      for (int i = 0; i < NUM; i++)
      {
                matrix_temp1 = matrix_Q*matrix_Q;
            matrix_temp1 = matrix_Q*matrix_Q;
            matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q;
                matrix_temp1 = matrix_Q;
                matrix_temp1 = matrix_Q;
      }
      for (int j = 0; j < NUM; j++)
      {
                for (int i = 0; i < NUM; i++)
                {
                        matrix_A += matrix_temp1*matrix_temp1;
                        matrix_A += matrix_temp1*matrix_temp1;
                        matrix_A += matrix_temp1*matrix_temp1;
                        matrix_A += matrix_temp1*matrix_temp1;
                        matrix_A += matrix_temp1*matrix_temp1;
                        matrix_A += matrix_temp1*matrix_temp1;
                }
      }
      
      for (int i = 0; i < NUM; i++)
      {
                for (int j = 0; j < NUM; j++)
                {
                        matrix_b += -matrix_temp1;
                }
                cout << "matrix_b[" << i <<"]" << matrix_b << endl;
                cout << "*********************************************" << endl;
      }
      for (int i = 0; i < NUM; i++)
      {
                for(int j = 0; j < NUM; j++)
                {
                        cout << "matrix_A[" << i <<"]" << "["<< j << "]" << matrix_A << endl;
                }
      }
         
}


/******************************************************************************
函数: SOR()
功能: SOR迭代法解线性方程组
调用: 外部
说明: 松弛因子0~1 比如0.9    精度自己决定 比如0.000001
返回值: 有
******************************************************************************/
int SOR()
{
      int i = 0, j = 0,k = 0;
      double w = 0, precision = 0, temp = 0, x = {0, 0, 0, 0};//w:松弛因子   precision:精度
      cout << "请输入松弛因子w: "<< endl;
      cin >> w ;
      cout << "请输入精度: "<< endl;
      cin >> precision;
      for(k = 0; k < 1000; k++)//迭代次数最多不超过100
      {
                for(i = 0; i < NUM; i++)
                {
                        temp = 0;
                        for(j = 0; j < NUM; j++)
                        {
                              if(j != i)
                              temp += matrix_A*matrix_x;      
                        }
                        matrix_x = (1-w)*x+w*(matrix_b-temp)/matrix_A;
                        cout << matrix_x << "      ";
                        if(i == 3)
                        cout << endl;      
                }
                for(i = 0; i < NUM; i++)
                {
                        if(fabs(matrix_x-x) < precision)
                        {
                              if(i == 3)
                              {
                                        cout << endl;
                                        cout << "您输入的松弛因子w = " << w << endl;
                                        cout << "输入的精度precision = " << precision << endl;
                                        cout << "迭代次数: " << k << endl;
                                        cout << "迭代结果:"<< endl;
                                        for (int i = 0; i < NUM; i++)
                                        {
                                                cout << "x" << i << ":" << matrix_x << endl;
                                        }
                              /*      cout <<"x1 = " << matrix_x << "    x2 = " << matrix_x;
                                        cout <<       "x3 = " << matrix_x << "    x4 = " << matrix_x;
                                        cout <<       "x5 = " << matrix_x << "    x6 = " << matrix_x << endl;*/
                                        return 0;
                              }
                        }
                        else
                              break;
                }
                for(i = 0; i < NUM; i++)
                x = matrix_x;
      }
}


void OO(void)
{
      matrix_M = matrix_x/2/matrix_x;
      matrix_M = matrix_x/2/matrix_x;
      matrix_M = matrix_x/2/matrix_x;
      matrix_M = 1.0/(matrix_x*matrix_x/matrix_x/4
                                          +matrix_x*matrix_x/matrix_x/4
                                          +matrix_x*matrix_x/matrix_x/4
                                          -1);//这里把地球磁场大小设成了1
      matrix_M = sqrtl(/*abs*/(matrix_M)*matrix_x);
      matrix_M = sqrtl(/*abs*/(matrix_M)*matrix_x);
      matrix_M = sqrtl(/*abs*/(matrix_M)*matrix_x);
      for(int i = 0; i < 7; i++)
      {
                cout << endl;
                cout << "******************" << endl;
                cout << "matrix_M[" << i << "]:" << matrix_M << endl;
      }
}

                              
int main()
{
      PP();
      SOR();
      OO();
      cout << (matrix_x*matrix_x/matrix_x/4
                                          +matrix_x*matrix_x/matrix_x/4
                                          +matrix_x*matrix_x/matrix_x/4) << endl;
}
说明:测量值      校正后   平移参数      缩放参数   关系:Xc=(Xm+Ox)*gx
         Yc=(Ym+Oy)*gy
         Zc=(Zm+Oz)*gz   
PP(); OO();函数不解释。
matrix_M对应Ox;matrix_M对应Oy;matrix_M对应Oz;
matrix_M对应gx; matrix_M对应gy; matrix_M对应gz;
这是我自己做的椭球拟合,最小二乘法。
问题来了:用AHRSupdate进行姿态解算时在开始的大概十几二十秒内会乱七八糟的飘,过了这会儿之后基本回归正常,但偏航角yaw会有偏移这时,这说的偏移不是漂移。过了刚刚说的时间段后姿态很稳定,yaw不飘,放那不动三个角都不会动,或者你动了之后放那也不会飘,关键在于为什么开始时会有那种乱七八糟的飘呢?一直找不到原因,另外因为还没装MATLAB,求教能否检验一下我的椭球拟合是否正确呢?

onev 发表于 2014-5-1 19:02:27

怎么字都变形了

onev 发表于 2014-5-3 12:25:51

再次用iPhone指南针确认了一下,yaw角没什么大偏差,只是开始时的乱飘还没解决自己顶起

jiangtianyu007 发表于 2014-5-4 10:49:11

楼主能介绍点原理性的东西么,或者参考什么论文的能方便发出来么?

onev 发表于 2014-5-4 12:03:50

jiangtianyu007 发表于 2014-5-4 10:49
楼主能介绍点原理性的东西么,或者参考什么论文的能方便发出来么?

椭球拟合的论文很多,你可以上知网之类的查   直接Google就能查到很多,也可以参考这篇网上到处有你可以直接将代码复制应用也行 但请勿用于商业目的谢谢

cshp138 发表于 2014-5-4 12:22:30

AHRSupdate这个是四元数解姿态吧,这里的四元数初始化时要把你的地磁航向角加进入,不能单单1,0,0,0这样赋值

onev 发表于 2014-5-4 12:28:36

cshp138 发表于 2014-5-4 12:22
AHRSupdate这个是四元数解姿态吧,这里的四元数初始化时要把你的地磁航向角加进入,不能单单1,0,0,0这样 ...

已初始化 但影响不大

jiangtianyu007 发表于 2014-7-17 15:29:50

楼主代码完全不能用

double matrix_temp1 = {0};

      for (int i = 0; i < NUM; i++)
      {
                matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q;
                matrix_temp1 = matrix_Q;
                matrix_temp1 = matrix_Q;
      }

最低级的错误也产生了。

chinamaken 发表于 2014-7-18 10:53:12

利用四元数,旋转矩阵,来计算定位矩阵,航向精度有所提高,你说这个只是说前期它需要校正数据计算

onev 发表于 2014-7-18 17:05:19

jiangtianyu007 发表于 2014-7-17 15:29
楼主代码完全不能用

double matrix_temp1 = {0};


不能用是什么意思,怎么个不能用法,具体点。

onev 发表于 2014-7-18 17:07:36

chinamaken 发表于 2014-7-18 10:53
利用四元数,旋转矩阵,来计算定位矩阵,航向精度有所提高,你说这个只是说前期它需要校正数据计算 ...

难道我的标题还能有其他解释不成?!

rei1984 发表于 2014-7-18 19:52:29

lz 会不会是编译器的问题。建议用icc试试。 我之前也是被 gcc 搞的一踏糊涂。

icc 比较简单的。

jiangtianyu007 发表于 2014-7-19 12:24:22

onev 发表于 2014-7-18 17:05
不能用是什么意思,怎么个不能用法,具体点。

就是我贴的代码呀。

定义了二维数组,却赋值给了地址。

onev 发表于 2014-8-27 14:13:46

rei1984 发表于 2014-7-18 19:52
lz 会不会是编译器的问题。建议用icc试试。 我之前也是被 gcc 搞的一踏糊涂。

icc 比较简单的。   ...

为什么都不仔细看呢

PANGKUN 发表于 2014-8-29 17:04:04

请问楼主,for (int i = 0; i < NUM; i++)
      {
                matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q*matrix_Q;
                matrix_temp1 = matrix_Q;
                matrix_temp1 = matrix_Q;
                matrix_temp1 = matrix_Q;
      }这断怎么理解,因为对C++不是很懂,那个for中的i似乎没有用到,请lz解释下吧

onev 发表于 2014-9-12 10:59:58

本帖最后由 onev 于 2014-9-12 11:03 编辑

很奇怪,我的代码明明不是这样的,复制粘贴就变了

onev 发表于 2014-9-12 11:05:40

jiangtianyu007 发表于 2014-7-17 15:29
楼主代码完全不能用

double matrix_temp1 = {0};


不知道为什么 我的代码不是这样的不知道为什么复制粘贴后变了

onev 发表于 2014-9-12 11:11:36

jiangtianyu007 发表于 2014-7-19 12:24
就是我贴的代码呀。

定义了二维数组,却赋值给了地址。

pp()函数应该是这样的至于复制粘贴后为什么变了我不知道   我的原始代码:

onev 发表于 2014-9-12 11:13:06

PANGKUN 发表于 2014-8-29 17:04
请问楼主,for (int i = 0; i < NUM; i++)
      {
                matrix_temp1 = matrix_Q*matr ...

请参照18楼

onev 发表于 2014-9-12 11:26:30

本帖最后由 onev 于 2014-9-12 11:53 编辑

chinamaken 发表于 2014-7-18 10:53
利用四元数,旋转矩阵,来计算定位矩阵,航向精度有所提高,你说这个只是说前期它需要校正数据计算 ...

磁力计不校正根本不能用,地球磁场太小很容易受周边环境影响,所以须要校正,一般的椭圆校正有很大的缺陷,本来就是一个椭球,虽然你只用到它的一个平面,但只用平面校正显然误差会大,因为你怎么知道你校正的时候是在水平面呢?所以须要进行椭球校正,这里的椭球校正是椭球校正中最简单的,但够用了,而且是本人写的,没有参考任何人的,因为无从参照。

xinmulan 发表于 2014-9-12 11:50:27

我也在想校准磁力计,特别是全姿的。

jiangtianyu007 发表于 2014-9-12 17:31:10

onev 发表于 2014-9-12 11:11
pp()函数应该是这样的至于复制粘贴后为什么变了我不知道   我的原始代码: ...

方便源代码压缩一下上传么?
还有请教下楼主,您拟合的约束方程是什么?Xc Yc Zc满足什么方程呢?

onev 发表于 2014-9-14 19:42:29

本帖最后由 onev 于 2014-9-14 19:43 编辑

jiangtianyu007 发表于 2014-9-12 17:31
方便源代码压缩一下上传么?
还有请教下楼主,您拟合的约束方程是什么?Xc Yc Zc满足什么方程呢? ...

Xc、Yc、Zc是校正后的输出,既然是椭球校正,当然是校正后满足 球 了,即在球面上。

onev 发表于 2014-9-14 19:58:07

onev 发表于 2014-9-14 19:42
Xc、Yc、Zc是校正后的输出,既然是椭球校正,当然是校正后满足 球 了,即在球面上。 ...

如果打不开 用CAJVIEWER打开

jiangtianyu007 发表于 2014-9-15 14:04:07

onev 发表于 2014-9-14 19:42
Xc、Yc、Zc是校正后的输出,既然是椭球校正,当然是校正后满足 球 了,即在球面上。 ...

但是满足的这个球的半径是怎么确定呢?

onev 发表于 2014-9-17 08:35:25

本帖最后由 onev 于 2014-9-17 08:37 编辑

jiangtianyu007 发表于 2014-9-15 14:04
但是满足的这个球的半径是怎么确定呢?
这跟你的当地地球磁场大小是一致的,你的数值我是不知道的哟!我在这里直接定义磁场大小为1。我明明写着注释,你没看啊!

残忆视觉 发表于 2014-9-17 11:54:12

楼主,我和你的情况一样,一上电一段时间内,姿态会漂的很乱,但是过段时间就会稳定。我觉得是初始化四元数的问题,因为我之前不用磁力计用 IMUupdate函数的时候也是需要初始化一下欧拉角和四元数上电之后才不会乱漂。用AHRSupdate函数时,就算我用之前的初始化程序初始化也不行,我认为应该是这段代码中的初始化yaw的公式有错误,但是一直没有解决。不知道这个问题楼主你有没有已经解决了?
/********************根据MPU6050得到的加速度值初始化欧拉角********************/
void init_euler_angle()
{
        HMC5883_out();
        euler_roll_init = atan2(ACCEL_ay(), ACCEL_az());                        //初始化欧拉翻滚角
        euler_pitch_init =-asin(ACCEL_ax() / ACCEL_1G);                        //初始化欧拉俯仰角       
        euler_yaw_init = atan((my*cos(euler_roll_init) - mz*sin(euler_roll_init))/(mx*cos(euler_pitch_init) + my*sin(euler_roll_init)*sin(euler_pitch_init)
                                        - mz*sin(euler_pitch_init)*cos(euler_roll_init)));
}

jiangtianyu007 发表于 2014-9-22 22:29:30

onev 发表于 2014-9-17 08:35
这跟你的当地地球磁场大小是一致的,你的数值我是不知道的哟!我在这里直接定义磁场大小为1。我明明写着注 ...

直接定为1应该不对吧。。。。个人觉得应该定为R,也是个参数,但是这样的话就变成非线性方程组了,需要用迭代法求解。

幻影穿梭 发表于 2014-9-22 22:47:31

mark 下回来看

onev 发表于 2014-10-1 23:32:01

jiangtianyu007 发表于 2014-9-22 22:29
直接定为1应该不对吧。。。。个人觉得应该定为R,也是个参数,但是这样的话就变成非线性方程组了,需要用 ...

定为1是可以的,对后面的解是没有影响的。你说变成非线性方程组,请给你的原因,另外线性方程组难道不是用迭代法来解?

onev 发表于 2014-10-1 23:33:25

残忆视觉 发表于 2014-9-17 11:54
楼主,我和你的情况一样,一上电一段时间内,姿态会漂的很乱,但是过段时间就会稳定。我觉得是初始化四元数 ...

最近乱七八糟的事很多,等闲下来了,希望咱们能专门来讨论这个问题

情迷MJ比莉珍 发表于 2014-10-2 12:55:28

真的很感谢!!

xwx 发表于 2014-10-2 14:01:53

09年到14年的进步

zkl1097 发表于 2014-10-8 19:57:14

楼主不错啊{:smile:}

javabean 发表于 2014-10-8 20:24:04

问题来了之后居然不是挖掘机……

qwe2231695 发表于 2014-12-9 21:47:52

使用vs2012实现了一下,感觉很不错。 要是能够自动识别典型值就好了。其实就是当每个轴取得最大最小值的时候记录下当前的xyz,这样就有6个xyz了。这种是一次线性拟合,期待楼主搞个非线性的拟合,能够纠正传感器的非线性就好了。
磁场强度应该取楼主给的示范数据的平方和应该为40000左右,大家根据各自的xyz取平方和就行了。这样校正出来的数据大小将会和原来类似。

还有一个问题,图片上的那行代码是【i】【j】吗? 我必须要取【j】【i】才能算对

onev 发表于 2014-12-11 09:58:59

qwe2231695 发表于 2014-12-9 21:47
使用vs2012实现了一下,感觉很不错。 要是能够自动识别典型值就好了。其实就是当每个轴取得最大最小值的时 ...

参考 18楼

onev 发表于 2015-3-29 09:22:11

qwe2231695 发表于 2014-12-9 21:47
使用vs2012实现了一下,感觉很不错。 要是能够自动识别典型值就好了。其实就是当每个轴取得最大最小值的时 ...

嗯 对的这都是复制粘贴代码时发生了错误

acchkr 发表于 2015-3-29 19:01:56

先标记一下 有空学习研究

iseafish 发表于 2015-4-15 22:23:15

请问楼主,算出修正后的X,Y,Z数据之后,要如何计算出航向角Yaw?

onev 发表于 2015-4-16 10:01:00

iseafish 发表于 2015-4-15 22:23
请问楼主,算出修正后的X,Y,Z数据之后,要如何计算出航向角Yaw?

用AHRS      

wei4350 发表于 2015-5-26 11:54:30

谢谢分享!!!

机械电子协会 发表于 2015-8-3 14:45:14

必须赞楼主一个 ,很少有看到椭球拟合的,正蛋疼着呢

机械电子协会 发表于 2015-8-5 08:57:30

qwe2231695 发表于 2014-12-9 21:47
使用vs2012实现了一下,感觉很不错。 要是能够自动识别典型值就好了。其实就是当每个轴取得最大最小值的时 ...

上边的数据有6组,你说的平方和是他们的平均值吗?这个40000怎么确认的呢?

机械电子协会 发表于 2015-8-5 09:36:41

我的旋转以电子罗盘为中心吗?我是菜鸟,望不吝赐教,

陶新成 发表于 2015-12-17 11:55:03

请教楼主一个问题,在计算罗盘方位角的时候用俯仰和横滚角进行补偿但是补偿之后的数据乱飘,Xr=Xcosα+Ysinαsinβ+ Zcosβsin,Yr=Ycosβ- Zsin,就是当罗盘水平时即俯仰和横滚都为零时方位角才准确,不知道什么原因请您帮我看一下谢谢了!
http://www.amobbs.com/thread-5639795-1-1.html

clarencxe 发表于 2016-7-13 10:49:41

谢谢分享!验证一下!

xiangbin099 发表于 2016-8-10 09:50:36

标记!!!!!

arkey 发表于 2016-8-12 15:41:47

正在研究中~

山高月小 发表于 2016-8-15 14:28:32

我看了您的这个帖子,里面拟合椭球时有个矩阵matrix_A,这个矩阵代表什么含义,它的求解方式又是什么意思呢?不知道是否方便介绍下,谢谢

山高月小 发表于 2016-8-15 14:44:12

感谢楼主分享,里面拟合椭球时有个矩阵matrix_A,这个矩阵代表什么含义,它的求解方式又是什么意思呢?

方便的话想加一下您的QQ,谢谢。{:smile:}

山高月小 发表于 2016-8-15 14:45:44

感谢分享,里面拟合椭球时有个矩阵matrix_A,这个矩阵代表什么含义,它的求解方式又是什么意思呢?

不知道是否方便加一下楼主QQ,谢谢。

2013的弹子球 发表于 2016-12-19 16:19:01

yaw在找地理北向呢。。。。所以才会乱跳

2013的弹子球 发表于 2016-12-19 16:19:16

yaw在找地理北向呢。。。。所以才会乱跳

逍遥不记年 发表于 2016-12-24 19:46:48

标记一下,最近也在看这方面的东西

ttbboo 发表于 2016-12-30 15:18:34

学习一下

恋芜 发表于 2016-12-30 20:01:05

收藏,留用

haotian_cui 发表于 2017-1-1 19:39:47

一直在找椭球拟合算法,没找到好用的,感谢楼主

xiaoquguang 发表于 2017-11-30 16:32:39

顶起,回头验证一下,多交流
页: [1]
查看完整版本: 电子罗盘校准 椭球拟合