磁力计椭球拟合使用篇 IMU 加速度、电子罗盘校准

磁力计校准椭球拟合使用篇

！！下方蓝色函数链接！！

matlab 椭球拟合函数链接

串口打印磁力计数据，可以选择原始数据不进行任何缩放（前提是各轴向分辨率一致）。

	get_RM3100_data(&mx,&my,&mz);
	printf("%d,%d,%d\r\n",mx,my,mz);

各个位置方向旋转，进行数据采样，一定要保证数据各个角度都采集到，数据越多效果越好。
将数据保存未txt文档。
从上面matlab 的椭球拟合文档中将function copy一下，稍微修改：

clc;
clear all;
filename = 'mag_ellipsoid1.txt';
mag_data = load(filename);
x= mag_data(:,1);
y= mag_data(:,2);
z= mag_data(:,3);
%椭球拟合函数1
[ center, radii, evecs, v, chi2 ] = ellipsoid_fit_new( [ x y z ], '' );
fprintf( '椭球 圆心: %.5g %.5g %.5g\n', center );
fprintf( '椭球 半径: %.5g %.5g %.5g\n', radii );
fprintf( '椭球面:\n' );
fprintf( '%.5g %.5g %.5g\n%.5g %.5g %.5g\n%.5g %.5g %.5g\n', ...
    evecs(1), evecs(2), evecs(3), evecs(4), evecs(5), evecs(6), evecs(7), evecs(8), evecs(9) );
fprintf( 'Algebraic form:\n' );
fprintf( '%.5g ', v );
fprintf( '\n拟合的平均偏差: %.5f\n', sqrt( chi2 / size( x, 1 ) ) );
fprintf( '\n' );
% draw data
figure,
plot3( x, y, z, '.r' );
hold on;
%draw fit
mind = min( [ x y z ] );
maxd = max( [ x y z ] );
nsteps = 50;
step = ( maxd - mind ) / nsteps;
[ x, y, z ] = meshgrid( linspace( mind(1) - step(1), maxd(1) + step(1), nsteps ), linspace( mind(2) - step(2), maxd(2) + step(2), nsteps ), linspace( mind(3) - step(3), maxd(3) + step(3), nsteps ) );
Ellipsoid = v(1) *x.*x +   v(2) * y.*y + v(3) * z.*z + ...
          2*v(4) *x.*y + 2*v(5)*x.*z + 2*v(6) * y.*z + ...
          2*v(7) *x    + 2*v(8)*y    + 2*v(9) * z;
p = patch( isosurface( x, y, z, Ellipsoid, -v(10) ) );
hold off;
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
grid on;
set( p, 'FaceColor', 'g', 'EdgeColor', 'none' );
view( -70, 40 );
axis vis3d equal auto;
camlight;
lighting phong;
hold on;

得到拟合结果：

如何使用得到的数值

		get_RM3100_data(&mx,&my,&mz);
		
		printf("%d,%d,%d\r\n",mx,my,mz);
		//椭球校准
		mx1 = mx-64.384;//椭圆圆心平移至0，0，0 
		my1 = (my+58.583)*  4269.5/ 3869;//平移轴圆心，并且将y轴按x轴缩放
		mz1 = (mz-31.555)*  4269.5/3783.7;//平移轴圆心，并且将z轴按x轴缩放

测试：

	//计算平面偏航角度
	angle=atan2(my1,mx1)*180/3.14159+180 ;

平面上计算偏航角度，找个桌子角旋转90°测试一下，看角度值是否可靠。

ps 简单一试，一切从简。大佬勿喷。基于MATLANB 的离线椭球校准窗户纸就在这了。其他方法大同小异。研究算法的去看matlab function。