1.非线性条件下的仿真
% 说明: 运用序贯重要性抽样(SIR)算法对一广泛应用的标量模型进行滤波分析。
% 该模型的状态方程和观测方程分别为:
% x(t+1) = 0.5x(t) + [2.5x(t)]/[(1+x(t)^2)]+ 8cos(1.2t) + process noise;
% y(t) = x(t)^2/ 20 + measurement noise;
clear;
echo off;
% 初始化相关参数
% =============
N =50; % 采样点数
T=1; % 采样周期
t=1:1:N; % 考察时间
x0=0.1; % 初始状态
R=1; % 测量噪声方差
Q=10; % 过程噪声方差
initVar=5; % 初始状态方差
numSamples=500; % 粒子数
M=100; % Monte Carlo仿真次数
% 产生过程噪声和量测噪声
% =====================
v=sqrt(R)*randn(N,1);
w=sqrt(Q)*randn(N,1);
figure(1);
clf;
subplot(221);
plot(v);
ylabel('Measurement Noise','fontsize',15);
xlabel('Time');
subplot(222);
plot(w);
ylabel('Process Noise','fontsize',15);
xlabel('Time');
%产生真实状态和量测值
% ==================
x=zeros(N,1); % 真实状态
y=zeros(N,1); % 量测
x(1,1)=x0;
y(1,1)=(x(1,1)^2)./20 + v(1,1);
for k=2:N
x(k,1)=0.5*x(k-1,1) + 2.5*x(k-1,1)/(1+x(k-1,1)^(2)) + 8*cos(1.2*k) + w(k-1,1);
y(k,1)=(x(k,1).^(2))./20 + v(k,1);
end;
subplot(223);
plot(x);
ylabel('State x','fontsize',15);
xlabel('Time','fontsize',15);
subplot(224);
plot(y);
ylabel('Observation y','fontsize',15);
xlabel('Time','fontsize',15);
fprintf('Press a key to continue');
pause;
fprintf('\n');
fprintf('Simulation in progress');
fprintf('\n');
% 粒子滤波:
% ========
x_pf=zeros(numSamples,N);
xu_pf=zeros(numSamples,N);
y_pf=zeros(numSamples,N);
q_pf=zeros(numSamples,N);
% 初始采样:
% ========
x_pf(:,1)=x0+sqrt(initVar)*randn(numSamples,1);
y_pf(:,1)=x_pf(:,1).^2/20;
% 更新与预测过程:
% ==============
for k=2:N
%采样
w1=sqrt(Q).*randn(numSamples,1);
xu_pf(:,k)=0.5.*x_pf(:,k-1) + 2.5.*x_pf(:,k-1)./(1+x_pf(:,k-1).^2) + 8*cos(1.2*k) + w1;
%重要性权值计算
y_pf(:,k)=(xu_pf(:,k).^2)./20;
for i=1:numSamples
q_pf(i,k)=exp(-.5*R^(-1)*(y(k,1)- y_pf(i,k))^2);%省略了常数项
end
q_pf(i,k)=q_pf(i,k)/sum(q_pf(:,k));%归一化权值
%重采样
u=rand(numSamples,1);
u=sort(u);
l=cumsum(q_pf(:,k));
i=1;
for j=1:numSamples
while (i<=numSamples)&(u(i)<=l(j))
x_pf(i,k)=xu_pf(j,k);%量更新
i=i+1;
end
end
end
% 计算后验均值估计、最大后验估计及估计方差:
% ====================================
xmean_pf=mean(x_pf);% 后验均值估计
bins=20;
xmap_pf=zeros(N,1);
for k=1:N
[p,pos]=hist(x_pf(:,k,1),bins);
map=find(p==max(p));
xmap_pf(k,1)=pos(map(1));%最大后验估计
end;
%xrmse_pf=sqrt(sum((x'-xmean_pf).^2)/N);
for k=1:N
xstd_pf(1,k)=std(x_pf(:,k)-x(k,1));%后验误差标准差估计
end
%绘出结果:
% =======
figure(2);
clf;
plot(t,x,'b',t,xmean_pf,'r',t,xmap_pf,'g');
legend('True value','Posterior mean estimate','MAP estimate');
ylabel('State estimate','fontsize',15);
xlabel('Time','fontsize',15);
figure(3);
subplot(121);
plot(xmean_pf,x,'+')
ylabel('True state','fontsize',15)
xlabel('Posterior mean estimate','fontsize',15)
hold on;
c=-25:1:25;
plot(c,c,'r');
axis([-25 25 -25 25]);
hold off;
subplot(122);
plot(xmap_pf,x,'+')
ylabel('True state','fontsize',15)
xlabel('MAP estimate','fontsize',15)
hold on;
c=-25:1:25;
plot(c,c,'r');
axis([-25 25 -25 25]);
hold off;
% Plot histogram:
domain=zeros(numSamples,1);
range=zeros(numSamples,1);
parameter=1;
bins=10;
support=[-20:1:20];
figure(4);
hold on;
ylabel('Time','fontsize',15);
xlabel('Sample space','fontsize',15);
zlabel('Posterior density','fontsize',15);
v=[0 1];
caxis(v);
for k=1:2:N
[range,domain]=hist(x_pf(:,k,parameter),support);
waterfall(domain,k,range);
end;
figure(5);
plot(t,xstd_pf,'-');
xlabel('时间(t/s)');
ylabel('状态估计误差标准差');
axis([0,N,0,10]);
2.非高斯条件下粒子滤波的目标跟踪
%说明:运用PF算法处理闪烁噪声情况下的雷达目标跟踪问题。
% 设目标作匀速直线运动,雷达位于(x0,y0).
% 状态方程为:x(k)=PHI*x(k-1)+G*w(k-1);
% 雷达观测方程为:z(k)=h(x(k))+v(k);
function particlefilter
clear;
echo off;
%初始化相关参数
%==============
M=100; %采样点数
T=1; %采样间隔
N=300; %粒子数
number=100; %Monte Carlo仿真次数
x0=50000; %初始状态
y0=50000;
vx=300;
vy=-100;
delta_w=0.1; %过程噪声标准差
delta_r=50; %闪烁噪声下观测距离标准差
delta_theta1=1*pi/180; %热噪声对应方位角标准差
delta_theta2=5*pi/180; %闪烁效应对应方位角标准差
eta=0.2;
Q=delta_w^2*eye(2); %过程噪声方差阵
R1=diag([delta_r^2,delta_theta1^2]);
R2=diag([delta_r^2,delta_theta2^2]);
R=(1-eta)*R1+eta*R2; %测量噪声方差阵
G=[T^2/2,0;T,0;0,T^2/2;0,T];
resamplingScheme = 1;
xmean_pf=zeros(number,M);
ymean_pf=zeros(number,M);
vxmean_pf=zeros(number,M);
vymean_pf=zeros(number,M);
%开始仿真(100次)
%==============
for j=1:number
%%产生真实数据&量测
%===================
x=zeros(4,M);
z=zeros(2,M);
xn=zeros(2,M);
w=randn(2,M);
v=randn(2,M);
%w=delta_w*randn(M,2);
%vr=delta_r*randn(M,1);
%vtheta1=delta_theta1*randn(M,1);
%vtheta2=delta_theta2*randn(M,1);
x(:,1)=[x0,vx,y0,vy]';%初始状态
xn(:,1)=[x0,y0]';
for t=2:M
x(:,t)=feval('sfun',x(:,t-1),T)+G*sqrtm(Q)*w(:,t);%真实数据
z(:,t)=feval('hfun',x(:,t),x0,y0)+sqrtm(R)*v(:,t);
xn(:,t)=ffun(z(:,t),x0,y0);%量测
end
%%粒子滤波
%=========
xparticle_pf=zeros(4,M,N);
xparticlePred_pf=zeros(4,M,N);
zPred_pf=zeros(2,M,N);
W=zeros(M,N);
%初始化
for i=1:N
xparticle_pf(:,1,i)=[x0,vx,y0,vy]';
end
s=randn(2,M);
for t=2:M
%采样
for i=1:N
xparticlePred_pf(:,t,i)=feval('sfun',xparticle_pf(:,t-1,i),T)+G*sqrtm(Q)*s(:,t-1);
end
%重要性权值计算
for i=1:N
zPred_pf(:,t,i)=feval('hfun',xparticlePred_pf(:,t,i),x0,y0);
W(t,i)=(1-eta)*inv(sqrt(2*pi*det(R1)))*exp(-.5*(z(:,t)-zPred_pf(:,t,i))'*inv(R1)*(z(:,t)-zPred_pf(:,t,i)))...
+eta*inv(sqrt(2*pi*det(R2)))*exp(-.5*(z(:,t)-zPred_pf(:,t,i))'*inv(R2)*(z(:,t)-zPred_pf(:,t,i)))+ 1e-99;%权值计算
end
W(t,:)=W(t,:)./sum(W(t,:));%归一化权值
%重新采样(可选择不同方案)
if resamplingScheme==1
outIndex = residualR(1:N,W(t,:)'); % Residual resampling.
elseif resamplingScheme==2
outIndex = randomR(1:N,W(t,:)'); % random resampling.
elseif resamplingScheme==3
outIndex=systematicR(1:N,W(t,:)'); % Systematic resampling.
else
outIndex=multinomialR(1:N,W(t,:)'); % Multinomial resampling.
end;
xparticle_pf(:,t,:) = xparticlePred_pf(:,t,outIndex); % 获取新采样值
end
%%状态估计
%=========
for t=1:M
xmean_pf(j,t)=mean(xparticle_pf(1,t,:));
ymean_pf(j,t)=mean(xparticle_pf(3,t,:));
vxmean_pf(j,t)=mean(xparticle_pf(2,t,:));
vymean_pf(j,t)=mean(xparticle_pf(4,t,:));
end
end
xrmse_pf=zeros(1,M);
yrmse_pf=zeros(1,M);
vxrmse_pf=zeros(1,M);
vyrmse_pf=zeros(1,M);
ex_pf=zeros(1,M);
ey_pf=zeros(1,M);
evx_pf=zeros(1,M);
evy_pf=zeros(1,M);
%%状态估计均方根
%===============
for t=1:M
ex_pf(1,t)=mean(xmean_pf(:,t)-x(1,t));
ey_pf(1,t)=mean(ymean_pf(:,t)-x(3,t));
evx_pf(1,t)=mean(vxmean_pf(:,t)-x(2,t));
evy_pf(1,t)=mean(vymean_pf(:,t)-x(4,t));
xstd_pf(1,t)=sqrt(sum((xmean_pf(:,t)-x(1,t)).^2)/number-ex_pf(1,t)^2);
ystd_pf(1,t)=sqrt(sum((ymean_pf(:,t)-x(3,t)).^2)/number-ey_pf(1,t)^2);
vxstd_pf(1,t)=sqrt(sum((vxmean_pf(:,t)-x(2,t)).^2)/number-evx_pf(1,t)^2);
vystd_pf(1,t)=sqrt(sum((vymean_pf(:,t)-x(4,t)).^2)/number-evy_pf(1,t)^2);
xrmse_pf(1,t)=sqrt(sum((xmean_pf(:,t)-x(1,t)).^2)/number);
yrmse_pf(1,t)=sqrt(sum((ymean_pf(:,t)-x(3,t)).^2)/number);
vxrmse_pf(1,t)=sqrt(sum((vxmean_pf(:,t)-x(2
评论0