60 lines
1.6 KiB
Matlab
60 lines
1.6 KiB
Matlab
|
clc;
|
|||
|
|
clear;
|
|||
|
|
load UWBIns2.mat
|
|||
|
|
|
|||
|
|
|
|||
|
|
x = lightHousePosX * 100;
|
|||
|
|
y = -lightHousePosZ * 100;
|
|||
|
|
|
|||
|
|
|
|||
|
|
theta = -1 / pi;
|
|||
|
|
% <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
|
|||
|
|
xt = x * cos(theta) - y * sin(theta);
|
|||
|
|
yt = x * sin(theta) + y * cos(theta);
|
|||
|
|
|
|||
|
|
x0 = xt(1) - imuPosX(1);
|
|||
|
|
y0 = yt(1) - imuPosY(1);
|
|||
|
|
|
|||
|
|
xt = xt - x0;
|
|||
|
|
yt = yt - y0;
|
|||
|
|
|
|||
|
|
% plot(xt,yt,'r');
|
|||
|
|
% hold on;
|
|||
|
|
% plot(imuPosX,imuPosY,'g');
|
|||
|
|
% plot(uwbPosX,uwbPosY,'b.');
|
|||
|
|
% plot(syncPosX,syncPosY,'k.');
|
|||
|
|
|
|||
|
|
% --------------------- ˮƽ<CBAE><C6BD>ʾ ---------------------------
|
|||
|
|
% %
|
|||
|
|
for i = 1:10:length(xt)
|
|||
|
|
|
|||
|
|
plot(xt(1:i),yt(1:i),'r');
|
|||
|
|
%set(gcf,'Position',[100 100 260 220]);
|
|||
|
|
hold on;
|
|||
|
|
%plot(imuPosX(1:i),imuPosY(1:i),'g');
|
|||
|
|
plot(uwbPosX(1:i),uwbPosY(1:i),'b.');
|
|||
|
|
plot(syncPosX(1:i),syncPosY(1:i),'k.');
|
|||
|
|
legend('<EFBFBD>ο<EFBFBD>ϵͳ', 'ԭʼUWB<EFBFBD><EFBFBD>λ','<EFBFBD>ഫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ں϶<EFBFBD>λ');
|
|||
|
|
pause(0.05);
|
|||
|
|
clf;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
% posPlot = [xt, yt, zeros(1,length(xt))'];
|
|||
|
|
% quatPlot = [ones(1,length(xt))',zeros(1,length(xt))',zeros(1,length(xt))',zeros(1,length(xt))'];
|
|||
|
|
% samplePeriod = 1/256;
|
|||
|
|
% SamplePlotFreq = 4;
|
|||
|
|
% Spin = 120;
|
|||
|
|
% SixDofAnimation(posPlot, quatern2rotMat(quatPlot), ...
|
|||
|
|
% 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'All', ...
|
|||
|
|
% 'Position', [9 39 1280 768], 'View', [(100:(Spin/(length(posPlot)-1)):(100+Spin))', 10*ones(length(posPlot), 1)], ...
|
|||
|
|
% 'AxisLength', 0.1, 'ShowArrowHead', false, ...
|
|||
|
|
% 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)', 'ShowLegend', false, ...
|
|||
|
|
% 'CreateAVI', false, 'AVIfileNameEnum', false, 'AVIfps', ((1/samplePeriod) / SamplePlotFreq));
|
|||
|
|
|
|||
|
|
% ----------------- <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾX<CABE><58><EFBFBD><EFBFBD><EFBFBD><EFBFBD> -------------------------
|
|||
|
|
% plot(xt);
|
|||
|
|
% hold on;
|
|||
|
|
% plot(imuPosX);
|
|||
|
|
%plot(uwbPosX,'.');
|