角域重采样matlab程序,粒子滤波程序重采样代码求助

clc

clear all

close all

% Particle filter example, adapted from Gordon, Salmond, and Smith paper.

x = 0.1; % initial state

Q = 1; % process noise covariance

R = 1; % measurement noise covariance

tf = 50; % simulation length

N = 200; % number of particles in the particle filter

xhat = x;

P = 2;

xhatPart = x;

% Initialize the particle filter.

for i = 1 : N

xpart(i) = x + sqrt(P) * randn;

end

xArr = [x];

yArr = [x^2 / 20 + sqrt(R) * randn];

xhatArr = [x];

PArr = [P];

xhatPartArr = [xhatPart];

for k = 1 : tf

% System simulation

x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%×′ì?·?3ì

y = x^2 / 20 + sqrt(R) * randn;%1?2a·?3ì

% Particle filter

for i = 1 : N

xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;

ypart = xpartminus(i)^2 / 20;

vhat = y - ypart;%1?2aoí?¤2aμ?2?

q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);

end

% Normalize the likelihood of each a priori estimate.

qsum = sum(q);

for i = 1 : N

q(i) = q(i) / qsum;%1éò??ˉè¨??

end

% Resample.

for i = 1 : N

u = rand; % uniform random number between 0 and 1

qtempsum = 0;

for j = 1 : N

qtempsum = qtempsum + q(j);

if qtempsum >= u

xpart(i) = xpartminus(j);

break;

end

end

end

% The particle filter estimate is the mean of the particles.

xhatPart = mean(xpart);

% Save data in arrays for later plotting

xArr = [xArr x];

yArr = [yArr y];

xhatPartArr = [xhatPartArr xhatPart];

end

t = 0 : tf;

figure;

plot(t, xArr, 'b.', t, xhatPartArr, 'k-');

set(gca,'FontSize',12); set(gcf,'Color','White');

xlabel('time step'); ylabel('state');

legend('True state', 'Particle filter estimate');

xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);

disp(['Particle filter RMS error = ', num2str(xhatPartRMS)]);

疑问：重采样部分的if qtempsum>=u有什么用处呢，为什么不用if q(j)>=u来判断呢？程序中每次判断都要考虑之前粒子权重的和，对此我列了下面数据的可能，每次采样的粒子并不是最大权重粒子啊，比如xpart(1)应该选q(2)=0.29,而此时qtempsum=0.30,刚好没超过u0.37的值，从而选侧下一个次优粒子，哪位达人给帮帮忙啊，不胜感谢了！

i=1  u=0.37    j=1  q(1)=0.01           qtempsum=0.01

j=2  q(2)=0.29           qtempsum=0.30

j=3  q(3)=0,10           qtempsum=0.40

xpart(1)=0.10

i=2  u=0.28    j=1  q(1)=0.01            qtempsum=0.01

j=2  q(2)=0.29            qtempsum=0.30

xpart(2)=0.29

i=3  u=0.64     j=1  q(1)=0.01            qtempsum=0.01

j=2  q(2)=0.29            qtempsum=0.30

j=3  q(3)=0.10            qtempsum=0.40

j=4  q(4)=0.08            qtempsum=0.48

j=5  q(5)=0.09            qtempsum=0.57

j=5  q(6)=0.09            qtempsum=0.65

xpart(3)=0.09

% Resample.

for i = 1 : N

u = rand; % uniform random number between 0 and 1

qtempsum = 0;

for j = 1 : N

qtempsum = qtempsum + q(j);

if qtempsum >= u

xpart(i) = xpartminus(j);

break;

end

end

end