磁力计椭球拟合（附MATLAB代码）

获取磁力计原始数据

//九轴模块获取原始数据
GY86_GetData(&gy);
//将原始数据打印出来，保存成 xxx.txt文件
//采集多些数据，绕八字也行，方便拟合
UART_printf(DATA_SEND_UART, "%f,%f,%f\r\n", gy.hX, gy.hY, gy.hZ);

MATLAB代码

获取原始数据后，需要对陀螺仪进行椭球拟合

原始代码下载链接Ellipsoid fit - File Exchange - MATLAB Central (mathworks.cn)

里面比较重要的应该只有ellipsoid_fit.m文件

下面是使用代码，在源代码基础上进行小小的修改

clc;
clear all;
%filename改成自己所读取磁力计的原始数据
filename = 'E:\VS_Code_Projects\hmc3.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( [ 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;

运行代码后

椭球 圆心: -16.348 -151.41 110.86
椭球 半径: 456.37 435.66 366.09
椭球面:
-0.24258 0.96948 0.035671
-0.96419 -0.245 0.10155
0.10719 -0.0097599 0.99419
Algebraic form:
6.6198e-06 6.0707e-06 9.3459e-06 1.3531e-07 2.9876e-07 -4.7062e-08 9.5587e-05 0.00092659 -0.0010383 -1 
拟合的平均偏差: 0.12241

拟合数据处理

//拟合球心(0,0,0)  
data->hX = (float)(int16_t)(hmc_data[0] << 8 | hmc_data[1]) + 16.348f;
//Z轴缩放
  data->hZ = ((float)(int16_t)(hmc_data[2] << 8 | hmc_data[3]) - 110.86f) * 456.37f / 366.09f;
//Y轴缩放
  data->hY = ((float)(int16_t)(hmc_data[4] << 8 | hmc_data[5]) + 151.41f) * 456.37f / 435.66f;

获取角度数据

angle = atan2(hY,hX)*180.0f/3.14f

将获得角度打印出来，可以和手机上的指南针进行对比，查看数据是否准确。