非高斯模型粒子滤波跟踪系统及其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;

非高斯噪声情况:
非高斯模型粒子滤波跟踪系统及其matlab仿真_第1张图片
非高斯模型粒子滤波跟踪系统及其matlab仿真_第2张图片
非高斯模型粒子滤波跟踪系统及其matlab仿真_第3张图片
非高斯模型粒子滤波跟踪系统及其matlab仿真_第4张图片

高斯噪声情况:
非高斯模型粒子滤波跟踪系统及其matlab仿真_第5张图片
非高斯模型粒子滤波跟踪系统及其matlab仿真_第6张图片
非高斯模型粒子滤波跟踪系统及其matlab仿真_第7张图片
非高斯模型粒子滤波跟踪系统及其matlab仿真_第8张图片

此博客均属原创或译文,欢迎转载但请注明出处
Github个人博客:https://joeyos.github.io

你可能感兴趣的:(目标检测与跟踪)