本文为博主“声时刻”原创文章，未经博主容许不得转载。html
问题 考虑到IMＵ中，x,y,z轴的度量单位并不相同，假设各轴之间相互直。git
那么加速度传感器在静止状态（也就是只受重力的状态下），各个姿态只受重力的，x,y,z轴值（假设x,y,z轴相互垂直而且度量单位都一致，如mpu9250三轴的度量单位都是2048，16g量程的状况下），在三维空间中，重力点都在一个球面上，但各轴之间的度量单位都会有误差，因此各姿态重力点都落在一个椭球面上，椭球的中心，就是加速度的偏移量，也就是校准值 。github
在磁力计上，因为测量磁场强度，在环境不变的状况下，传感器每一个姿态感觉磁场强度是相同，因此不须要静止状态，磁力计测量的x,y,z轴值，在没有误差的状况下，在传感器内部x,y,z轴相互垂直的状况下，在三维空间中组成一个圆球面。可是磁力计存在Hard Iron Distortion和Soft Iron Distortion。使得x,y,z轴度量单位不相同，各轴也并不是相互垂直，（说明一下，任意椭球的三个轴都是相互垂直的，几何上，椭球最长的轴与最短的轴相互垂直，从代数的角度看，对称正定矩阵
A
=
R
′
B
R
A=R'BR
A = R ′ B R
R
R
R
R
′
R
=
I
R'R=I
R ′ R = I 磁通量在三维空间组成的椭球球心，是磁力计的校准值的一部分 。web
数学模型 因此问题在于给定椭球球面上的点，如何求椭球球心。其实就是一个椭球拟合问题 。算法
a
1
x
2
+
a
2
y
2
+
a
3
z
2
+
a
4
x
y
+
a
5
x
z
+
a
6
y
z
+
a
7
x
+
a
8
y
+
a
9
z
=
1
(1)
a_1x^2+a_2y^2+a_3z^2+a_4xy+a_5xz+a_6yz+a_7x+a_8y+a_9z=1 \tag{1}
a 1 x 2 + a 2 y 2 + a 3 z 2 + a 4 x y + a 5 x z + a 6 y z + a 7 x + a 8 y + a 9 z = 1 ( 1 ) app
从几何的角度表示上式的椭球为ide
[
x
−
c
x
y
−
c
y
z
−
c
z
]
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
T
[
λ
1
0
0
0
λ
2
0
0
0
λ
3
]
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
[
x
−
c
x
y
−
c
y
z
−
c
z
]
=
1
+
[
c
x
c
y
c
z
]
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
T
[
λ
1
0
0
0
λ
2
0
0
0
λ
3
]
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
[
c
x
c
y
c
z
]
(2)
\begin{bmatrix} x-c_x & y-c_y&z-c_z\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}^T\begin{bmatrix} \lambda_1 & 0 &0 \\ 0 & \lambda_2&0 \\0 &0&\lambda_3\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}\begin{bmatrix} x-c_x \\ y-c_y\\z-c_z\end{bmatrix}\\= 1+\begin{bmatrix} c_x & c_y&c_z\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}^T\begin{bmatrix} \lambda_1 & 0 &0 \\ 0 & \lambda_2&0 \\0 &0&\lambda_3\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}\begin{bmatrix} c_x \\ c_y\\c_z\end{bmatrix} \tag{2}
[ x − c x y − c y z − c z ] ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ T ⎣ ⎡ λ 1 0 0 0 λ 2 0 0 0 λ 3 ⎦ ⎤ ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ ⎣ ⎡ x − c x y − c y z − c z ⎦ ⎤ = 1 + [ c x c y c z ] ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ T ⎣ ⎡ λ 1 0 0 0 λ 2 0 0 0 λ 3 ⎦ ⎤ ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ ⎣ ⎡ c x c y c z ⎦ ⎤ ( 2 ) svg
上式写成矩阵形式spa
[
X
−
C
]
M
[
X
−
C
]
T
=
1
+
C
M
C
T
(3)
[X-C]M[X-C]^T=1+CMC^T\tag{3}
[ X − C ] M [ X − C ] T = 1 + C M C T ( 3 )
X
M
X
T
−
2
C
M
X
T
+
C
M
C
T
=
1
+
C
M
C
T
(4)
XMX^T-2CMX^T+CMC^T=1+CMC^T\tag{4}
X M X T − 2 C M X T + C M C T = 1 + C M C T ( 4 ) .net
其中
X
=
[
x
y
z
]
X=\begin{bmatrix}x &y & z\end{bmatrix}
X = [ x y z ]
C
=
[
c
x
c
y
c
z
]
C=\begin{bmatrix}c_x &c_y & c_z\end{bmatrix}
C = [ c x c y c z ]
M
=
R
T
B
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
T
[
λ
1
0
0
0
λ
2
0
0
0
λ
3
]
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
[
a
1
a
4
/
2
a
5
/
2
a
4
/
2
a
2
a
6
/
2
a
5
/
2
a
6
/
2
a
3
]
M=R^TBR=\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}^T\begin{bmatrix} \lambda_1 & 0 &0 \\ 0 & \lambda_2&0 \\0 &0&\lambda_3\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}=\begin{bmatrix} a_1 & a_4/2&a_5/2 \\ a_4/2& a_2&a_6/2 \\a_5/2 &a_6/2&a_3\end{bmatrix}
M = R T B R = ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ T ⎣ ⎡ λ 1 0 0 0 λ 2 0 0 0 λ 3 ⎦ ⎤ ⎣ ⎡ r 1 1 r 2 1 r 3 1 r 1 2 r 2 2 r 3 2 r 1 3 r 2 3 r 3 3 ⎦ ⎤ = ⎣ ⎡ a 1 a 4 / 2 a 5 / 2 a 4 / 2 a 2 a 6 / 2 a 5 / 2 a 6 / 2 a 3 ⎦ ⎤
C
=
−
1
2
[
a
7
a
8
a
9
]
(
M
)
−
1
C=-\frac{1}{2}[a_7\ a_8\ a_9](M)^{-1}
C = − 2 1 [ a 7 a 8 a 9 ] ( M ) − 1
S
S
=
C
M
C
T
+
1
SS=CMC^T+1
S S = C M C T + 1
x
s
c
a
l
e
=
S
S
λ
1
x_{scale}=\sqrt{\frac{SS}{\lambda_1}}
x s c a l e = λ 1 S S
y
s
c
a
l
e
=
S
S
λ
2
y_{scale}=\sqrt{\frac{SS}{\lambda_2}}
y s c a l e = λ 2 S S
z
s
c
a
l
e
=
S
S
λ
3
z_{scale}=\sqrt{\frac{SS}{\lambda_3}}
z s c a l e = λ 3 S S
算法实现 function[ Center,Scale_axis] = fit_elliposoid9( data )
% input data is n*3, n points of the ellipsoid surface
% Least Square Method
% a(1)x^2+a(2)y^2+a(3)z^2+a(4)xy+a(5)xz+a(6)yz+a(7)x+a(8)y+a(9)z=1
% output Center is 1*3, center of elliposoid,
% Scale_axis is 1*3, 3 axis' scale
% author Zhang Xin
x=data(:,1);
y=data(:,2);
z=data(:,3);
D=[x.*x y.*y z.*z x.*y x.*z y.*z x y z ];
a=inv(D'*D)*D'*ones(size(x));
M=[a(1) a(4)/2 a(5)/2;...
a(4)/2 a(2) a(6)/2;...
a(5)/2 a(6)/2 a(3)];
Center=-1/2*[a(7),a(8),a(9)]*inv(M);
SS=Center*M*Center'+1;
[U,V]=eig(M); %Matlab计算特征值是大小顺序
[~,n1]=max(abs(U(:,1))); %输出的，但和xyz轴顺序不
[~,n2]=max(abs(U(:,2))); %不一样，这个操做就是让特征
[~,n3]=max(abs(U(:,3))); %值和xyz轴对应上。
lambda(n1)=V(1,1);
lambda(n2)=V(2,2);
lambda(n3)=V(3,3);
Scale_axis=[sqrt(SS/lambda(1)),sqrt(SS/lambda(2)),sqrt(SS/lambda(3))];
Center=round(Center);
Scale_axis=round(Scale_axis);
figure
plot3(x,y,z,'b.',Center(1),Center(2),Center(3),'ro');
axis equal
xlabel('X');
ylabel('Y');
zlabel('Z');
end
新写的一篇关于IMU校订与姿态融合的博客IMU校订以及姿态融合 ，里面有更好校订参数。IMUCalibration-Gesture .
赞助：若是您以为此文对您所要作的工做有帮助，欢迎打赏。或者帮忙在github上标星星。
