28 lines
402 B
Matlab
28 lines
402 B
Matlab
|
clc;
|
|||
|
|
clear;
|
|||
|
|
load UWBIns.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 + 20;
|
|||
|
|
yt = yt - y0 - 10;
|
|||
|
|
|
|||
|
|
plot(xt,yt);
|
|||
|
|
hold on;
|
|||
|
|
plot(imuPosX,imuPosY);
|
|||
|
|
hold on;
|
|||
|
|
plot(uwbPosX,uwbPosY,'.');
|
|||
|
|
hold on;
|
|||
|
|
plot(syncPosX,syncPosY,'.');
|