IMU加速度、磁力计校正－－椭球拟合

版权声明：本文为博主原创文章，未经博主允许不得转载。

博主：shenshikexmu

联系方式：[e-mail]

问题

考虑到IMＵ中，x,y,z轴的度量单位并不相同，假设各轴之间相互直。

那么加速度传感器在静止状态（也就是只受重力的状态下），各个姿态只受重力的，x,y,z轴值（假设x,y,z轴相互垂直并且度量单位都一致，如mpu9250三轴的度量单位都是2048，16g量程的情况下），在三维空间中，重力点都在一个球面上，但各轴之间的度量单位都会有偏差，所以各姿态重力点都落在一个椭球面上，椭球的中心，就是加速度的偏移量，也就是校准值。

在磁力计上，由于测量磁场强度，在环境不变的情况下，传感器每个姿态感受磁场强度是相同，所以不需要静止状态，磁力计测量的x,y,z轴值，在没有偏差的情况下，在传感器内部x,y,z轴相互垂直的情况下，在三维空间中组成一个圆球面。但是磁力计存在Hard Iron Distortion和Soft Iron Distortion。使得x,y,z轴度量单位不相同，各轴也并非相互垂直，（说明一下，任意椭球的三个轴都是相互垂直的，几何上，椭球最长的轴与最短的轴相互垂直，从代数的角度看，对称正定矩阵A=R′BR，其中B为对角线大于0表示各轴长度，其他位置为0的矩阵，R为旋转矩阵，R′R=I，所以磁通量的空间坐标虽然形成一个椭球，椭球各轴相互垂直，但这个垂直的轴已经不是传感器x,y,z轴了）椭球球心也并非[0,0,0]坐标磁通量在三维空间组成的椭球球心，是磁力计的校准值的一部分。

数学模型

所以问题在于给定椭球球面上的点，如何求椭球球心。其实就是一个椭球拟合问题。

a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1

从几何的角度表示上式的椭球为

[x−cxy−cyz−cz]⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥T⎡⎣⎢λ1000λ2000λ3⎤⎦⎥⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥⎡⎣⎢x−cxy−cyz−cz⎤⎦⎥=1

上式写成矩阵形式

[X−C]M[X−C]T=1
XMXT−2CMXT+CMCT=1

其中
X=[xyz]，表示球面上的点。
C=[cxcycz]，表示球心。
M=RTBR=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥T⎡⎣⎢λ1000λ2000λ3⎤⎦⎥⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥=⎡⎣⎢a1a4/2a5/2a4/2a2a6/2a5/2a6/2a3⎤⎦⎥

C=−12[a7 a8 a9](M)−1

SS=CMCT+1
xscale=SSλ1−−−√，椭球的x轴长度
yscale=SSλ2−−−√，椭球的y轴长度
zscale=SSλ3−−−√，椭球的z轴长度

算法实现

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 Xinx=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);[~,n1]=max(abs(U(:,1)));[~,n2]=max(abs(U(:,2)));[~,n3]=max(abs(U(:,3)));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);figureplot3(x,y,z,'b.',Center(1),Center(2),Center(3),'ro');axis equalxlabel('X');ylabel('Y');zlabel('Z');end

加速度校正

磁力计校正