标题: 超详细讲解：罗盘和加速度计校正方法 [打印本页]
作者: XFZ    时间: 2017-11-7 10:08
标题: 超详细讲解：罗盘和加速度计校正方法
罗盘和加计的校准是日常开发中最基础的工作，特邀Echo老师对罗盘和加速度计校准的工程方法进行总结，为小伙伴你们解惑，是有此文。

作者信息
Echo，本名邹佳池，从事嵌入式软件开发。
超详细讲解：罗盘和加速度计校正方法
(附C源代码)
1.为什么要校正我们都知道，罗盘是测量周围的磁场强度，若不存在外在磁场的干扰，只存在地磁的话，理论上罗盘旋转测得的磁场是一个圆球。
可是现实空间中，除了地磁场外，还存在其他的磁场干扰，这里我索性将它分为两大类。
第一类：地球空间中的磁场，这类磁场有个特点，就是随着罗盘坐标系的转动，磁场方向不变，类似地磁场。
第二类：罗盘坐标空间中的磁场，这类磁场源一般是固定在飞机上的，所以随着罗盘坐标系的转动，磁场方向也跟着转动。但是对于罗盘坐标系来说，却是一个恒定值。
对于第一类的磁场，目前我了解到的还没有什么好的方式可以进行校正（如果哪位大神知道，还请告知）。而我后面要介绍的校正方法，即是滤除第二类磁场的干扰，校正的思想即是基于最小二乘法的椭球拟合算法。
注：这个只能校正磁场强度固定不变的磁场，而对于电机这种变化的磁场，我没有测验过，不知道电机产生磁场的强度大小跟电机转速的关系怎样，如果谁有研究过的，还请告知，谢谢。
2.椭球拟合校正理论推导网络上有许多关于椭球拟合校正的论文，我都没有细看，因为那些公式都写得晦涩难懂，没有那个耐心，我这里尽量用最简洁的语言介绍校正方法的理论基础。
首先建立数值模型，设测量值为：


,校正后的值为：，


平移参数为：


缩放参数为：



他们之间的关系如下所示：


我们校正后的目标就是使得校正值近似分布在一个圆球上，而圆球的公式大家都知道：x2+y2+z2=R2，故我们将校正后的值带入圆球公式，与理论的圆球半径平方做差，构建误差u：


将校正值用测量值替换，变为：


可以看到，这分明就是个椭圆公式嘛~
记：


则我们的误差u可以写为如下形式：


下面就是校正的核心思想了：假设我们有许多组数据，我们要求得一组参数，使得所有数据的误差和最小，即∑u最小，但是由于u有正有负，所以符号相反的误差有可能相互抵消。那么加绝对值呢？这个也不可取，因为对绝对值函数求极小值十分复杂。那么我们自然就想到对u求平方和，即：
U=∑u^2
我们把u看成一个未知数，这个函数是一个二次函数，其有极小值点。而为了求得这个极小值点，我们对其做偏导即可：


记：


则我们可以将偏导写成如下形式：


B是已知的，P是我们待求的参数矩阵，故可以通过求齐次线性方程组，来求得P的各个参数的解。
齐次线性方程组求解的过程我这里就不详细解释了，我算法中使用的方法是经典的高斯消元法，有兴趣的可以仔细看看。
当我们求得P的各个未知数a,b,c,d,e,f,g后，需要通过这几个参数反求出我们的偏移量(ox,oy,oz)和缩放量(gx,gy,gz)。
在反求解之前，我们先回到上一个式子，BxP=0。其实满足这个式子的解P有无数组，我们可以将式子改写成BxCP=0，C是一个任意常数，即


我们通过解线性方程组求得的只是这个解系中的一个基本解，所以我们首先要求出这个基本解的C。
其算式经推导如下,带入a,b,c,d,e,f,g即可求解：
C=(d2/a + e2/b + f2/c - 4g)/4R2  (R为理论圆球半径)
ox=d/2a
oy=e/2b
oz=f/2c
gx=sqrt(a/C)
gy=sqrt(b/C)
gz=sqrt(c/C)
最后，将这六个参数回调到之前的式子中去，即完成校正。
3.校正程序源码(C语言)代码太长了 就不贴出来了，放在网盘种大家下载
罗盘与加计校准方法 C源代码


