% 绘制粒子搜索历史图
figure;
for i = 1:N_particles
plot3(squeeze(particles_history(i,:,3)), squeeze(particles_history(i,:,1)), squeeze(particles_history(i,:,2)), 'linewidth', 1);
hold on;
end
title('Particle Search History');
xlabel('Time');
ylabel('Value');
zlabel('Weight');
grid on;
hold off;