clc
close all
clear all
for loop=1:1
snr=7;
sigma=1;
Ak=sqrt(10^(snr/10))*sigma;
T=1;
q1=0.001;
q2=0.01;
F = [1 1 0 0 0; 0 1 0 0 0; 0 0 1 1 0; 0 0 0 1 0; 0 0 0 0 1 ];
Q = [ q1*T^3/3 q1*T^2/2 0 0 0;
q1*T^2/2 q1*T 0 0 0;
0 0 q1*T^3/3 q1*T^2/2 0;
0 0 q1*T^2/2 q1*T 0;
0 0 0 0 q2*T];
initx = [4.2 .45 4.7 .25 Ak]'; % initial state value
T1 = 30;
[xx] = state_sample(F, Q, initx, T1); % generate state data
state=xx;
epsilon=0.7;
T2=30;
Nx=20;
Ny=20;
[Z_ob]=measurement_sample(xx,Nx,Ny,snr,epsilon,T2,F,Ak,sigma); %generate measurement data
% for i=1:30
% figure(11);
% mesh(Z_ob{i});
% end
%
% pause;
% i
% end
P=[.95 .05;.05,.95];
N=2000; %particle number
targetexistencestate=2; %one target exist and no target exist.
%=================Initial========================
E(1,:)=target_existence_transition(P,N,targetexistencestate)';
Thre=1.5;
Vxmax=.6;
Vymax=.6;
Imin=0;
Imax=5;
s=zeros(5,N,T2);
%=============================================
k=1;
for n=1:N
temp=1;
if E(1,n)==1; % only compute the important weight for Ek=1;
s(:,n,k)=[unifrnd(2,15,1,1) ,unifrnd(-Vxmax,Vxmax,1,1), unifrnd(2,14,1,1), unifrnd(-Vymax,Vymax,1,1), unifrnd(Imin,Imax,1,1)]';
[x,y]=meshgrid(1:Nx,1:Ny);
hkij=(s(5,n,k)*exp(-((x-s(1,n,k)).^2+(y-s(3,n,k)).^2)/(2*epsilon^2))); %Intensity(k,n
ix=round(s(1,n,k));
iy=round(s(3,n,k));
for i=max(ix-3,1):min(ix+3,Nx)
for j=max(iy-3,1):min(iy+3,Ny)
temp=temp*exp(-hkij(i,j)*(hkij(i,j)-2*Z_ob{k}(i,j))/(2*sigma^2));
end
end
w(n,k)=temp;
else
w(n,k)=temp;
end
end
Weight = w(:,k)/sum(w(:,k)); % Normalize the importance weights
ii = residualR(1:N,Weight);
E(k,:)=E(k,ii);
s(:,:,k)=s(:,ii,k);
Pd(k)=sum(E(k,:))/N
x_est(k)=sum(s(1,:,k).*E(k,:))/sum(E(k,:));
y_est(k)=sum(s(3,:,k).*E(k,:))/sum(E(k,:));
k
for k=2:T2
E(k,:)=target_existence_transition(P,N,targetexistencestate,E(k-1,:)); %
for n=1:N
temp=1;
if E(k-1,n)==0&&E(k,n)==1;
s(:,n,k)=[unifrnd(2,15,1,1) unifrnd(-Vxmax,Vxmax,1,1) unifrnd(2,14,1,1) unifrnd(-Vymax,Vymax,1,1) unifrnd(Imin,Imax,1,1)]';
[x,y]=meshgrid(1:Nx,1:Ny);
hkij=(s(5,n,k)*exp(-((x-s(1,n,k)).^2+(y-s(3,n,k)).^2)/(2*epsilon^2))); %Intensity(k,n
ix=round(s(1,n,k));
iy=round(s(3,n,k));
for i=max(ix-3,1):min(ix+3,Nx)
for j=max(iy-3,1):min(iy+3,Ny)
temp=temp*exp(-hkij(i,j)*(hkij(i,j)-2*Z_ob{k}(i,j))/(2*sigma^2));;
end
end
w(n,k)=temp;
else if E(k-1,n)==1&&E(k,n)==1;
E_flag=1;
initx = s(:,n,k-1);
s(:,n,k)=state_updat(F, Q, initx ,1,Nx,Ny);
[x,y]=meshgrid(1:Nx,1:Ny);
hkij=(s(5,n,k)*exp(-((x-s(1,n,k)).^2+(y-s(3,n,k)).^2)./(2*epsilon^2))); %Intensity(k,n
ix=round(s(1,n,k));
iy=round(s(3,n,k));
for i=max(ix-3,1):min(ix+3,Nx)
for j=max(iy-3,1):min(iy+3,Ny)
temp=temp*exp(-hkij(i,j)*(hkij(i,j)-2*Z_ob{k}(i,j))/(2*sigma^2));
end
end
w(n,k)=temp;
else
w(n,k)=temp;
end
end
end
Weight = w(:,k)/sum(w(:,k)); % Normalize the importance weights
ii = residualR(1:N,Weight);
ii=systematicR(1:N,Weight);
E(k,:)=E(k,ii);
s(:,:,k)=s(:,ii,k);
Pd(k)=sum(E(k,:))/N
x_est(k)=sum(s(1,:,k).*E(k,:))/sum(E(k,:));
y_est(k)=sum(s(3,:,k).*E(k,:))/sum(E(k,:));
k
end
Pd_loop(loop,:)=Pd;
x_est_loop(loop,:)=x_est;
y_est_loop(loop,:)=y_est;
%x_rmse=sqrt(sum(mean(x_est(k)-(s(1,:,k)))/sum(E(k,:))
end
完整代码链接:https://pan.baidu.com/s/1laAXcJcy2hRQSOe4_om03w
提取码:3wa5