%在二维空间,假设运动物体的一组(非线性)运动位置、速度、加速度数据,用粒子滤波方法进行处理
%实验室的博客
% 参数设置
N = 100;
Q = 5;
R = 5;
T = 10;
theta = pi/T;
distance = 80/T;
WorldSize = 100;
X = zeros(2, T);
Z = zeros(2, T);
P = zeros(2, N);
PCenter = zeros(2, T);
w = zeros(N, 1);
err = zeros(1,T);
X(:, 1) = [50; 20];
Z(:, 1) = [50; 20] + wgn(2, 1, 10*log10(R));
%初始化粒子群
for i = 1 : N
end
PCenter(:, 1) = sum(P, 2) / N;
%%
err(1) = norm(X(:, 1) - PCenter(:, 1));
figure(1);
set(gca,'FontSize',12);
hold on
plot(X(1, 1), X(2, 1), 'r.', 'markersize',30)
axis([0 100 0 100]);
plot(P(1, :), P(2, :), 'k.', 'markersize',5);
plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的中心位置
legend('True State', 'Particles', 'The Center of Particles');
title('Initial State');
hold off
%%
%开始运动
for k = 2 : T
end
%%
figure(3);
set(gca,'FontSize',12);
plot(X(1,:), X(2,:), 'r', Z(1,:), Z(2,:), 'g', PCenter(1,:), PCenter(2,:), 'b-');
axis([0 100 0 100]);
legend('True State', 'Measurement', 'Particle Filter');
xlabel('x', 'FontSize', 20); ylabel('y', 'FontSize', 20);
%%
figure(4);
set(gca,'FontSize',12);
plot(err,'.-');
xlabel('t', 'FontSize', 20);
title('The err');