首页资源分类嵌入式系统 > 加速度标定

加速度标定

已有 445117个资源

下载专区

上传者其他资源

    文档信息举报收藏

    标    签:最小二乘法

    分    享:

    文档简介

    椭圆拟合

    最小二乘法

    文档预览

    从 MPU6050 读取的数据包括三轴陀螺仪三轴加速度,陀螺仪校正可以 用陀螺仪测量值减去前 200 次数据的平均值就可以得到较好的效果。而加 速度的值采用这种办法在微型四轴上也是可以的,但是对加速度和磁力计 的校正常用的是椭球拟合的办法来实现。 加速的计测量的对象是比力,也就是等效重力加速的和运动加速度的 和,当静止时,运动加速度为 0,加速度测量值为等效重力加速度,可以利 用这一点校正加速度计。主要思路为: 对测量值平移和缩放,把测量值拟合到重力加速度。因此校正的任务 为: 寻找最佳的缩放和平移参数,使总体测量数据更靠近重力加 速度。 设 G0 为重力加速度,[ xm ym zm ]为实际加速度分量,理想情况下有下列等式: xm2  ym2  zm2  G02 (1-1) 设测量值为[ xc yc zc ],则测量值和实际值可以通过下面的公式拟合,拟合 为线性拟合,其中 a,b,c 比例因子,d,e,f 为偏移因子。通过这个公式就可以把测量 值变为实际值。标定加速度计的目标就是确定[a b c d e f]这 6 个参数,使测量值 更接近实际值。 xm  a  xc  d ym  b  yc  e zm  c  zc  f (1-2) 将此公式带入理想情况的等式: (a  xc  d)2 (b  yc  e)2 (c  zc  f)2  G02 (1-3) 展开得: a2 xc2  b2 yc2  c2  zc2  2a  d  xc  2b  e  yc  2c  f  xc  d 2  e2  f 2  G02  0 可以 简化成如下形式: axc2  byc2  czc2  d xc  eyc  f zc  g  0 比较以上两式有: ka  a2 kb  b2 kc  c2 kd  2ad ke  2be kf   2cf (需要加上系数 K) kg  d 2  e2  f 2  G02 反过来有: a  ka b  kb c  kc d  kd 2 ka e  ke 2 kb f  kf  2 kc k  d 2  4G0 2 e2  f 2  4g a b c 在静止状态时测得一些数据,记为[xn, yn, zn ] ,表示第 n 组测量值,对应 的实际值都是重力 G0。为了衡量参数的合适度,引入误差函数 n ,表示第 n 个估计值与实际值得偏差。 n  axn2  byn2  czn2  d xn  eyn  f z n  g n 是第 n 组数据的偏差,我们的目标是使整体偏差最小,所以引入整 体偏差指数 I,等于全部数据的偏差得平方和。  I  n2 为了到达我们校正的任务,即使偏差平方和 I 最小。用 I 对 [a,b, c, d, e, f , g] 求偏导,使偏导为 0,这是 I 最小的必要条件。于是 得到 7 个线性方程,组合起来就是一组 7*7 齐次线性方程组。  I  aI    b (2n  xn2 )  0 (2n  yn2 )  0  I  cI  dI      e (2n  zn2 )  0 (2n  xn )  0 (2n  yn )  0  I f    (2n  zn )  0 gI   (2n 1)  0 可以用向量的形式化简上式: 设: V  [a,b, c, d, e, f , g] P  [xn2 , yn2 , zn2 , xn , yn , zn ,1]T n  V  P 根据偏导公式可以得出:  n  P  0  (V  P)  P  0  (P  T P ) V  0 那么接下来就是解线性齐次方程组了,可以用经典的高斯消元法。然 而线性齐次方程组的解一般为 0 解。所以可以这样解: 忽略掉最后一行,另 g=1 可以得到一组近似解。 高斯消元法代码为: (该方法可以将 7*7 矩阵消成对角矩阵) #define M 7 //列数 #define N 7 //行数 #define L N-1 //行数-1 //高斯消元法求解非线性方程 void gauss(double a[N][M]) { int i,j,l,n,m,k=0; double temp[N]; /*第一个 do-while 是将增广矩阵消成上三角形式*/ do{ n=0; for(l=k;l=0;l--) temp[n++]=a[k-l][k+1]/a[k+1][k+1]; for(m=0,i=k;i>=0;i--,m++) for(j=k;j=0) ; 就可以得到对角矩阵 } //得到校正参数的值 void correct_canshu(double x[N],double par[6]) { float c; c=4*G*G / (x[3]*x[3 ]/ x[0] + x[4]*x[4] / x[1]+x[5]*x[5] / x[2] - 4 * x[6]) ; par[0]=sqrtf(c*x[0]); par[1]=sqrtf(c*x[1]); par[2]=sqrtf(c*x[2]); par[3]=c*x[3]/(2.0*par[0]); par[4]=c*x[4]/(2.0*par[1]); par[5]=c*x[5]/(2.0*par[2]); } //x[N]为线性方程求出的接 //par[5]为求得的一组 a,b,c,d,e,f 参数

    Top_arrow
    回到顶部
    EEWORLD下载中心所有资源均来自网友分享,如有侵权,请发送举报邮件到客服邮箱bbs_service@eeworld.com.cn 或通过站内短信息或QQ:273568022联系管理员 高进,我们会尽快处理。