VR技术研究—9轴融合算法—磁力计校准（二）

VR技术研究—9轴融合算法—磁力计校准(二)

上一篇发现，在绘制磁力计后发现磁力计的圆心出现了明显的偏移，各轴的长度也不同，若是不校准，融合的效果显然不够好。

1、 算法设计

    为了解决这个问题我在也网上找了各类磁力计校准的方法和源码。由于从软件开发的角度上将，不开源的程序没法整合到本身的程序中去使用，不少时候不得不手工去校准确实麻烦。最后决定本身写一个磁力计校订算法。算法思路以下：

2、 算法实现

    下面是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<errlast)   % 有更好的解，接受新解
        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

3、 算法测试

    首先我先用程序从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的两个符号纯粹是格式输入输出的问题，负负得正。暂时不知道去哪里上传个人磁力计测试数据，若是有看官须要的话，能够发邮件到719901725@qq.com 索取。