compass_revise.zip_correct for gravity_correct gravity_椭圆拟合圆心_磁力

clear;close all; %清空内存，关闭活动窗口，清空命令窗口。 log_in = textread('test1.txt'); %导进数据 %log_in=xlsread('YZ.xls'); xdata = log_in(:,2); %取第三列 ydata = log_in(:,6); %取第四列 length=size(xdata,1); %数据组数 oneArray = ones(length,1);%全1列矩阵 theta = 0; %原来的数据太好，为看出效果，先旋转了一定度数，使用时为0就可以了。 R = [cos(theta),sin(theta),0;-sin(theta),cos(theta),0;0,0,1]; %旋转矩阵 G = [1,0,0;0,1,0;0,0,1]; %缩放矩阵，也是为了测试效果，使用时为单位矩阵 xydata = [xdata,ydata,oneArray]*R*G; %所有采集的坐标点，由于2D矩阵变换需要3维数，第三列全为1 xdata = xydata(:,1); %x轴坐标 ydata = xydata(:,2); %y轴坐标 xMin=min(xdata);xMax=max(xdata);yMin=min(ydata);yMax=max(ydata); %为了定义坐标轴范围使用 Fun=@(p,x)p(1)*x(:,1).^2+p(2)*x(:,2).^2+p(3)*x(:,1).*x(:,2)+p(4)*x(:,1)+p(5)*x(:,2)-1;%椭圆一般方程 a x^2 + b y^2 + c xy + d x + e y - 1 = 0; p0=[1 1 1 1 1 ]; %预估初始值 p=nlinfit(xydata,zeros(length,1),Fun,p0); %拟合出合适的椭圆曲线 plot(xdata,ydata,'r.');hold on; %画出所有坐标点，测试时坐标点是变换过的，使用时是原始坐标点。 fig1=ezplot(@(x,y)Fun(p,[x,y]),[xMin-1000,xMax+1000,yMin-1000,yMax+1000]);hold on; %显示拟合曲线 set(fig1, 'Color', 'b'); %设置颜色为蓝色 %[m,n,l]=solve('i*(1+cos(k))+j*(1-cos(k))=2*p(1)','i*(1-cos(k))+j*(1+cos(k))=2*p(2)','(j-i)*sin(k)=p(3)')%该方程组解出k/2的等式如下 theta = atan((p(1) - p(2) + (p(1)^2 - 2*p(1)*p(2) + p(2)^2 + p(3)^2)^(1/2))/p(3)); %该算式由方程组解得，两个解只取一个 Angel = theta / pi * 180; %将弧度化成角度方便观察 R = [cos(theta),sin(theta),0;-sin(theta),cos(theta),0;0,0,1] %旋转 L = [1,0,0;0,1,0;p(4)/2/p(1),p(5)/2/p(2),1] %平移 if p(1)>p(2) %y轴一定拉伸，原因不详，可能是角度用atan的原因。 Gain =sqrt(p(1)/p(2)); else Gain =sqrt(p(2)/p(1)); end G = [1,0,0;0,Gain,0;0,0,1] %缩放 xydata = [xdata,ydata,oneArray]*L*R*G; %对原始数据进行矩阵变换，得出来的数据再进行拟合看看最终是否校正为圆。 xdata = xydata(:,1); %旋转和缩放都是相对于原点来说的，所以一定要先平移到原点。 ydata = xydata(:,2); p=nlinfit(xydata,zeros(length,1),Fun,p0); %再次拟合最终曲线，看看是否近似为圆。 plot(xdata,ydata,'y.'); hold on; %画出所有坐标点 fig2=ezplot(@(x,y)Fun(p,[x,y]),[xMin-1000,xMax+1000,yMin-1000,yMax+1000]);hold on; set(fig2, 'Color', 'g'); axis equal; %xy轴等间距等长 title('磁力计校正'); %图像名称 legend('样本点','拟合曲线','矫正点','矫正曲线'); %图像注释 figure(2); Angle = zeros(length,1); Offset = 0; Angle(1) = atan2(ydata(1),xdata(1))/pi*180; for i = 2:length Angle(i) = atan2(ydata(i),xdata(i))/pi*180+Offset; if Angle(i)>Angle(i-1)+90 Angle(i) = Angle(i)-360; Offset = Offset-360; end if Angle(i)<Angle(i-1)-90 Angle(i) = Angle(i)+360; Offset = Offset+360; end end plot(1:length,Angle,'y.'); hold on; xlabel('时间/t');ylabel('角度/°');title('拟合后的角度');