从
MPU6050
读取的数据包括三½陀螺仪三½加速度，
陀螺仪校正可以
用陀螺仪测量值减去前
200
次数据的平均值就可以得到较½的效果。而加
速度的值采用这种办法在微型四½上也是可以的，½是对加速度和磁力计
的校正常用的是椭球拟合的办法来实现。
加速的计测量的对象是比力，也就是等效重力加速的和运动加速度的
和，½静止时，运动加速度为
0，加速度测量值为等效重力加速度，可以利
用这一点校正加速度计。主要思路为：
对测量值平移和缩放，把测量值拟合到重力加速度。因此校正的任务
为：
寻找最½的缩放和平移参数，½总½测量数据更靠近重力加
速度。
设
G
0
为重力加速度，
x
m
y
m
z
m
]为实际加速度分量，
[
理想情况下有下列等式：
x
m
y
m
z
m
½
G
0
2
2
2
2
（1-1）
设测量值为[
x
c
y
c
z
c
],则测量值和实际值可以通过下面的公式拟合，拟合
为线性拟合，其中
a,b,c
比例因子,d,e,f 为偏移因子。通过这个公式就可以把测量
值变为实际值。标定加速度计的目标就是确定[a
b c d e f]这 6
个参数，½测量值
更接近实际值。
x
m
½
a
x
c
d
y
m
½
b
y
c
e
z
m
½
c
z
c
f
（1-2）
将此公式带入理想情况的等式：
2
2
2
（
a
x
c
d
）
b
y
c
e
）
c
z
c
f
）
½
G
0
（
（
2
（1-3）
展开得：
a
2
x
c
b
2
y
c
c
2
z
c
2
a
d
x
c
2
b
e
y
c
2
c
f
x
c
d
2
e
2
f
2
G
0
½
0
可以
简化成如下½式：
2
2
2
2
a
x
c
b
y
c
c
z
c
d
x
c
e
y
c
f
c
g
½
0
z
2
2
2
比较以上两式有：
ka
½
a
2
kb
½
b
2
kc
½
c
2
kd
½
2
ad
ke
½
2
be
（需要加上系数
K）
kf
½
2
cf
2
2
2
2
kg
½
d
e
f
G
0
反过来有：
a
½
ka
b
½
kb
c
½
kc
kd
d
½
2
ka
ke
e
½
2
kb
kf
f
½
2
kc
4
G
0
k
½
2
d
e
2
f
2
4
g
a
b
c
2
在静止状态时测得一些数据，记为
[
x
n
,
y
n
,
z
n
]
，表示第
n
组测量值，对应
的实际值½是重力
G0。为了衡量参数的合适度，引入误差½数
n
，表示第
n
个估计值与实际值得偏差。
n
½
a
x
n
b
y
n
c
z
n
d
x
n
e
y
n
f z
n
g
2
2
2
n
是第
n
组数据的偏差，我们的目标是½整½偏差最小，所以引入整
½偏差指数
I，等于全部数据的偏差得平方和。
I
½
n
2
为了到达我们校正的任务，即½偏差平方和
I
最小。用
I
对
[
a
,
b
,
c
,
d
,
e
,
f
,
g
]
求偏导，½偏导为
0，这是 I
最小的必要条件。于是
得到
7
个线性方程，组合起来就是一组
7*7
½次线性方程组。
I
2
½
(2
n
x
n
)
½
0
a
I
½
(2
n
y
n
2
)
½
0
b
I
½
(2
n
z
2
)
½
0
n
c
I
½
(2
n
x
n
)
½
0
d
I
½
(2
n
y
n
)
½
0
e
I
½
(2
n
z
)
½
0
n
f
I
½
(2
n
1)
½
0
g
可以用向量的½式化简上式：
设：
V
½
[
a
,
b
,
c
,
d
,
e
,
f
,
g
]
P
½
[
x
n
,
y
n
,
z
n
,
x
n
,
y
n
,
z
n
,1]
T
n
½
V
P
根据偏导公式可以得出：
2
2
2
n
P
½
0
(
V
P
)
P
½
0
(
P
P
)
V
½
0
T
那么接下来就是解线性½次方程组了，可以用经典的高斯消元法。然
而线性½次方程组的解一般为
0
解。所以可以这样解：
½略掉最后一行，另
g=1
可以得到一组近似解。
高斯消元法代码为：
(该方法可以将 7*7
矩阵消成对角矩阵)
#define
M
#define N
#define L
7
7
//列数
//行数
N-1
//行数-1
//高斯消元法求解非线性方程
void gauss(double a[N][M])
{
int
i,j,l,n,m,k=0;
double
do{
n=0;
for(l=k;l<L;l++)
temp[n++]=a[l+1][k]/a[k][k];
for(m=0,i=k;i<L;i++,m++)
for(j=k;j<M;j++)
a[i+1][j]-=temp[m]*a[k][j];
k++;
}while(k<L) ;
temp[N];
/*第一个 do-while
是将增广矩阵消成上三角½式*/
/*第二个 do-while
是将矩阵消成对角½式，并且重新给
k
赋值*/
k=L-1;
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<M;j++)
a[k-i][j]-=temp[m]*a[k+1][j];
k--;
}while(k>=0) ;
就可以得到对角矩阵
}
//得到校正参数的值
void correct_canshu(double
{
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],double par[6])
//x[N]为线性方程求出的接
//par[5]为求得的一组 a,b,c,d,e,f
参数
京公网安备 11010802033920号
Copyright © 2005-2024 EEWORLD.com.cn, Inc. All rights reserved
评论