四元数.zip

load('radardata4.csv'); load('imu4.csv'); fig = figure(... 'Name','3D Pointcloud',... 'NumberTitle','off',... 'IntegerHandle','off',... 'Units','normalized',... 'Position',[0.2 0.1 0.6 0.7],... 'Visible','on',... 'Toolbar','figure',... 'Tag','hdl_player'); %whitebg(fig,'black'); cameratoolbar(fig,'show'); cameratoolbar('SetMode','orbit'); % setup axis for animation axe = gca; set(axe,... 'NextPlot','replaceChildren',... 'Projection','perspective'); grid(axe,'off'); axis(axe,'xy'); axis(axe,'vis3d'); set(axe,... 'XTickLabelMode','manual','XTickLabel','',... 'YTickLabelMode','manual','YTickLabel','',... 'ZTickLabelMode','manual','ZTickLabel',''); %set color of the pointcloud to be 'red' set(axe,'ColorOrder',[1 0 0]); lasersAll = plot3(ones(1,3),ones(1,3),ones(1,3),'.'); set(lasersAll,'MarkerSize',1); set(lasersAll,'Visible','on'); X=radardata4'; unnamed1=imu4; [m,n]=size(X); scan=zeros(1081,3); cellscan=cell(m,1); for j=1:m for i=1:1081 scan(i,1)=-pi/4+(i-1)*pi/2*3/1080; scan(i,2)=X(j,i)/2000; if(scan(i,2)==30) scan(i,2)=scan(i-1,2); end end cellscan{j,1}=scan; end for j=1:m for i=1:1081 jiao= cellscan{j,1}(i,1); r=cellscan{j,1}(i,2); cellscan{j,1}(i,1)=r*cos(jiao); cellscan{j,1}(i,2)=r*sin(jiao); % cellscan{j,1}=cellscan{j,1}' end end % for i=1:m % cellscan(j,1)=cellscan(j,1)'; % end di=1; % figure(1); % plot(cellscan{di,1}(:,1),cellscan{di,1}(:,2)); % hold on xyz=zeros(615,3); for i=1:615; % xyz(i,:)=unnamed1(i+1,:)-unnamed1(i,:); xyz(i,:)=unnamed1(i,:) end for i=1:400 theta_z=xyz(i,3); theta_y=xyz(i,2); theta_x=xyz(i,1); theta_z = theta_z*pi/180; theta_y = theta_y*pi/180; theta_x = theta_x*pi/180; cos_z_2 = cos(0.5*theta_z); cos_y_2 = cos(0.5*theta_y); cos_x_2 = cos(0.5*theta_x); sin_z_2 = sin(0.5*theta_z); sin_y_2 = sin(0.5*theta_y); sin_x_2 = sin(0.5*theta_x); Q1 = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; Q2 = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; Q3 = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; Q4 = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; T=zeros(3,3); T(1,1) = Q1*Q1+Q2*Q2-Q3*Q3-Q4*Q4 ; T(1,2) = 2*(Q2*Q3-Q1*Q4); T(1,3) = 2*(Q2*Q4+Q1*Q3); T(2,1) = 2*(Q2*Q3+Q1*Q4); T(2,2) = Q1*Q1-Q2*Q2+Q3*Q3-Q4*Q4 ; T(2,3) = 2*(Q3*Q4-Q1*Q2); T(3,1) = 2*(Q2*Q4-Q1*Q3); T(3,2) = 2*(Q3*Q4+Q1*Q2); T(3,3) = Q1*Q1-Q2*Q2-Q3*Q3+Q4*Q4 ; % cellscan{i+1,1}=T*cellscan{i+1,1}; cellscan{i+1,1}=cellscan{i+1,1}*T; end for i=20:125 di=i; % figure(2); plot3(cellscan{di,1}(:,1),cellscan{di,1}(:,2),cellscan{di,1}(:,3),'b.'); hold on end