forked from ZhanLi/UWBIns
200 lines
5.2 KiB
Matlab
200 lines
5.2 KiB
Matlab
clear
|
|
clc
|
|
close all
|
|
load('data1.mat');
|
|
nn = size(imuPosX,1);
|
|
%% lightHouse坐标系转换
|
|
x = lightHousePosX * 100;
|
|
y = -lightHousePosZ * 100;
|
|
|
|
% 定义误差函数，即均方根误差
|
|
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;
|
|
%%
|
|
tagN = 4;
|
|
%标签坐标
|
|
XN(:,1)=[-300;-300];
|
|
XN(:,2)=[-300;300];
|
|
XN(:,3)=[300;300];
|
|
XN(:,4)=[300;-300];
|
|
sim2=6;
|
|
Q=diag(repmat(sim2,1,2*tagN));%协方差矩阵
|
|
measure_AOA = zeros(4,nn);
|
|
measure_d = zeros(4,nn);
|
|
measure_AOA(1,:) = aoa1';
|
|
measure_AOA(2,:) = aoa2';
|
|
measure_AOA(3,:) = aoa3';
|
|
measure_AOA(4,:) = aoa4';
|
|
measure_d(1,:) = d1';
|
|
measure_d(2,:) = d2';
|
|
measure_d(3,:) = d3';
|
|
measure_d(4,:) = d4';
|
|
%% uwb解算
|
|
x_uwb(1) = 0;
|
|
y_uwb(1) = 0;
|
|
theta_uwb=zeros(nn,1);
|
|
for i=2:nn
|
|
if measure_d(1,i) == 0
|
|
x_uwb(i) = x_uwb(i-1);
|
|
y_uwb(i) = y_uwb(i-1);
|
|
continue;
|
|
end
|
|
[t1,theta] = WLS(XN,measure_AOA(:,i),measure_d(:,i),Q);
|
|
x_uwb(i) = t1(1);
|
|
y_uwb(i) = t1(2);
|
|
theta_uwb(i) = 90-theta;
|
|
theta_uwb(i) = mod(theta_uwb(i)+180,360)-180;
|
|
derr_WLS(i)=norm(t1-[xt(i);yt(i)]);
|
|
end
|
|
thetat=zeros(nn,1);
|
|
for i=2:nn
|
|
detx=xt(i)-xt(i-1);
|
|
dety=yt(i)-yt(i-1);
|
|
thetat(i)=atan2d(detx,dety);
|
|
end
|
|
% Uwb.x=x_uwb;
|
|
% Uwb.y=y_uwb;
|
|
% Uwb.alpha=theta_uwb;
|
|
|
|
|
|
% 角速度校准
|
|
detW = mean(wZ(1:200));
|
|
wZ = wZ - detW;
|
|
|
|
x_imu(1) = 0;
|
|
y_imu(1) = 0;
|
|
Z=zeros(3,1);
|
|
WW = 0.05;
|
|
theta_imu(1) = 0;
|
|
|
|
|
|
%% EKF
|
|
KK = zeros(5,3);
|
|
|
|
%%
|
|
%%AEKF
|
|
R = diag([1 1 1]);
|
|
% qq = 50;
|
|
% qq = 10;
|
|
qq = 0.01;
|
|
Q = diag([qq qq qq qq qq]);
|
|
P0 = diag([0 0 0 0 0]);
|
|
H = [1 0 0 0 0;
|
|
0 1 0 0 0;
|
|
0 0 0 1 0];
|
|
X_aekf = zeros(5,nn);
|
|
X_aekf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)];
|
|
I = eye(5);
|
|
JF = zeros(5,5);
|
|
X_pre = zeros(5,nn);
|
|
alfa = 0.97;
|
|
|
|
windowlength=5;
|
|
x_aekf_sw=zeros(1,nn);
|
|
y_aekf_sw=zeros(1,nn);
|
|
for i=2:nn
|
|
detImuTime = imuDataRxTime(i) - imuDataRxTime(i-1);
|
|
detOdomTime = odomDataRxTime(i)-odomDataRxTime(i-1);
|
|
w = wZ(i);
|
|
v = vXY(i)*100;
|
|
detV=(vXY(i)-vXY(i-1))*100;
|
|
detD = v*detImuTime;
|
|
detTheta = w*180/pi*detImuTime;
|
|
detW = w-wZ(i-1);
|
|
theta_imu(i) = theta_imu(i-1)+detTheta;
|
|
theta_imu(i) = mod(theta_imu(i)+180,360)-180;
|
|
F = [1 0 detImuTime*cosd(X_aekf(4,i-1)) 0 0;
|
|
0 1 detImuTime*sind(X_aekf(4,i-1)) 0 0;
|
|
0 0 0 0 0;
|
|
0 0 0 0 0;
|
|
0 0 0 0 0;];
|
|
|
|
if w<WW
|
|
vtx = detD*cosd(theta_imu(i));
|
|
vty = detD*sind(theta_imu(i));
|
|
else
|
|
vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
|
|
vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));
|
|
end
|
|
|
|
x_imu(i) = x_imu(i-1)+vtx;
|
|
y_imu(i) = y_imu(i-1)+vty;
|
|
X_next = [vtx;vty;detV;detTheta;detW];
|
|
|
|
if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
|
|
X_aekf(:,i) = X_aekf(:,i-1)+X_next;
|
|
errAEKf(i) = norm(X_aekf(1:2,i)-[xt(i);yt(i)]);
|
|
theta_aekf(i) = X_aekf(4,i);
|
|
continue
|
|
end
|
|
if w<WW
|
|
X_pre(:,i) = X_aekf(:,i-1)+X_next;
|
|
Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
|
|
P = F*P0*F'+Q;
|
|
Kg = P*H'*inv(H*P*H'+R);
|
|
KK(1,1) = Kg(1,1);
|
|
KK(2,2) = Kg(2,2);
|
|
KK(4,3) = Kg(4,3);
|
|
X_aekf(:,i) = X_pre(:,i)+KK*(Z-H*X_pre(:,i));
|
|
P0 = (I-Kg*H)*P;
|
|
else
|
|
JF = [1 0 1/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) v/w*(cosd(theta_imu(i))-cosd(theta_imu(i-1))) detD/w*cosd(theta_imu(i))-v/(w^2)*(sind(theta_imu(i))-sind(theta_imu(i-1)));
|
|
0 1 1/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1))) v/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) detD/w*sind(theta_imu(i))-v/(w^2)*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));
|
|
0 0 1 0 0;
|
|
0 0 0 1 detImuTime;
|
|
0 0 0 0 1];
|
|
X_pre(:,i) = X_aekf(:,i-1)+X_next;
|
|
dk = Z - H*X_pre(:,i);
|
|
Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
|
|
P = JF*P0*JF'+Q;
|
|
Kg = P*H'*inv(H*P*H'+R);
|
|
KK(1,1) = Kg(1,1);
|
|
KK(2,2) = Kg(2,2);
|
|
KK(4,3) = Kg(4,3);
|
|
X_aekf(:,i) = X_pre(:,i)+KK*(Z-H*X_pre(:,i));
|
|
epz = Z-H*X_aekf(:,i);
|
|
R=alfa*R+(1-alfa)*(epz*epz'+H*P*H');
|
|
Q=alfa*Q+(1-alfa)*Kg*dk*dk'*Kg';
|
|
P0 = (I-Kg*H)*P;
|
|
end
|
|
errAEKf(i) = norm(X_aekf(1:2,i)-[xt(i);yt(i)]);
|
|
theta_aekf(i) = X_aekf(4,i);
|
|
% if errAEKf(i)>15
|
|
% xxx = 1;
|
|
% end
|
|
end
|
|
errAEKf_sw=zeros(1,nn);
|
|
weightedvector=[0.3;0.25;0.2;0.15;0.1];
|
|
for i=1:nn
|
|
if i>windowlength-1
|
|
x_win=X_aekf(1,i-windowlength+1:i);
|
|
y_win=X_aekf(2,i-windowlength+1:i);
|
|
x_aekf_sw(i)=x_win*weightedvector;
|
|
y_aekf_sw(i)=y_win*weightedvector;
|
|
else
|
|
x_aekf_sw(i)=X_aekf(1,i);
|
|
y_aekf_sw(i)=X_aekf(2,i);
|
|
end
|
|
errAEKf_sw(i) = norm([x_aekf_sw(i);y_aekf_sw(i)]-[xt(i);yt(i)]);
|
|
end
|
|
|
|
figure;
|
|
plot(errAEKf_sw);
|
|
figure;
|
|
plot(x_imu,y_imu,xt,yt);
|
|
mean(errAEKf_sw)
|
|
|
|
|
|
|