非高斯模型粒子滤波跟踪系统及其matlab仿真
Github个人博客：https://joeyos.github.io
卡尔曼滤波要求噪声模型是高斯的，而粒子滤波对噪声模型没有要求。目标跟踪是非线性问题。在实际情况中，由于目标的散射特性，雷达观测噪声不是高斯白噪声，而是尾部较长的“闪烁噪声”。因此，上述算法在实际应用中存在缺陷。考虑一般的雷达目标跟踪问题，设目标做匀速直线运动，雷达位于(x0，y0)。
非高斯粒子滤波模型
状态方程：X(k)=AX(k-1)+Gw(k-1)
式中，A为状态转移矩阵，G为过程噪声驱动矩阵，w(k)为过程噪声，X(k)=[x(k),x1(k),y(k),y1(k)]T为目标状态矢量。
观测方程：Z(k) = h(X(k)) + v(k-1)
h(X(k)) = [((x(k)-x0)2+(y(k)-y0)2)^0.5，arctan( (y(k)-y0) / (x(k)-x0) )]T，v(k)为观测噪声，理想情况下为零均值的高斯白噪声，实际情况为“闪烁噪声”。
假设目标在二维平民啊内匀速运动，初始位置为(50,50)km，初始速度为（0.3，-0.1）km/s，雷达位于坐标原点，PF中粒子数目N=300，采样周期为1s，做100次采样。针对两种观测噪声情况分别用PF算法做100次蒙特卡洛仿真，得出位置和速度的均方根误差。
（1）main.m
%说明：运用PF算法处理闪烁噪声情况下的雷达目标跟踪问题。
% 设目标作匀速直线运动，雷达位于(x0,y0)．
% 状态方程为:X(k)=PHI*X(k-1)+G*w(k-1);
% 雷达观测方程为:Z(k)=h(X(k))+v(k);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function main
M=100; %采样点数
T=1; %采样间隔
N=100; %粒子数
number=10; %蒙特卡洛仿真次数
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.3; %此参数控制噪声形式，=0为高斯噪声，非零为闪烁噪声
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];
%产生真实数据&量测
X=zeros(4,M);
Z=zeros(2,M);
Xn=zeros(2,M);
w=sqrtm(Q)*randn(2,M);
v=sqrtm(R)*randn(2,M);
X(:,1)=[x0,vx,y0,vy]'; %初始状态
Z(:,1)=feval('hfun',X(:,1),x0,y0)+v(:,1);
Xn(:,1)=ffun(Z(:,1),x0,y0);
for t=2:M
X(:,t)=feval('sfun',X(:,t-1),T)+G*w(:,t); %真实数据
Z(:,t)=feval('hfun',X(:,t),x0,y0)+v(:,t);
Xn(:,t)=ffun(Z(:,t),x0,y0); %量测
end
%粒子滤波估计初始化
Xmean_pf=zeros(number,4,M);
for i=1:number
Xmean_pf(i,:,1)=X(:,1)+randn(4,1);
end
%开始仿真（number次）
for j=1:number
%粒子集初始化
Xparticle_pf=zeros(4,M,N);
XparticlePred_pf=zeros(4,M,N);
zPred_pf=zeros(2,M,N);
weight=zeros(M,N); %粒子权值
%初始化
for i=1:N
Xparticle_pf(:,1,i)=[x0,vx,y0,vy]'+20*randn(4,1);
end
ww=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)*ww(:,t-1);
end
%重要性权值计算
for i=1:N
zPred_pf(:,t,i)=feval('hfun',XparticlePred_pf(:,t,i),x0,y0);
weight(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; %权值计算，为避免权值为0，用1e-99
end
weight(t,:)=weight(t,:)./sum(weight(t,:));%归一化权值
outIndex = randomR(1:N,weight(t,:)'); %随机采样
Xparticle_pf(:,t,:) = XparticlePred_pf(:,t,outIndex);%获取新采样值
%状态估计
mx=mean(Xparticle_pf(1,t,:));
my=mean(Xparticle_pf(3,t,:));
mvx=mean(Xparticle_pf(2,t,:));
mvy=mean(Xparticle_pf(4,t,:));
Xmean_pf(j,:,t)=[mx,mvx,my,mvy]';
end
end
%对numberci蒙特卡洛仿真求最终均值
Xpf=zeros(4,M);
for k=1:M
Xpf(:,k)=[mean(Xmean_pf(:,1,k)),mean(Xmean_pf(:,2,k)),...
mean(Xmean_pf(:,3,k)),mean(Xmean_pf(:,4,k))]';
end
%求粒子滤波估计状态与真实状态之间的偏差
Div_Of_Xpf_X=Xpf-X;
%求估计误差标准差，及RMSE
for k=1:M
sumX=zeros(4,1);
for j=1:number
sumX=sumX+(Xmean_pf(j,:,k)'-X(:,k)).^2;
end
RMSE(:,k)=sumX/number;
Div_Std_Xpf(:,k)=sqrt(RMSE(:,k)-Div_Of_Xpf_X(:,k).^2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1); %跟踪轨迹图
plot(X(1,:),X(3,:),'b',Xn(1,:),Xn(2,:),'g',Xpf(1,:),Xpf(3,:),'r');
legend('真实轨迹','观测轨迹','估计轨迹');
xlabel('X/m');ylabel('X/m');
figure(2);
subplot(2,2,1);plot(Div_Of_Xpf_X(1,:),'b');
ylabel('value/m');xlabel('(a) x方向位置估计误差均值曲线');
subplot(2,2,2);plot(Div_Of_Xpf_X(2,:),'b');
ylabel('value');xlabel('(b) x方向速度估计误差均值曲线');
subplot(2,2,3);plot(Div_Of_Xpf_X(3,:),'b');
ylabel('value/m');xlabel('(c) y方向位置估计误差均值曲线');
subplot(2,2,4);plot(Div_Of_Xpf_X(4,:),'b');
ylabel('value');xlabel('(d) y方向速度估计误差均值曲线');
figure(3);
subplot(2,2,1);plot(Div_Std_Xpf(1,:),'b');
ylabel('value');xlabel('(a) x方向位置估计误差标准差曲线');
subplot(2,2,2);plot(Div_Std_Xpf(2,:),'b');
ylabel('value');xlabel('(b) x方向速度估计误差标准差曲线');
subplot(2,2,3);plot(Div_Std_Xpf(3,:),'b');
ylabel('value');xlabel('(c) y方向位置估计误差标准差曲线');
subplot(2,2,4);plot(Div_Std_Xpf(4,:),'b');
ylabel('value');xlabel('(d) y方向速度估计误差标准差曲线');
figure(4);
subplot(2,2,1);plot(RMSE(1,:),'b');
ylabel('value');xlabel('(a) x方向位置估计误差均方根曲线');
subplot(2,2,2);plot(RMSE(2,:),'b');
ylabel('value');xlabel('(b) x方向速度估计误差均方根曲线');
subplot(2,2,3);plot(RMSE(3,:),'b');
ylabel('value');xlabel('(c) y方向位置估计误差均方根曲线');
subplot(2,2,4);plot(RMSE(4,:),'b');
ylabel('value');xlabel('(d) y方向速度估计误差均方根曲线');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
（2）ffun.m
% 程序说明： 求目标位置函数
% 输入参数： 观测站一次观测值x,观测站的位置（x0,y0)
% 输出参数： 目标的位置信息
function [y]=ffun(x,x0,y0)
y=zeros(2,1);
y(1)=x(1)*cos(x(2))+x0;
y(2)=x(1)*sin(x(2))+y0;
（3）hfun.m
% 程序说明： 观测方程函数
% 输入参数： x目标的状态，（x0,y0)是观测站的位置
% 输出参数： y是距离
function [y]=hfun(x,x0,y0)
y=zeros(2,1);
y(1)=sqrt((x(1)-x0)^2+(x(3)-y0)^2);
y(2)=atan2((x(3)-y0),((x(1)-x0)));
（4）randomR.m
% 随机采样子函数
function outIndex = randomR(inIndex,q)
if nargin < 2
error('Not enough input arguments.');
end
outIndex=zeros(size(inIndex));
[num,col]=size(q);
u=rand(num,1);
u=sort(u);
l=cumsum(q);
i=1;
for j=1:num
while (i<=num)&(u(i)<=l(j))
outIndex(i)=j;
i=i+1;
end
end
（5）sfun.m
function [y]=sfun(x,T)
phi=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
y=phi*x;
非高斯噪声情况：
高斯噪声情况：
此博客均属原创或译文，欢迎转载但请注明出处
Github个人博客：https://joeyos.github.io