科學研究

Scientific Research
VR技術研究—9軸融合算法—磁力計校準(二)
2018

05/15

16:09

393人閱讀

發布者:admin

分享

 算法實現

下面是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
相關推薦
聯系我們
  • 合作電話:0791-87193886(轉短號611683)
  • 公司地址:江西省南昌市紅谷灘新區九龍大道1388號VR產業基地一號樓6樓
關注我們
友情鏈接:
Copyright ? 2018 南昌虛擬現實研究院 版權所有 ( 備案號: 贛ICP備18004577號-1 ) 技術支持:云端科技
欧美av电影,欧美av旡码高清在线