VR技术研究—9轴融合算法—磁力计校准（二）
VR技术研究—9轴融合算法—磁力计校准(二)
上一篇发现，在绘制磁力计后发现磁力计的圆心出现了明显的偏移，各轴的长度也不一样，如果不校准，融合的效果显然不够好。
一、 算法设计
为了解决这个问题我在也网上找了各种磁力计校准的方法和源码。因为从软件开发的角度上将，不开源的程序无法整合到自己的程序中去使用，很多时候不得不手工去校准确实麻烦。最后决定自己写一个磁力计校正算法。算法思路如下：
二、 算法实现
下面是Matlab代码实现，因为偷懒暂时没有转换成C语言的，实际上我仅仅调用了Matlab的随机函数，甚至没有使用矩阵运算。这个代码里面循环还是比较多的，Matlab也正是循环最慢，所以如果如果转化为C语言的，速度应该能快不少。实际上我测试后发现，求解一遍只需要0.4s，速度还是可以接受的。
load data.txt
M(:,1) = data(:,1);
M(:,2) = data(:,2);
M(:,3) = data(:,3);
tic;
plot3(M(:,1),M(:,2),M(:,3));
xmin = min(data(:,1));
ymin = min(data(:,2));
zmin = min(data(:,3));
xmax = max(data(:,1));
ymax = max(data(:,2));
zmax = max(data(:,3));
xc = 0.5*(xmax+xmin);
yc = 0.5*(ymax+ymin);
zc = 0.5*(zmax+zmin);
a = 0.5*abs(xmax-xmin);
b = 0.5*abs(ymax-ymin);
c = 0.5*abs(zmax-zmin);
%得到初始的解
x = M(:,1);
y = M(:,2);
z = M(:,3);
%开始计算误差
L = length(x(:,1));
err = 0;
for i=1:L
err = err + abs((x(i)-xc)^2/a^2 + (y(i)-yc)^2/b^2+(z(i)-zc)^2/c^2 - 1 );
end
fprintf('xc = %f yc = %f zc = %f, a = %f b = %f c= %f,初始化InitErr = %f\n',xc,yc,zc,a,b,c,err);
%为了测试算法有效性，我们选取0,0,0,1,1,1作为初始值求解
%xc = 0;
%yc = 0;
%zc = 0;
%a = 1;
%b = 1;
%c = 1;
%如果是采用上面的特殊参数验证算法的话，最好将下面的循环改为10000次以得到的更好的结果。
%采样数据量不宜过大，我的磁力计测试数据90k左右的txt，也基本足够了。因为初始解是计算
%出来的，所以能够降低计算量。
x = M(:,1);
y = M(:,2);
z = M(:,3);
%xclast = xc;
yclast = yc;
zclast = zc;
alast = a;
blast = b;
clast = c;
errlast = 100000000000;
for i = 1:1000
%产生随机扰动
r = rand(1,6);
xcnew = xclast + r(1)-0.5;
ycnew = yclast + r(2)-0.5;
zcnew = zclast + r(3)-0.5;
anew = abs(alast + r(4)-0.5);
bnew = abs(blast + r(5)-0.5);
cnew = abs(clast + r(6)-0.5);
errnew = 0;
for j=1:L
errnew = errnew + abs((x(j)-xcnew)^2/anew^2 + (y(j)-ycnew)^2/bnew^2+(z(j)-zcnew)^2/cnew^2 - 1 );
end
if(errnew% 有更好的解，接受新解
xclast = xcnew;
yclast = ycnew;
zclast = zcnew;
alast = anew;
blast = bnew;
clast = cnew;
errlast = errnew;
end
end
fprintf('xc = %f yc = %f zc = %f, a = %f b = %f c= %f,最后InitErr = %f\n',xclast,yclast,zclast,alast,blast,clast,errlast);
avr = (alast+blast+clast)/3;
fprintf('mx = (mx - %f)*%f\n',xclast,alast/avr);
fprintf('my = (my - %f)*%f\n',yclast,blast/avr);
fprintf('mz = (mz - %f)*%f\n',zclast,clast/avr);
toc
三、 算法测试
首先我先用程序从MPU9250中得到磁力计数据，筛选掉重复数据。并写成txt。使用Matlab读取磁力计的三轴数据，并显示：
我们大致是可以发现这个椭球的圆心实际上并不是在原点的。而且三轴也并不是完全相等。甚至看到某个轴已经几乎已经完全大于0了，这里吐槽下MPU9250不校准实在是没法用。现在要做的主要就是求出这个椭球的方程，然后利用椭球到正球的变换关系得到磁力计的修正参数。
算法运行结果：
xc = -19.000000 yc = 153.000000 zc = 27.500000, a = 205.000000 b = 198.000000 c= 216.500000,初始化InitErr = 315.052512
xc = -17.838688 yc = 148.716175 zc = 25.126346, a = 192.950563 b = 195.238901 c= 204.867032,最后InitErr = 173.079483
mx = (mx - -17.838688)*0.976048
my = (my - 148.716175)*0.987624
mz = (mz - 25.126346)*1.036328
Elapsed time is 0.442320 seconds.
可以看到算法输出了椭球的方程，xc,yc,zc表示椭球的球心，a,b,c表示的是椭球各轴的长度。最后得到的是椭球的校准参数，通过这个参数我们就可以校准磁力计了。PS: 上述的mx的两个符号纯粹是格式输入输出的问题，负负得正。暂时不知道去哪里上传我的磁力计测试数据，如果有看官需要的话，可以发邮件到[email protected] 索取。