forked from ZhanLi/UWBIns
37 lines
1.0 KiB
Matlab
37 lines
1.0 KiB
Matlab
|
|
|
|
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)
|
|
|