单片机源程序如下:
  1. #include "stdafx.h"
  2. #include "string.h"
  3. #include "math.h"

  5. #define MATRIX_SIZE 7
  6. #define u8 unsigned char

  8. double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
  9. int m = MATRIX_SIZE;       
  10. int n = MATRIX_SIZE+1;
  11. double m_result[MATRIX_SIZE];       

  13. void DispMatrix(void);

  15. double Abs(double a)
  16. {
  17.         return a<0 ? -a : a;
  18. }

  20. u8 Equal(double a,double b)
  21. {
  22.         return Abs(a-b) < 1e-6;
  23. }

  25. void ResetMatrix(void)
  26. {
  27.         int row , column;
  28.        
  29.         for(row = 0 ; row<m ; row++){
  30.                 for(column = 0 ; column<n ; column++)
  31.                         m_matrix[row][column] = 0.0f;
  32.         }
  33. }
  34.        
  35. void CalcData_Input(double x , double y , double z)
  36. {
  37.         double V[MATRIX_SIZE];
  38.         int row , column;
  39.        
  40.         V[0] = x*x;
  41.     V[1] = y*y;
  42.     V[2] = z*z;
  43.     V[3] = x;
  44.     V[4] = y;
  45.     V[5] = z;
  46.     V[6] = 1.0;
  47.        
  48.         //构建VxVt矩阵(Vt为V的转置)，并进行累加
  49.         for(row = 0 ; row<MATRIX_SIZE ; row++){
  50.                 for(column = 0 ; column<MATRIX_SIZE ; column++){
  51.                         m_matrix[row][column] += V[row]*V[column];
  52.                 }
  53.         }
  54. }

  56. void SwapRow(int row1 , int row2)
  57. {
  58.         int column;
  59.         double tmp;
  60.        
  61.         for(column = 0 ; column<n ; column++){
  62.                 tmp = m_matrix[row1][column];
  63.                 m_matrix[row1][column] = m_matrix[row2][column];
  64.                 m_matrix[row2][column] = tmp;
  65.         }
  66. }

  68. void MoveBiggestElement2Top(int s_row , int s_column)
  69. {
  70.         int row,column;
  71.        
  72.         for(row = s_row+1 ; row<m ; row++){
  73.                 if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
  74.                         SwapRow(s_row , row);
  75.                 }
  76.         }
  77. }

  79. //高斯消元法，求行阶梯型矩阵
  80. u8 Matrix_GaussElimination(void)
  81. {
  82.         int row,column,i,j;
  83.         double tmp;
  84.        
  85.         for(row = 0,column=0 ; row<m-1 && column<n-1 ; row++,column++){
  86.                 //将当前列最大的一行移上来
  87.                 MoveBiggestElement2Top(row , column);
  88.                
  89.                 //整列都为0
  90.                 if(Equal(m_matrix[row][column],0.0f)){
  91.                         printf("qiyi matrix:%d %d\r\n" , row , column);
  92.                         //DispMatrix();
  93.                         //return 0;
  94.                         row--;
  95.                         continue;
  96.                 }
  97.                
  98.                 //高斯消元
  99.                 for(i = row+1 ; i<m ; i++){       
  100.                         if(Equal(m_matrix[i][column],0.0f))
  101.                                 continue;        //为0，无需处理
  102.                        
  103.                         tmp = m_matrix[i][column]/m_matrix[row][column];
  104.                        
  105.                         for(j = column ; j<n ; j++){
  106.                                 m_matrix[i][j] -= m_matrix[row][j]*tmp;
  107.                         }
  108.                 }

  110.                 DispMatrix();
  111.                 printf("\r\n");
  112.         }

  114.         return 1;
  115. }

  117. //求行最简型矩阵
  118. int Matrix_RowSimplify(void)
  119. {
  120.     int c = n;//返回值，表示(解的任意常量数+1)；
  121.     //
  122.     int row,column,k,s,t;
  123.     double tmp;
  124.     //
  125.     for(row=0,column=0;row<m && column<n;row++,column++)
  126.     {
  127.         if(Equal(m_matrix[row][column],0))//平移，找出本行第一个非零；
  128.         {
  129.             row--;
  130.             continue;
  131.         }
  132.         //
  133.         c--;//少一个常量；
  134.         //
  135.         //化a[i][j]为1；
  136.         tmp = 1 / m_matrix[row][column];
  137.         for(k=column;k<n;k++)//前面的"0"就不处理了；
  138.             m_matrix[row][k] *= tmp;
  139.         //
  140.         //化a[s][j]为0
  141.         for(s=0;s<row;s++)//下面的0也不用处理；
  142.         {
  143.             if(Equal(m_matrix[s][column],0))
  144.                 continue;//已经为0；
  145.             //
  146.             tmp = m_matrix[s][column] / m_matrix[row][column];
  147.             for(t=column;t<n;t++)
  148.                 m_matrix[s][t] -= m_matrix[row][t]*tmp;
  149.             //
  150.         }
  151.     }
  152.     //
  153.     return c;
  154. }

  156. void Matrix_Solve(double* C , double* sol)
  157. {
  158.         int row,column,i;
  159.         int any_sol[MATRIX_SIZE];

  161.         //找出任意解的位置
  162.         memset(any_sol , 0 , MATRIX_SIZE);
  163.         for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
  164.                 if(Equal(m_matrix[row][column] , 0.0f)){
  165.                         any_sol[column] = 1;        //记录任意解的位置
  166.                         row--;        //右移1列
  167.                 }
  168.         }

  170.         //求解
  171.         row = 0;
  172.         for(column = 0 ; column<n-1 ; column++){
  173.                 if(any_sol[column] == 1){        //任意解
  174.                         sol[column] = C[column];
  175.                 }else{
  176.                         sol[column] = m_matrix[row][n-1];
  177.                         //加上任意解
  178.                         for(i = column+1 ; i<n-1 ; i++){
  179.                                 if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
  180.                                         sol[column] -= m_matrix[row][i]*C[i];
  181.                                 }
  182.                         }       
  183.                         row++;
  184.                 }
  185.         }
  186. }

  188. void DispMatrix(void)
  189. {
  190.         int row,column;
  191.        
  192.         for(row = 0 ; row<m ; row++){
  193.                 for(column = 0 ; column<n ; column++){
  194.                         printf("%.3f        " , m_matrix[row][column]);
  195.                 }
  196.                 printf("\r\n");
  197.         }
  198. }

  200. void Calc_Process(double radius)
  201. {
  202.         double C[MATRIX_SIZE];
  203.         double Res[MATRIX_SIZE];
  204.         int i;
  205.         double k;

  207.         ResetMatrix();

  209.         //输入任意个数磁场测量点坐标，请尽量保证在椭球上分布均匀
  210.         CalcData_Input(7 , -7 , -2);
  211.         CalcData_Input(-1 , -7 , -2);
  212.         CalcData_Input(3 , 3 , -2);
  213.         CalcData_Input(3 , -17 , -2);
  214.         CalcData_Input(3 , -7 , 4);
  215.         CalcData_Input(3 , -7 , -8);

  217.         Matrix_GaussElimination();
  218.         Matrix_RowSimplify();

  220.     //赋值任意解参数值
  221. ……………………

  223. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
Calibration.rar (2 KB, 下载次数: 143)




作者: admin    时间: 2017-11-9 04:12
好资料，51黑有你更精彩!!!
作者: xinzaizai    时间: 2017-11-24 16:50

好资料，帮了大忙
作者: my51hei-huashen    时间: 2018-2-12 11:16
非常感谢！
作者: iyzyh    时间: 2018-2-22 15:22
谢谢分享！刚刚好需要！
作者: essien    时间: 2018-3-10 09:35
学习一下
作者: essien    时间: 2018-3-10 09:36
谢谢分享！刚刚好需要，学习一下
作者: KELIXIER    时间: 2018-4-30 20:27
好东西
作者: KELIXIER    时间: 2018-4-30 20:28
谢谢分享！刚刚好需要，学习一下
作者: 摩天轮1111    时间: 2018-5-23 16:41
有点骗积分的感觉啊，我这里全贴出来
  1. // 本程序对三维散点数据进行椭球拟合
  2. // 编 写 人：邹佳池
  3. // 编写日期：2016-5-25
  4. // 技术交流QQ:529380360
  5. // 请尊重作者的劳动成果，转载请注明出处，谢谢
  6. #include "stdio.h"
  7. #include "string.h"
  8. #include "math.h"
  9.   
  10. #define MATRIX_SIZE 7
  11. #define u8 unsigned char
  12.   
  13. double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
  14. int m = MATRIX_SIZE;        
  15. int n = MATRIX_SIZE+1;
  16. double m_result[MATRIX_SIZE];        
  17.   
  18. void DispMatrix(void);
  19.   
  20. double Abs(double a)
  21. {
  22.         return a<0 ? -a : a;
  23. }
  24.   
  25. u8 Equal(double a,double b)
  26. {
  27.         return Abs(a-b) < 1e-6;
  28. }
  29.   
  30. void ResetMatrix(void)
  31. {
  32.         int row , column;
  33.          
  34.         for(row = 0 ; row<m ; row++){
  35.                 for(column = 0 ; column<n ; column++)
  36.                         m_matrix[row][column] = 0.0f;
  37.         }
  38. }
  39.          
  40. void CalcData_Input(double x , double y , double z)
  41. {
  42.         double V[MATRIX_SIZE];
  43.         int row , column;
  44.          
  45.         V[0] = x*x;
  46.     V[1] = y*y;
  47.     V[2] = z*z;
  48.     V[3] = x;
  49.     V[4] = y;
  50.     V[5] = z;
  51.     V[6] = 1.0;
  52.          
  53.         //构建VxVt矩阵(Vt为V的转置)，并进行累加
  54.         for(row = 0 ; row<MATRIX_SIZE ; row++){
  55.                 for(column = 0 ; column<MATRIX_SIZE ; column++){
  56.                         m_matrix[row][column] += V[row]*V[column];
  57.                 }
  58.         }
  59. }
  60.   
  61. void SwapRow(int row1 , int row2)
  62. {
  63.         int column;
  64.         double tmp;
  65.          
  66.         for(column = 0 ; column<n ; column++){
  67.                 tmp = m_matrix[row1][column];
  68.                 m_matrix[row1][column] = m_matrix[row2][column];
  69.                 m_matrix[row2][column] = tmp;
  70.         }
  71. }
  72.   
  73. void MoveBiggestElement2Top(int s_row , int s_column)
  74. {
  75.         int row,column;
  76.          
  77.         for(row = s_row+1 ; row<m ; row++){
  78.                 if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
  79.                         SwapRow(s_row , row);
  80.                 }
  81.         }
  82. }
  83.   
  84. //高斯消元法，求行阶梯型矩阵
  85. u8 Matrix_GaussElimination(void)
  86. {
  87.         int row,column,i,j;
  88.         double tmp;
  89.          
  90.         for(row = 0,column=0 ; row<m-1 && column<n-1 ; row++,column++){
  91.                 //将当前列最大的一行移上来
  92.                 MoveBiggestElement2Top(row , column);
  93.                   
  94.                 //整列都为0
  95.                 if(Equal(m_matrix[row][column],0.0f)){
  96.                         printf("qiyi matrix:%d %d\r\n" , row , column);
  97.                         //DispMatrix();
  98.                         //return 0;
  99.                         row--;
  100.                         continue;
  101.                 }
  102.                   
  103.                 //高斯消元
  104.                 for(i = row+1 ; i<m ; i++){        
  105.                         if(Equal(m_matrix[i][column],0.0f))
  106.                                 continue;        //为0，无需处理
  107.                           
  108.                         tmp = m_matrix[i][column]/m_matrix[row][column];
  109.                           
  110.                         for(j = column ; j<n ; j++){
  111.                                 m_matrix[i][j] -= m_matrix[row][j]*tmp;
  112.                         }
  113.                 }
  114.   
  115.                 DispMatrix();
  116.                 printf("\r\n");
  117.         }
  118.   
  119.         return 1;
  120. }
  121.   
  122. //求行最简型矩阵
  123. int Matrix_RowSimplify(void)
  124. {
  125.     int c = n;//返回值，表示(解的任意常量数+1)；
  126.     //
  127.     int row,column,k,s,t;
  128.     float tmp;
  129.     //
  130.     for(row=0,column=0;row<m && column<n;row++,column++)
  131.     {
  132.         if(Equal(m_matrix[row][column],0))//平移，找出本行第一个非零；
  133.         {
  134.             row--;
  135.             continue;
  136.         }
  137.         //
  138.         c--;//少一个常量；
  139.         //
  140.         //这里不化成对角矩阵为1的矩阵，为了防止输入的数据较大的时候，求出的解为接近于0值的情况
  141.         //tmp = 1 / m_matrix[row][column];
  142.         //for(k=column;k<n;k++)//前面的"0"就不处理了；
  143.             //m_matrix[row][k] *= tmp;
  144.         //

  146.         //化上三角矩阵为对角矩阵[/i][/i][/i]
  147. [i][i][i]       if(row == m-1)
  148.                         m_matrix[row][column] = 0.0f;        //强制为0，释放一个自由度，否则很难有解

  150.         for(s=0;s<row;s++)//下面的0也不用处理；
  151.         {
  152.             if(Equal(m_matrix[s][column],0))
  153.                 continue;//已经为0；
  154.             //
  155.             tmp = m_matrix[s][column] / m_matrix[row][column];
  156.             for(t=column;t<n;t++)
  157.                 m_matrix[s][t] -= m_matrix[row][t]*tmp;
  158.             //
  159.         }
  160.     }

  162.         DispMatrix();
  163.     printf("\r\n");
  164.     //
  165.     return c;
  166. }
  167.   
  168. void Matrix_Solve(double* C , double* sol)
  169. {
  170.         int row,column,i;
  171.         int any_sol[MATRIX_SIZE];
  172.   
  173.         //找出任意解的位置
  174.         memset(any_sol , 0 , MATRIX_SIZE);
  175.         for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
  176.                 if(Equal(m_matrix[row][column] , 0.0f)){
  177.                         any_sol[column] = 1;        //记录任意解的位置
  178.                         row--;        //右移1列
  179.                 }
  180.         }
  181.   
  182.         //求解
  183.         row = 0;
  184.         for(column = 0 ; column<n-1 ; column++){
  185.                 if(any_sol[column] == 1){        //任意解
  186.                         sol[column] = C[column];
  187.                 }else{
  188.                         sol[column] = m_matrix[row][n-1];
  189.                         //加上任意解
  190.                         for(i = column+1 ; i<n-1 ; i++){
  191.                                 if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
  192.                                         sol[column] -= m_matrix[row][i]*C[i];
  193.                                 }
  194.                         }

  196.                                                 sol[column] /= m_matrix[row][column];        //除以对角线元素
  197.                         
  198.                                                 row++;
  199.                 }
  200.         }
  201. }
  202.   
  203. void DispMatrix(void)
  204. {
  205.         int row,column;
  206.          
  207.         for(row = 0 ; row<m ; row++){
  208.                 for(column = 0 ; column<n ; column++){
  209.                         printf("%.3f        " , m_matrix[row][column]);
  210.                 }
  211.                 printf("\r\n");
  212.         }
  213. }
  214.   
  215. void Calc_Process(double radius)
  216. {
  217.         double C[MATRIX_SIZE];
  218.         double Res[MATRIX_SIZE];
  219.         int i;
  220.         double k;
  221.   
  222.         ResetMatrix();
  223.   
  224.         //这里输入任意个点的罗盘(加速度)参数，尽量在球面上均匀分布(可使用APM的六面采集法采集)
  225.                 CalcData_Input(7 , -7 , -2);
  226.         CalcData_Input(-1 , -7 , -2);
  227.         CalcData_Input(3 , 3 , -2);
  228.         CalcData_Input(3 , -17 , -2);
  229.         CalcData_Input(3 , -7 , 4);
  230.         CalcData_Input(3 , -7 , -8);
  231.   
  232.         Matrix_GaussElimination();
  233.         Matrix_RowSimplify();
  234.   
  235.         for(i = 0 ; i<MATRIX_SIZE ; i++){
  236.                 C[i] = 1000.0f;
  237.         }
  238.   
  239.         Matrix_Solve(C , Res);
  240.   
  241.         printf("a:%.2f b:%.2f c:%.2f d:%.2f e:%.2f f:%.2f g:%.2f\r\n" , Res[0],Res[1],Res[2],Res[3],Res[4],Res[5],Res[6]);
  242.   
  243.         k = (Res[3]*Res[3]/Res[0]+Res[4]*Res[4]/Res[1]+Res[5]*Res[5]/Res[2] - 4*Res[6])/(4*radius*radius);
  244.   
  245.         m_result[0] = sqrt(Res[0] / k);
  246.                 m_result[1] = sqrt(Res[1] / k);
  247.                 m_result[2] = sqrt(Res[2] / k);
  248.                 m_result[3] = Res[3] / (2 * Res[0]);
  249.                 m_result[4] = Res[4] / (2 * Res[1]);
  250.                 m_result[5] = Res[5] / (2 * Res[2]);
  251.   
  252.         printf("Xo:%f Yo:%f Zo:%f Xg:%f Yg:%f Zg:%f C:%f\r\n" , m_result[3],m_result[4],m_result[5],m_result[0],m_result[1],m_result[2],k);
  253.         while(1);
  254. }
  255.   
  256.   
  257. int main(int argc, char* argv[])
  258. {
  259.         Calc_Process(2.0);
  260.         return 0;
  261. }
