clc;
clear;
load UWBIns2.mat

  
x =  lightHousePosX * 100;
y = -lightHousePosZ * 100;


theta = -1 / pi;
% 旋转坐标
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.');

% --------------------- 水平显示  ---------------------------
% % 
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('参考系统', '原始UWB定位','多传感器融合定位');
    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));

% ----------------- 单独显示X轴情况 -------------------------
% plot(xt);
% hold on;
% plot(imuPosX);
%plot(uwbPosX,'.');