clear
clc
close all
load('data1.mat');
nn = size(imuPosX,1);
%% lightHouse坐标系转换
x =  lightHousePosX * 100;
y = -lightHousePosZ * 100;

plot(x,y,'r',imuPosX,imuPosY);

% 定义误差函数，即均方根误差
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));

% 使用 fminsearch 优化误差函数，找到使误差最小的旋转角和位移
initialGuess = [0, 10, 10]; % 初始猜测值，[旋转角, 位移]
optimizedParams = fminsearch(errorFunction, initialGuess);

% 输出优化后的旋转角和位移
rotationAngle = optimizedParams(1);
xOffset = optimizedParams(2);
yOffset = optimizedParams(3);
% 旋转坐标
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)