獲取磁力計原始資料
//九軸模塊獲取原始資料
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 = https://www.cnblogs.com/recordli/p/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
將獲得角度列印出來，可以和手機上的指南針進行對比，查看資料是否準確，
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/501914.html
標籤：其他