ZhanLi/UWBIns
1
0
Fork 1
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity
UWBIns/Code/Matlab/AOA-IMU/RUN_AEKF.m

580 lines
17 KiB
Mathematica
Raw Normal View History

多种卡尔曼滤波器的参考代码
2025-04-23 11:51:06 +08:00
clear
clc
close all
load('data1.mat');
nn = size(imuPosX,1);
%% lightHouse<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<EFBFBD><EFBFBD>
x = lightHousePosX * 100;
y = -lightHousePosZ * 100;
% <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;
%%
tagN = 4;
%<EFBFBD><EFBFBD>ǩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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));%Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
% <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>У׼
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
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>IMU<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̼Ƶ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
detImuTime = imuDataRxTime(i) - imuDataRxTime(i-1);
detOdomTime = odomDataRxTime(i)-odomDataRxTime(i-1);
% <EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD>ʱZ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
w = wZ(i);
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ<EFBFBD>ٶ<EFBFBD>
v = vXY(i)*100;
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
detV=(vXY(i)-vXY(i-1))*100;
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
detD = v*detImuTime;
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
detTheta = w*180/pi*detImuTime;
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵı仯<EFBFBD><EFBFBD>
detW = w-wZ(i-1);
% <EFBFBD><EFBFBD><EFBFBD>ּ<EFBFBD><EFBFBD><EFBFBD>IMU<EFBFBD>ĺ<EFBFBD>λ<EFBFBD><EFBFBD>
theta_imu(i) = theta_imu(i-1)+detTheta;
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD>
theta_imu(i) = mod(theta_imu(i)+180,360)-180;
% ״̬<EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD>
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;];
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ<EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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 )];
%<EFBFBD>ڶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Sigma<EFBFBD>㼯<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD>
[Xsigmapre]=fun1(Xsigma,detImuTime);
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD>״̬
Xpred = ukf_W0_m * Xsigmapre(:,1) + ukf_Wi_m * sum(Xsigmapre(:,2:end), 2);
%<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͷ<EFBFBD><EFBFBD><EFBFBD>
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), '-')';
%<EFBFBD><EFBFBD>5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>UT<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD>µ<EFBFBD>sigma<EFBFBD>㼯
Xx = repmat(Xpred, 1, length(xestimate));
Xsigmapre = [Xpred, ( Xx + ukf_gamma * Ppred ), ( Xx - ukf_gamma * Ppred )];
%<EFBFBD><EFBFBD>6<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۲<EFBFBD>Ԥ<EFBFBD><EFBFBD>
Zsigmapre=H*Xsigmapre;
%<EFBFBD><EFBFBD>7<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۲<EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)' );
%<EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>kalman<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Kukf= (Pxz / Pzz') / Pzz;
%<EFBFBD>ڰ˲<EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD>ͷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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
%%
%%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);
%%
for i=2:length(theta_uwb)-1
if theta_uwb(i) == 0
theta_uwb(i) = theta_uwb(i-1);
end
end
figure(1)
clf(1)
plot(x_uwb,y_uwb,'.','color',[205/255, 133/255, 63/255],'LineWidth',0.2);hold on
plot(x_imu, y_imu, '-','color',[0, 128/255, 0],'LineWidth',0.5);
plot(xt, yt, '-black','LineWidth',0.5);
legend('AUAM','IMU','True value');
xlabel('X(cm)');
ylabel('Y(cm)');
grid on
% figure(1)
% clf(1)
% hold on
% plot((1:nn)/100,thetat,'-black','LineWidth',1);
% plot((1:nn)/100,theta_uwb,'-','color','#CD853F','LineWidth',1);
% plot((1:nn)/100,theta_imu, '-','color','#008000','LineWidth',1);
% legend('LightHouse','UWB', 'INS');
% xlabel('time(s)');
% ylabel('\alpha(<EFBFBD><EFBFBD>)');
% grid on
% figure(2)
% clf(2)
% hold on
% plot(theta_imu,'.');
% plot(theta_uwb,'.');
% legend('imu','uwb');
% xlabel('<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䣨10ms<EFBFBD><EFBFBD>');
% ylabel('ƫ<EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><EFBFBD>㣩');
% title('<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>У׼<EFBFBD><EFBFBD>imu<EFBFBD><EFBFBD>uwb<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>');
% grid on
%
% figure(3)
% clf(3)
% hold on
% plot(xt, yt, '.-black','LineWidth',1);
% plot(X_ekf(1,:), X_ekf(2,:), '.-','color','#7E2F8E','LineWidth',1);
% plot(X_aekf(1,:), X_aekf(2,:), '.-','color','#D95319','LineWidth',0.5)
% plot(x_aekf_sw, y_aekf_sw, '.-','color','#2E8B57','LineWidth',0.5)
% legend('True','EKF', 'AEKF', 'AEKF-SWF');
% xlabel('x(cm)');
% ylabel('y(cm)');
% grid on
%
% figure(4)
% clf(4)
% hold on
% plot(errKf,'.','color','#0072BD');
% plot(errEKf,'.','color','#EDB120');
% plot(errUKf,'.','color','#D95319');
% plot(errAEKf,'.','color','#7E2F8E');
% plot(errAEKf_sw,'.','color','#77AC30');
% legend('KF','EKF','UKF','AEKF','AEKF-SWF');
% xlabel('<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䣨10ms<EFBFBD><EFBFBD>');
% ylabel('<EFBFBD><EFBFBD><EFBFBD>cm<EFBFBD><EFBFBD>');
% title('<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>仯ͼ');
% slt = 1;
% uwbcount = 9;
% imucount = 15;
% ekfcount = 12;
% aekfcount = 5;
% figure(5)
% clf(5)
% hold on
% [fUWB,xUWB] = ksdensity(errUwb(slt:end));
% [fIMU,xIMU] = ksdensity(errImu(slt:end));
% [fAEKF,xAEKF] = ksdensity(errAEKf(slt:end));
% [fUKF,xUKF] = ksdensity(errUKf(slt:end));
% [fAEKF_sw,xAEKF_sw] = ksdensity(errAEKf_sw(slt:end));
% [fEKF,xEKF] = ksdensity(errEKf(slt:end));
% [fKF,xKF] = ksdensity(errKf(slt:end));
% plot(xKF(ekfcount:end),fKF(ekfcount:end), 'color','#0072BD','LineWidth',0.5);
% plot(xEKF(ekfcount:end),fEKF(ekfcount:end),'color','#EDB120','LineWidth',0.5);
% plot(xUKF(ekfcount:end), fUKF(ekfcount:end),'color','#D95319','LineWidth',0.5);
% plot(xAEKF(aekfcount:end),fAEKF(aekfcount:end),'color','#7E2F8E','LineWidth',0.5);
% plot(xAEKF_sw(aekfcount:end),fAEKF_sw(aekfcount:end), 'color','#77AC30','LineWidth',0.5)
% legend('KF','EKF','UKF','AEKF','AEKF-SWF');%'UWB','INS',
% xlabel('Error(cm)');
% ylabel('Probability(%)');
% grid on
%
%
%
% slt = 3000;
% uwbcount = 9;
% imucount = 9;
% ekfcount = 13;
% aekfcount = 7;
% figure(6)
% clf(6)
% hold on
% [fAEKF,xAEKF] = ksdensity(errAEKf(slt:end));
% [fUKF,xUKF] = ksdensity(errUKf(slt:end));
% [fAEKF_sw,xAEKF_sw] = ksdensity(errAEKf_sw(slt:end));
% [fEKF,xEKF] = ksdensity(errEKf(slt:end));
% [fKF,xKF] = ksdensity(errKf(slt:end));
% plot(xKF(ekfcount:end),fKF(ekfcount:end), 'color','#0072BD','LineWidth',0.5);
% plot(xEKF(ekfcount:end),fEKF(ekfcount:end),'color','#EDB120','LineWidth',0.5);
% plot(xUKF(ekfcount:end), fUKF(ekfcount:end),'color','#D95319','LineWidth',0.5);
% plot(xAEKF(aekfcount:end),fAEKF(aekfcount:end),'color','#7E2F8E','LineWidth',0.5);
% plot(xAEKF_sw(aekfcount:end),fAEKF_sw(aekfcount:end), 'color','#77AC30','LineWidth',0.5)
% legend('KF','EKF','UKF','AEKF','AEKF-SWF');%'UWB','INS',
% xlabel('Error(cm)');
% ylabel('PDF');
% grid on
%
% figure(7)
% clf(7)
% hold on
% h1 = cdfplot(errKf(slt:end));
% h2 = cdfplot(errEKf(slt:end));
% h3 = cdfplot(errUKf(slt:end));
% h4 = cdfplot(errAEKf(slt:end));
% h5 = cdfplot(errAEKf_sw(slt:end));
% set(h1,'color','#0072BD','LineWidth',0.5);
% set(h2,'color','#EDB120','LineWidth',0.5);
% set(h3,'color','#D95319','LineWidth',0.5);
% set(h4,'color','#7E2F8E','LineWidth',0.5);
% set(h5,'color','#77AC30','LineWidth',0.5);
% legend('KF','EKF','UKF','AEKF','AEKF-SWF');
% xlabel('Error(cm)');
% ylabel('CDF');
% grid on
Reference in New Issue Copy Permalink