Score
0
Watch 6 Star 16 Fork 5

Nrush / EKF_SLAMMatlabMIT

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
Clone or download
analysis.m 6.33 KB
Copy Edit Web IDE Raw Blame History
Nrush authored 2019-08-29 11:23 . init
close all
clear all;
% load('sim_result_iekf_map3.mat')
load('sim_result.mat')
IEKF_pre_trajectory = sim_result.EKF_pre_trajectory;
load('sim_result_color_noise_map3.mat')
EKF_pre_trajectory_color = sim_result.EKF_pre_trajectory;
load('sim_result_asy_map3.mat')
EKF_pre_trajectory_asy = sim_result.EKF_pre_trajectory;
load('sim_result_less_landmark_map3.mat')
EKF_pre_trajectory_ls = sim_result.EKF_pre_trajectory;
% load('sim_result_ekf_map3.mat')
% load('sim_result_less_landmark_map3.mat')
load('sim_result_color_noise_map3.mat')
ture_trajectory = sim_result.ture_trajectory;
model_pre_trajectory = sim_result.model_pre_trajectory;
EKF_pre_trajectory = sim_result.EKF_pre_trajectory;
landmarks = sim_result.landmarks;
wp = sim_result.wp;
length = size(ture_trajectory,2);
figure
axis equal
hold on;
% 画出landmarks
scatter( landmarks(1, :), landmarks(2, :), 'b*' )
% 画出历史轨迹
truep = plot( ture_trajectory(1, :), ture_trajectory(2, :), 'k--','linewidth',3);
% 画出历史EKF预测轨迹
ekfp = plot( EKF_pre_trajectory(1, :), EKF_pre_trajectory(2, :), 'r','linewidth',3 );
% 画出历史model预测轨迹
modelp = plot( model_pre_trajectory(1, :), model_pre_trajectory(2, :), 'b-.','linewidth',3 );
% 画出车的位姿
xtrue = ture_trajectory(:,length-1);
draw_car(xtrue,5,'k');
% EKF预测位姿
x = EKF_pre_trajectory(:,length-1);
draw_car(x,5,'r');
% 模型预测位姿
x_model_pre = model_pre_trajectory(:,length-1);
draw_car(x_model_pre,5,'g');
%图例
legend([truep ekfp,modelp],'true','ekf','model');
hold off;
e_ekf = EKF_pre_trajectory - ture_trajectory;
e_ekf(3,:) = pi_to_pi(e_ekf(3,:));
e_model = model_pre_trajectory - ture_trajectory;
e_model(3,:) = pi_to_pi(e_model(3,:));
e_ekf = abs(e_ekf);
e_model = abs(e_model);
fig2 = figure;
hold on
ticks = 1:1:length;
subplot(3,1,1)
hold on
plot(ticks,e_ekf(1,:),'linewidth',2);
plot(ticks,e_model(1,:),'linewidth',2);
xlabel("ticks")
ylabel("|x-error|")
subplot(3,1,2)
hold on
plot(ticks,e_ekf(2,:),'linewidth',2);
plot(ticks,e_model(2,:),'linewidth',2);
xlabel("ticks")
ylabel("|y-error|")
subplot(3,1,3)
hold on
plot(ticks,pi_to_pi(e_ekf(3,:)),'linewidth',2);
plot(ticks,pi_to_pi(e_model(3,:)),'linewidth',2);
xlabel("ticks")
ylabel("|\phi-error|")
legend('ekf-error','model-error');
ekf_eN2 = zeros(1,length);
model_eN2 = zeros(1,length);
for i=1:1:length
ekf_eN2(i) = norm(e_ekf(:,i),2);
model_eN2(i) = norm(e_model(:,i),2);
end
hold off
fig3 = figure;
hold on
plot(ticks,ekf_eN2,'linewidth',2);
plot(ticks,model_eN2,'linewidth',2);
xlabel("ticks")
ylabel("error")
legend('ekf-error','model-error');
e_ekf_ls = EKF_pre_trajectory_ls - ture_trajectory;
e_ekf_ls(3,:) = pi_to_pi(e_ekf_ls(3,:));
e_ekf_ls = abs(e_ekf_ls);
fig4 = figure;
hold on
ticks = 1:1:length;
subplot(3,1,1)
hold on
plot(ticks,e_ekf(1,:),'linewidth',2);
plot(ticks,e_ekf_ls(1,:),'linewidth',2);
xlabel("ticks")
ylabel("|x-error|")
subplot(3,1,2)
hold on
plot(ticks,e_ekf(2,:),'linewidth',2);
plot(ticks,e_ekf_ls(2,:),'linewidth',2);
xlabel("ticks")
ylabel("|y-error|")
subplot(3,1,3)
hold on
plot(ticks,pi_to_pi(e_ekf(3,:)),'linewidth',2);
plot(ticks,pi_to_pi(e_ekf_ls(3,:)),'linewidth',2);
xlabel("ticks")
ylabel("|\phi-error|")
legend('ekf','ekf-less-observation');
ekf_ls_eN2 = zeros(1,length);
for i=1:1:length
ekf_ls_eN2(i) = norm(e_ekf_ls(:,i),2);
end
fig5 = figure;
hold on
plot(ticks,ekf_eN2,'linewidth',2);
plot(ticks,ekf_ls_eN2,'linewidth',2);
xlabel("ticks")
ylabel("error")
legend('ekf-error','ekf-less-observation');
fig6 = figure;
axis equal
hold on;
% 画出landmarks
scatter( landmarks(1, :), landmarks(2, :), 'b*' );
% 画出历史轨迹
truep = plot( ture_trajectory(1, :), ture_trajectory(2, :), 'k--','linewidth',3);
% 画出历史EKF预测轨迹
ekfp = plot( EKF_pre_trajectory(1, :), EKF_pre_trajectory(2, :), 'r','linewidth',3 );
ekfpls = plot( EKF_pre_trajectory_ls(1, :), EKF_pre_trajectory_ls(2, :), 'b-.','linewidth',3);
%图例
legend([truep,ekfp ekfpls],'true','ekf','ekf-less-observation');
fig7 = figure;
axis equal
hold on;
% 画出landmarks
scatter( landmarks(1, :), landmarks(2, :), 'b*' );
% 画出历史轨迹
truep = plot( ture_trajectory(1, :), ture_trajectory(2, :), 'k--','linewidth',3);
% 画出历史EKF预测轨迹
ekfp = plot( EKF_pre_trajectory(1, :), EKF_pre_trajectory(2, :), 'r','linewidth',3 );
ekfasyp = plot( EKF_pre_trajectory_asy(1, :), EKF_pre_trajectory_asy(2, :), 'b-.','linewidth',3);
%图例
legend([truep ekfp,ekfasyp],'true','ekf','ekf-asynchronous');
fig8 = figure;
axis equal
hold on;
% 画出landmarks
scatter( landmarks(1, :), landmarks(2, :), 'b*' );
% 画出历史轨迹
truep = plot( ture_trajectory(1, :), ture_trajectory(2, :), 'k--','linewidth',3);
% 画出历史EKF预测轨迹
ekfp = plot( EKF_pre_trajectory(1, :), EKF_pre_trajectory(2, :), 'r','linewidth',3 );
ekfcolorp = plot( EKF_pre_trajectory_color(1, :), EKF_pre_trajectory_color(2, :), 'b-.','linewidth',3);
%图例
legend([truep ekfp,ekfcolorp],'true','ekf','ekf-color');
fig9 = figure;
axis equal
hold on;
% 画出landmarks
scatter( landmarks(1, :), landmarks(2, :), 'b*' );
% 画出历史轨迹
truep = plot( ture_trajectory(1, :), ture_trajectory(2, :), 'k--','linewidth',3);
% 画出历史EKF预测轨迹
ekfp = plot( EKF_pre_trajectory(1, :), EKF_pre_trajectory(2, :), 'r','linewidth',3 );
iekfcolorp = plot( IEKF_pre_trajectory(1, :), IEKF_pre_trajectory(2, :), 'b-.','linewidth',3);
%图例
legend([truep ekfp,iekfcolorp],'true','ekf','iekf');
e_iekf = IEKF_pre_trajectory - ture_trajectory;
e_iekf(3,:) = pi_to_pi(e_iekf(3,:));
e_iekf = abs(e_iekf);
fig10 = figure;
hold on
ticks = 1:1:length;
subplot(3,1,1)
hold on
plot(ticks,e_ekf(1,:),'linewidth',2);
plot(ticks,e_iekf(1,:),'linewidth',2);
xlabel("ticks")
ylabel("|x-error|")
subplot(3,1,2)
hold on
plot(ticks,e_ekf(2,:),'linewidth',2);
plot(ticks,e_iekf(2,:),'linewidth',2);
xlabel("ticks")
ylabel("|y-error|")
subplot(3,1,3)
hold on
plot(ticks,pi_to_pi(e_ekf(3,:)),'linewidth',2);
plot(ticks,pi_to_pi(e_iekf(3,:)),'linewidth',2);
xlabel("ticks")
ylabel("|\phi-error|")
legend('ekf-error','iekf-error');
iekf_eN2 = zeros(1,length);
for i=1:1:length
iekf_eN2(i) = norm(e_iekf(:,i),2);
end
hold off
fig11 = figure;
hold on
plot(ticks,ekf_eN2,'linewidth',2);
plot(ticks,iekf_eN2,'linewidth',2);
xlabel("ticks")
ylabel("error")
legend('ekf-error','iekf-error');
% mean(model_eN2(:))
% mean(ekf_eN2(:))
% mean(ekf_ls_eN2(:))
% mean(e_ekf_ls(3,:))
% mean(e_ekf_ls(2,:))
% mean(e_ekf_ls(1,:))
%
% mean(e_ekf(3,:))
% mean(e_ekf(2,:))
% mean(e_ekf(1,:))

Comment ( 0 )

Sign in for post a comment