复制代码
作者: nero0335    时间: 2019-3-6 10:02
楼主您好，附件里的代码“Calibration.rar ”不是文章里讲的代码，“罗盘与加计校准方法”这个从哪里下载呢？
作者: nero0335    时间: 2019-3-6 10:15
摩天轮1111 发表于 2018-5-23 16:41
有点骗积分的感觉啊，我这里全贴出来

兄弟，谢谢！！！！！
作者: sf116    时间: 2019-3-29 11:52
谢谢分享！刚刚好需要！
作者: yp9770    时间: 2019-5-24 08:31

谢谢分享！刚刚好需要！
作者: liuzbl    时间: 2019-7-8 08:34
图片怎么打不开呀？
作者: peakzuo    时间: 2020-5-26 10:44
图片看不到啊。
作者: skychilang    时间: 2021-3-17 22:41
進來求知!雖看不太懂!但還是學習了
作者: sczhangxx    时间: 2021-5-4 17:40
        //输入任意个数磁场测量点坐标，请尽量保证在椭球上分布均匀
        CalcData_Input(7 , -7 , -2);
        CalcData_Input(-1 , -7 , -2);
        CalcData_Input(3 , 3 , -2);
        CalcData_Input(3 , -17 , -2);
        CalcData_Input(3 , -7 , 4);
        CalcData_Input(3 , -7 , -8);

请教下，这是输入的坐标还是磁力计采集数据呀，如果是坐标，磁力数据哪里输入的呢
作者: suzhao2008    时间: 2023-2-23 19:37

進來求知!雖看不太懂!但還是學習了
作者: chenyucy72    时间: 2023-3-13 10:25
太强大了，虽然现在看不明白，感谢老师
作者: 兀自清凉    时间: 2023-9-12 15:05
图片看不到了，有啥办法可以看吗，压缩包里面也没有讲解的内容，只有代码了



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1