⼆维粒⼦滤波纯代码
% 参数设置
N = 100; %粒⼦总数
Q = 5; %过程噪声
R = 5; %测量噪声
T = 20; %测量时间
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]; %初始系统状态
%wgn（m,n,p）产⽣⼀个m⾏n列的⾼斯⽩噪声的矩阵，p以dBW为单位指定输出噪声的强度。
Z(:, 1) = [50; 20] + wgn(2, 1, 10*log10(R)); %初始系统的观测状态
%初始化粒⼦
for i = 1 : N
P(:, i) = [WorldSize*rand; WorldSize*rand];%在worldSize区域内随机⽣成N个粒⼦
dist = norm(P(:, i)-Z(:, 1)); %与测量位置相差的距离，⽤于估算该粒⼦的权重
%由于上⾯已经随机⽣成了N个粒⼦，现在将其与真实的测量值z进⾏⽐较，越接近则权重越⼤，或者说差值越⼩权重越⼤
%这⾥的权重计算是关于p(z/x)的分布，即观测⽅程的分布，假设观测噪声满⾜⾼斯分布，那么w（i）=p(z/x)
w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); %求权重
end
PCenter(:, 1) = sum(P, 2) / N; %所有粒⼦的⼏何中⼼位置
%%
err(1) = norm(X(:, 1) - PCenter(:, 1)); %粒⼦⼏何中⼼与系统真实状态的误差
figure(1);
%利⽤set(gca,'propertyname','propertyvalue'......)命令可以调整图形的坐标属性。
%propertyname和property value分别为坐标属性名称（不区分字母⼤⼩写）及对应的属性值。
set(gca,'FontSize',10);
hold on%使当前及图形保持⽽不被刷新，准备接受此后将绘制的新曲线
plot(X(1, 1), X(2, 1), 'r.', 'markersize',30) %系统状态位置
%axis[xmin,xmax,ymin,ymax]设定坐标范围，必须满⾜xmin<xmax，后⾯同理
axis([0 100 0 100]);
plot(P(1, :), P(2, :), 'kx', 'markersize',5); %绘出各个粒⼦位置
plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒⼦的⼏何中⼼位置
%图形标注函数legend(string1,)，在当前图中添加图例
正则化粒子滤波legend('True State', 'Particles', 'The Center of Particles');
%添加图形标题命令title('string')，在当前坐标系的顶部加⼀个⽂本串string，作为该图形的标题
title('Initial State');
%使当前轴及图形不再具备不被刷新的性质
hold off
%%
%开始运动
for k = 2 : T
%模拟⼀个弧线运动的状态
%wgn（m,n,p）产⽣⼀个m⾏n列的⾼斯⽩噪声的矩阵，p以dBW为单位指定输出噪声的强度。
X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10*log10(Q)); %状态⽅程
Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R)); %观测⽅程
%粒⼦滤波
%预测
for i = 1 : N
%将之前⽣成的粒⼦带⼊到状态⽅程，进⾏下⼀步状态的预测
P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));
dist = norm(P(:, i)-Z(:, k)); %与测量位置相差的距离，⽤于估算该粒⼦的权重
%由于上⾯已经随机⽣成了N个粒⼦，现在将其与真实的测量值z进⾏⽐较，越接近则权重越⼤，或者说差值越⼩权重越⼤
%这⾥的权重计算是关于p(z/x)的分布，即观测⽅程的分布，假设观测噪声满⾜⾼斯分布，那么w（i）=p(z/x)
w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); %求权重
end
%归⼀化权重
wsum = sum(w);
for i = 1 : N
w(i) = w(i) / wsum;
end
%重采样（更新）
for i = 1 : N
wmax = 2 * max(w) * rand; %另⼀种重采样规则，获取权重当中的最⼤值与均匀分布在0-1之间数的乘积再乘以2作为后⾯判断选出⼤权重粒⼦的依据 %randi()函数⽣成均匀分布的伪随机整数，范围为imin--imax，如果没指定imin，则默认为1
%r = randi([imin,imax],...)返回⼀个在[imin,imax]范围内的伪随机整数
index = randi(N, 1);
%在这⾥随机产⽣N个粒⼦当中的某个粒⼦，然后选择该粒⼦的权值，判断是否是⼤的粒⼦权重，是的话就把该粒⼦选出来
while(wmax > w(index))
%使vmax递减，不然的话单个粒⼦的权重是不可能⼤于vmax的值的
wmax = wmax - w(index);
index = index + 1;
%如果从某个随机的粒⼦开始，没到从该粒⼦开始到最后的粒⼦权值总和⼤于vmax，那么就重新从第⼀个粒⼦开始查if index > N
index = 1;
end
end
P(:, i) = P(:, index); %从上⾯的index中得到新粒⼦
end
%sum(X,2)表⽰把X按⾏求和；如果是sum(X)，那就是按列求和
PCenter(:, k) = sum(P, 2) / N; %所有粒⼦的⼏何中⼼位置
%计算误差
err(k) = norm(X(:, k) - PCenter(:, k)); %粒⼦⼏何中⼼与系统真实状态的误差
figure(2);
set(gca,'FontSize',12);
%clf; ⽤来清除图形的命令。⼀般在画图之前⽤
clf;
hold on
plot(X(1, k), X(2, k), 'r.', 'markersize',50); %系统状态位置
axis([0 100 0 100]);
plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒⼦位置
plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒⼦的中⼼位置
legend('True State', 'Particle', 'The Center of Particles');
hold off
pause(0.1);
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');
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论