forked from ZhanLi/UWBIns
37 lines
1.0 KiB
Mathematica
37 lines
1.0 KiB
Mathematica
|
|
|||
|
|
|
|||
|
|
clear
|
|||
|
|
clc
|
|||
|
|
close all
|
|||
|
|
load('data1.mat');
|
|||
|
|
nn = size(imuPosX,1);
|
|||
|
|
%% lightHouse<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<EFBFBD><EFBFBD>
|
|||
|
|
x = lightHousePosX * 100;
|
|||
|
|
y = -lightHousePosZ * 100;
|
|||
|
|
|
|||
|
|
plot(x,y,'r',imuPosX,imuPosY);
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
errorFunction = @(params) sqrt(mean((x(1:500) * cos(params(1)) - y(1:500) * sin(params(1)) + params(2) - imuPosX(1:500)).^2 + (x(1:500) * sin(params(1)) + y(1:500) * cos(params(1)) + params(3) - imuPosY(1:500)).^2));
|
|||
|
|
|
|||
|
|
% ʹ<EFBFBD><EFBFBD> fminsearch <EFBFBD>Ż<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ǻ<EFBFBD>λ<EFBFBD><EFBFBD>
|
|||
|
|
initialGuess = [0, 10, 10]; % <EFBFBD><EFBFBD>ʼ<EFBFBD>²<EFBFBD>ֵ<EFBFBD><EFBFBD>[<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>, λ<EFBFBD><EFBFBD>]
|
|||
|
|
optimizedParams = fminsearch(errorFunction, initialGuess);
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ǻ<EFBFBD>λ<EFBFBD><EFBFBD>
|
|||
|
|
rotationAngle = optimizedParams(1);
|
|||
|
|
xOffset = optimizedParams(2);
|
|||
|
|
yOffset = optimizedParams(3);
|
|||
|
|
% <EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
xt = x * cos(rotationAngle) - y * sin(rotationAngle) + xOffset;
|
|||
|
|
yt = x * sin(rotationAngle) + y * cos(rotationAngle) + yOffset;
|
|||
|
|
|
|||
|
|
plot(xt,yt,'r',imuPosX,imuPosY);
|
|||
|
|
|
|||
|
|
ds = zeros(1,length(imuDataRxTime));
|
|||
|
|
for i = 1:length(imuDataRxTime)-1
|
|||
|
|
ds(i) = 1 / (imuDataRxTime(i+1)-imuDataRxTime(i));
|
|||
|
|
end
|
|||
|
|
plot(ds)
|
|||
|
|
|