单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 4052|回复: 9
打印 上一主题 下一主题
收起左侧

超详细讲解:罗盘和加速度计校正方法

[复制链接]
跳转到指定楼层
楼主
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"

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

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

  10. void DispMatrix(void);

  11. double Abs(double a)
  12. {
  13.         return a<0 ? -a : a;
  14. }

  15. u8 Equal(double a,double b)
  16. {
  17.         return Abs(a-b) < 1e-6;
  18. }

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

  49. void SwapRow(int row1 , int row2)
  50. {
  51.         int column;
  52.         double tmp;
  53.        
  54.         for(column = 0 ; column<n ; column++){
  55.                 tmp = m_matrix[row1][column];
  56.                 m_matrix[row1][column] = m_matrix[row2][column];
  57.                 m_matrix[row2][column] = tmp;
  58.         }
  59. }

  60. void MoveBiggestElement2Top(int s_row , int s_column)
  61. {
  62.         int row,column;
  63.        
  64.         for(row = s_row+1 ; row<m ; row++){
  65.                 if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
  66.                         SwapRow(s_row , row);
  67.                 }
  68.         }
  69. }

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

  100.                 DispMatrix();
  101.                 printf("\r\n");
  102.         }

  103.         return 1;
  104. }

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

  143. void Matrix_Solve(double* C , double* sol)
  144. {
  145.         int row,column,i;
  146.         int any_sol[MATRIX_SIZE];

  147.         //找出任意解的位置
  148.         memset(any_sol , 0 , MATRIX_SIZE);
  149.         for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
  150.                 if(Equal(m_matrix[row][column] , 0.0f)){
  151.                         any_sol[column] = 1;        //记录任意解的位置
  152.                         row--;        //右移1列
  153.                 }
  154.         }

  155.         //求解
  156.         row = 0;
  157.         for(column = 0 ; column<n-1 ; column++){
  158.                 if(any_sol[column] == 1){        //任意解
  159.                         sol[column] = C[column];
  160.                 }else{
  161.                         sol[column] = m_matrix[row][n-1];
  162.                         //加上任意解
  163.                         for(i = column+1 ; i<n-1 ; i++){
  164.                                 if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
  165.                                         sol[column] -= m_matrix[row][i]*C[i];
  166.                                 }
  167.                         }       
  168.                         row++;
  169.                 }
  170.         }
  171. }

  172. void DispMatrix(void)
  173. {
  174.         int row,column;
  175.        
  176.         for(row = 0 ; row<m ; row++){
  177.                 for(column = 0 ; column<n ; column++){
  178.                         printf("%.3f        " , m_matrix[row][column]);
  179.                 }
  180.                 printf("\r\n");
  181.         }
  182. }

  183. void Calc_Process(double radius)
  184. {
  185.         double C[MATRIX_SIZE];
  186.         double Res[MATRIX_SIZE];
  187.         int i;
  188.         double k;

  189.         ResetMatrix();

  190.         //输入任意个数磁场测量点坐标,请尽量保证在椭球上分布均匀
  191.         CalcData_Input(7 , -7 , -2);
  192.         CalcData_Input(-1 , -7 , -2);
  193.         CalcData_Input(3 , 3 , -2);
  194.         CalcData_Input(3 , -17 , -2);
  195.         CalcData_Input(3 , -7 , 4);
  196.         CalcData_Input(3 , -7 , -8);

  197.         Matrix_GaussElimination();
  198.         Matrix_RowSimplify();

  199.     //赋值任意解参数值
  200. ……………………

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

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




评分

参与人数 1黑币 +1 收起 理由
zycwswws + 1 很给力!

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏1 转播转播 分享分享 分享淘帖 顶 踩
回复

使用道具 举报

沙发
admin 发表于 2017-11-9 04:12 | 只看该作者
好资料,51黑有你更精彩!!!
回复

使用道具 举报

板凳
xinzaizai 发表于 2017-11-24 16:50 | 只看该作者

好资料,帮了大忙
回复

使用道具 举报

无效楼层,该帖已经被删除
5#
my51hei-huashen 发表于 2018-2-12 11:16 | 只看该作者
非常感谢!
回复

使用道具 举报

6#
iyzyh 发表于 2018-2-22 15:22 | 只看该作者
谢谢分享!刚刚好需要!
回复

使用道具 举报

7#
essien 发表于 2018-3-10 09:35 | 只看该作者
学习一下
回复

使用道具 举报

8#
essien 发表于 2018-3-10 09:36 | 只看该作者
谢谢分享!刚刚好需要,学习一下
回复

使用道具 举报

9#
KELIXIER 发表于 2018-4-30 20:27 | 只看该作者
好东西
回复

使用道具 举报

10#
KELIXIER 发表于 2018-4-30 20:28 | 只看该作者
谢谢分享!刚刚好需要,学习一下
回复

使用道具 举报

11#
摩天轮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.         //

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

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

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

  192.                                                 sol[column] /= m_matrix[row][column];        //除以对角线元素
  193.                         
  194.                                                 row++;
  195.                 }
  196.         }
  197. }
  198.   
  199. void DispMatrix(void)
  200. {
  201.         int row,column;
  202.          
  203.         for(row = 0 ; row<m ; row++){
  204.                 for(column = 0 ; column<n ; column++){
  205.                         printf("%.3f        " , m_matrix[row][column]);
  206.                 }
  207.                 printf("\r\n");
  208.         }
  209. }
  210.   
  211. void Calc_Process(double radius)
  212. {
  213.         double C[MATRIX_SIZE];
  214.         double Res[MATRIX_SIZE];
  215.         int i;
  216.         double k;
  217.   
  218.         ResetMatrix();
  219.   
  220.         //这里输入任意个点的罗盘(加速度)参数,尽量在球面上均匀分布(可使用APM的六面采集法采集)
  221.                 CalcData_Input(7 , -7 , -2);
  222.         CalcData_Input(-1 , -7 , -2);
  223.         CalcData_Input(3 , 3 , -2);
  224.         CalcData_Input(3 , -17 , -2);
  225.         CalcData_Input(3 , -7 , 4);
  226.         CalcData_Input(3 , -7 , -8);
  227.   
  228.         Matrix_GaussElimination();
  229.         Matrix_RowSimplify();
  230.   
  231.         for(i = 0 ; i<MATRIX_SIZE ; i++){
  232.                 C[i] = 1000.0f;
  233.         }
  234.   
  235.         Matrix_Solve(C , Res);
  236.   
  237.         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]);
  238.   
  239.         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);
  240.   
  241.         m_result[0] = sqrt(Res[0] / k);
  242.                 m_result[1] = sqrt(Res[1] / k);
  243.                 m_result[2] = sqrt(Res[2] / k);
  244.                 m_result[3] = Res[3] / (2 * Res[0]);
  245.                 m_result[4] = Res[4] / (2 * Res[1]);
  246.                 m_result[5] = Res[5] / (2 * Res[2]);
  247.   
  248.         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);
  249.         while(1);
  250. }
  251.   
  252.   
  253. int main(int argc, char* argv[])
  254. {
  255.         Calc_Process(2.0);
  256.         return 0;
  257. }
复制代码

评分

参与人数 1黑币 +50 收起 理由
admin + 50 回帖助人的奖励!

查看全部评分

回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

QQ|手机版|小黑屋|单片机论坛 |51黑电子论坛单片机.

Powered by 单片机教程网

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