forked from ZhanLi/UWBIns
342 lines
9.7 KiB
Matlab
342 lines
9.7 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;
|
||
|
||
|
||
%
|
||
% Imu.x=x_imu;
|
||
% Imu.y=y_imu;
|
||
% Imu.v=v_imu;
|
||
% Imu.alpha=alpha_imu;
|
||
% Imu.omega=omega_imu;
|
||
|
||
%% KF
|
||
R = diag([1 1 1]);
|
||
% qq = 1;
|
||
qq = 0.00001;
|
||
Q1 = 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];
|
||
I = eye(5);
|
||
JF = zeros(5,5);
|
||
X_pre = zeros(5,nn);
|
||
X_kf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)];
|
||
for i=2:nn
|
||
% 计算IMU和里程计的时间差值
|
||
detImuTime = imuDataRxTime(i) - imuDataRxTime(i-1);
|
||
detOdomTime = odomDataRxTime(i)-odomDataRxTime(i-1);
|
||
% 获得此时Z轴角速度
|
||
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);
|
||
% 积分计算IMU的航位角
|
||
theta_imu(i) = theta_imu(i-1)+detTheta;
|
||
% 航向约束
|
||
theta_imu(i) = mod(theta_imu(i)+180,360)-180;
|
||
|
||
% 状态更新方程
|
||
F = [1 0 detImuTime*cosd(X_kf(4,i-1)) 0 0;
|
||
0 1 detImuTime*sind(X_kf(4,i-1)) 0 0;
|
||
0 0 0 0 0;
|
||
0 0 0 0 0;
|
||
0 0 0 0 0;];
|
||
% 获得水平和垂直方向的位置增量
|
||
vtx = detD*cosd(theta_imu(i));
|
||
vty = detD*sind(theta_imu(i));
|
||
|
||
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_kf(:,i) = X_kf(:,i-1)+X_next;
|
||
errKf(i) = norm(X_kf(1:2,i)-[xt(i);yt(i)]);
|
||
theta_kf(i) = X_kf(4,i);
|
||
continue
|
||
end
|
||
X_pre(:,i) = X_kf(:,i-1)+X_next;
|
||
Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
|
||
P = F*P0*F'+Q1;
|
||
Kg_kf = P*H'*inv(H*P*H'+R);
|
||
X_kf(:,i) = X_pre(:,i)+Kg_kf*(Z-H*X_pre(:,i));
|
||
P0 = (I-Kg_kf*H)*P;
|
||
|
||
errKf(i) = norm(X_kf(1:2,i)-[xt(i);yt(i)]);
|
||
theta_kf(i) = X_kf(4,i);
|
||
end
|
||
|
||
|
||
|
||
|
||
%% EKF
|
||
R = diag([1 1 1]);
|
||
% qq = 1;
|
||
qq = 0.00001;
|
||
Q1 = 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];
|
||
KK = zeros(5,3);
|
||
X_ekf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)];
|
||
I = eye(5);
|
||
JF = zeros(5,5);
|
||
X_pre = zeros(5,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_ekf(4,i-1)) 0 0;
|
||
0 1 detImuTime*sind(X_ekf(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;
|
||
errUwb(i) = norm([x_uwb(i);y_uwb(i)]-[xt(i);yt(i)]);
|
||
errImu(i) = norm([x_imu(i);y_imu(i)]-[xt(i);yt(i)]);
|
||
X_next = [vtx;vty;detV;detTheta;detW];
|
||
if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
|
||
X_ekf(:,i) = X_ekf(:,i-1)+X_next;
|
||
errEKf(i) = norm(X_ekf(1:2,i)-[xt(i);yt(i)]);
|
||
theta_ekf(i) = X_ekf(4,i);
|
||
continue
|
||
end
|
||
if w<WW
|
||
X_pre(:,i) = X_ekf(:,i-1)+X_next;
|
||
Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
|
||
P = F*P0*F'+Q1;
|
||
Kg = P*H'*inv(H*P*H'+R);
|
||
X_ekf(:,i) = X_pre(:,i)+Kg*(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_ekf(:,i-1)+X_next;
|
||
Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
|
||
P = JF*P0*JF'+Q1;
|
||
Kg = P*H'*inv(H*P*H'+R);
|
||
X_ekf(:,i) = X_pre(:,i)+Kg*(Z-H*X_pre(:,i));
|
||
P0 = (I-Kg*H)*P;
|
||
end
|
||
errEKf(i) = norm(X_ekf(1:2,i)-[xt(i);yt(i)]);
|
||
theta_ekf(i) = X_ekf(4,i);
|
||
end
|
||
%% UKF
|
||
% UKF settings
|
||
ukf_L = 5; %numer of states
|
||
ukf_m = 3; %numer of measurements
|
||
ukf_kappa = 3 - ukf_L;
|
||
ukf_alpha = 0.9;
|
||
ukf_beta = 2;
|
||
|
||
ukf_lambda = ukf_alpha^2*(ukf_L + ukf_kappa) - ukf_L;
|
||
ukf_gamma = sqrt(ukf_L + ukf_lambda);
|
||
ukf_W0_c = ukf_lambda / (ukf_L + ukf_lambda) + (1 - ukf_alpha^2 + ukf_beta);
|
||
ukf_W0_m = ukf_lambda / (ukf_L + ukf_lambda);
|
||
ukf_Wi_m = 1 / (2*(ukf_L + ukf_lambda));
|
||
ukf_Wi_c = ukf_Wi_m;
|
||
|
||
q=0.01; %std of process
|
||
r=5; %std of measurement
|
||
p=2;
|
||
Qu=q*eye(ukf_L); % std matrix of process
|
||
Ru=r*eye(ukf_m); % std of measurement
|
||
Pu=p*eye(ukf_L);
|
||
H = [1 0 0 0 0;
|
||
0 1 0 0 0;
|
||
0 0 0 1 0];
|
||
X_ukf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)];
|
||
I = eye(5);
|
||
JF = zeros(5,5);
|
||
X_pre = zeros(5,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_ekf(4,i-1)) 0 0;
|
||
% 0 1 detImuTime*sind(X_ekf(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;
|
||
errUwb(i) = norm([x_uwb(i);y_uwb(i)]-[xt(i);yt(i)]);
|
||
errImu(i) = norm([x_imu(i);y_imu(i)]-[xt(i);yt(i)]);
|
||
X_next = [vtx;vty;detV;detTheta;detW];
|
||
if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
|
||
X_ukf(:,i) = X_ukf(:,i-1)+X_next;
|
||
errUKf(i) = norm(X_ukf(1:2,i)-[xt(i);yt(i)]);
|
||
theta_ukf(i) = X_ukf(4,i);
|
||
continue
|
||
end
|
||
|
||
xestimate = X_ukf(:,i-1);
|
||
Xx = repmat(xestimate, 1, length(xestimate));
|
||
Xsigma = [xestimate, ( Xx + ukf_gamma * Pu ), ( Xx - ukf_gamma * Pu )];
|
||
|
||
%第二步：对Sigma点集进行一步预测
|
||
[Xsigmapre]=fun1(Xsigma,detImuTime);
|
||
%第三步：估计预测状态
|
||
Xpred = ukf_W0_m * Xsigmapre(:,1) + ukf_Wi_m * sum(Xsigmapre(:,2:end), 2);
|
||
%第四步：均值和方差
|
||
Xx = repmat(Xpred, 1, length(xestimate)*2);
|
||
[~, R] = qr([sqrt(ukf_Wi_c) * ( Xsigmapre(:,2:end) - Xx ), Qu]', 0);
|
||
Ppred = cholupdate(R, sqrt(ukf_W0_c) * (Xsigmapre(:,1) - Xpred), '-')';
|
||
|
||
%第5步：根据预测值，再一次使用UT变换，得到新的sigma点集
|
||
Xx = repmat(Xpred, 1, length(xestimate));
|
||
Xsigmapre = [Xpred, ( Xx + ukf_gamma * Ppred ), ( Xx - ukf_gamma * Ppred )];
|
||
|
||
%第6步：观测预测
|
||
Zsigmapre=H*Xsigmapre;
|
||
|
||
%第7步：计算观测预测均值和协方差
|
||
Zpred = ukf_W0_m * Zsigmapre(:,1) + ukf_Wi_m * sum(Zsigmapre(:,2:end), 2);
|
||
|
||
Yy = repmat(Zpred, 1, length(xestimate)*2);
|
||
[~, Rz] = qr([sqrt(ukf_Wi_c) * (Zsigmapre(:,2:end) - Yy), Ru]', 0);
|
||
Pzz = cholupdate(Rz, sqrt(ukf_W0_c) * (Zsigmapre(:,1) - Zpred), '-')';
|
||
|
||
Xd = Xsigmapre - repmat(Xpred, 1, length(xestimate)*2 + 1);
|
||
Zd = Zsigmapre - repmat(Zpred, 1, length(xestimate)*2 + 1);
|
||
|
||
Pxz = ( ukf_W0_c* Xd(:,1) * Zd(:,1)' ) + ( ukf_Wi_c * Xd(:,2:end) * Zd(:,2:end)' );
|
||
|
||
%第七步：计算kalman增益
|
||
Kukf= (Pxz / Pzz') / Pzz;
|
||
%第八步：状态和方差更新
|
||
Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
|
||
Xpred=Xpred+Kukf*(Z-Zpred);
|
||
U = Kukf * Pzz;
|
||
Rp = Ppred';
|
||
for ii=1:size(U, 2)
|
||
Rp = cholupdate(Rp, U(:,ii), '-');
|
||
end
|
||
Ppred = Rp';
|
||
X_ukf(:,i)=Xpred;
|
||
|
||
errUKf(i) = norm(X_ukf(1:2,i)-[xt(i);yt(i)]);
|
||
theta_ukf(i) = X_ukf(4,i);
|
||
end
|
||
plot(errUKf);
|
||
mean(errUKf) |