红外图像弱小目标PF-TBD算法源程序% 将粒子滤波算法应用于红外弱小目标TBD问题，验证其检测、跟踪目标的有效性clear;clc% 粒子数目N = 2000;% 采样时间T = 1;% 仿真结束时间(采样总帧数)T_end = 30;% 假定目标从某一特定帧开始出现，然后在另一特定帧消失T_ap = 6;T_dp = 24;% 采样时间序列SimTime = zeros(floor(T_end/T),1);% 分辨单元数目N_x = 32; % 横向分布单元数目M_y = 32; % 纵向分布单元数目% 分辨单元的宽度Delta_X = 1;Delta_Y = 1;% 传感器的模糊参数值SIGMA = 0.7;% 目标初始出现概率mu = 0.05;% 目标速度区间vmin = 0.2;vmax = 1;% 目标强度(灰度值)区间Imin = 10;Imax = 30;% 抽样阈值(在大于r_th的区域内均匀分布)r_th = 2.5;% 扩散因子(目标影响相邻分辨单元的程度)p = 3;% 目标Markov 过程转移概率相关参数Pb = 0.05;Pd = 0.05;% 转移矩阵的表达式PI_T = [1-Pb,PbPd,1-Pd];% 转移矩阵PI 的行数(列数)PI_s = size(PI_T,1);% 系统状态转移矩阵Phi = [1,T,0,0,00,1,0,0,00,0,1,T,00,0,0,1,00,0,0,0,1];% 系统噪声协方差矩阵中的目标状态和灰度幅值噪声强度q1 = 0.001; % 目标状态变化强度q2 = 0.01; % 目标灰度值变化强度% 系统噪声协方差矩阵Q = [q1*T^3/3,q1*T^2/2,0,0,0q1*T^2/2,q1*T, 0,0,00,0,q1*T^3/3,q1*T^2/2,00,0,q1*T^2/2, q1*T, 00,0,0,0,q2*T];% 系统观测噪声R = 1.5^2;%*************************************% 变量取值初始化过程%*************************************% 定义滤波初值(假定目标出现时的初值)X = [4.2,0.45,7.2,0.25,20]';% 整个粒子的集合，其中第6位代表当前时刻E判决标识位，而第7位为上一时刻值X_PF = zeros(7,N);% 单个粒子状态值X_PF_i = zeros(7,1);% 初始时假定每个粒子的权值为均匀的w_i = 1/N*zeros(1,N);% 预测粒子的均值及其协方差X_PF_mean = zeros(5,1);Px_PF = Q;Xtru = zeros(5,floor(T_end/T));Xest_PF = zeros(5,floor(T_end/T));% 经PF估计出的目标出现概率Prob_PF = zeros(1,floor(T_end/T));% 每帧红外图像的观测值(以每个像素点为基准)Measure_i = zeros(N_x,M_y);H_i = zeros(N_x,M_y);% 为了计算各粒子的权值，生成初始时刻的红外观测图像for i=1:N_xfor j = 1:M_yMeasure_i(i,j) = sqrt(R)*randn(1);endend% 当考虑不设置阈值的情况时，此时假定目标会在所有的观测单元中随机出现；% 当考虑阈值时，在大于门限的区域内均匀分布(如何实现)% 由于假定目标初始出现概率为0.05，则可认为只有100个粒子的E值为1，其余均为0for i=1:20:2000% 假定每20个粒子出现一个E0=1的粒子X_PF_i = zeros(7,1);% 在测量值超过门限的区域中均匀地抽取粒子，首先找出超过门限的区域[PosRow,PosCol] = find(Measure_i>r_th);NPoints = length(PosRow);TarPos = ceil(rand(1)*NPoints);% 抽取粒子X_PF_i(1) = PosRow(TarPos);X_PF_i(2) = vmin + (vmax-vmin)*rand(1);X_PF_i(3) = PosCol(TarPos);X_PF_i(4) = vmin + (vmax-vmin)*rand(1);X_PF_i(5) = Imin + (Imax-Imin)*rand(1);X_PF_i(6) = 1;X_PF_i(7) = 0;% 保存的是上一时刻的E判决值X_PF(:,i) = X_PF_i;end% 得到各粒子的初始权值for n=1:Nif (X_PF(6,n) == 1)% 横向分辨单元下限确定(下同)Tar_X_min = floor(X_PF(1,n)-p);if (Tar_X_min<= 0)Tar_X_min = 1;end% 纵向分辨单元上限确定(下同)Tar_X_max = ceil(X_PF(1,n)+p);if (Tar_X_max>= N_x)Tar_X_max = N_x;end% 接下来确定Y方向上的目标扩散区域Tar_Y_min = floor(X_PF(3,n)-p);if (Tar_Y_min<= 0)Tar_Y_min = 1;endTar_Y_max = ceil(X_PF(3,n)+p);if (Tar_Y_max>= M_y)Tar_Y_max = M_y;end% 求取似然概率值PL = 1;for i=Tar_X_min:Tar_X_maxfor j=Tar_Y_min:Tar_Y_max% 根据文献公式，求解似然比值h =Delta_X*Delta_Y*X_PF(5,n)/(2*pi*SIGMA^2)*exp(-((X_PF(1,n)-i*Delta_X)^2+(X_ PF(3,n)-j*Delta_Y)^2)/(2*SIGMA^2));Prob = exp(-h*(h-2*Measure_i(i,j))/2/R);PL = PL*Prob;endend% 最终得到权值w_i(n) = PL;else% 当粒子出现标识位为0时，不进行判断，直接赋1w_i(n) = 1;endend% 对各粒子权值归一化处理w_i = w_i./sum(w_i);% % 绘制出初始的粒子分布图% ParPos = find(X_PF(1,:) ~= 0);% NPoint = length(ParPos);%% for i=1:NPoint% plot(X_PF(1,ParPos(i)),X_PF(3,ParPos(i)),'k*'),hold on% end%*****************************************% 开始整个的滤波和处理过程%*****************************************for k=1:T:T_end% 更新当前帧对应的仿真时刻SimTime(k) = (k-1)*T;% 初始化随机数发生器randn('state',sum(100*clock));%*****************************************% 在T_ap之前，由于没有目标出现%*****************************************if (k <T_ap)% 系统状态不发生变化，仅进行量测的更新for i=1:N_xfor j = 1:M_yMeasure_i(i,j) = sqrt(R)*randn(1);endend%*****************************************% 在T_ap之后，目标已经出现，且保持一段时间%*****************************************elseif (k <T_dp)% 目标尚未消失，观测中将带有目标信息，观测采用点扩散模型% 1. 首先更新当前时刻的目标状态w = sqrt(Q)*randn(5,1);X = Phi*X + w;% 2. 生成每个分辨单元中的观测值for i=1:N_xfor j = 1:M_y% 首先计算目标的点扩散效应，X(5)对应于目标的强度h =Delta_X*Delta_Y*X(5)/(2*pi*SIGMA^2)*exp(-((X(1)-i*Delta_X)^2+(X(3)-j*Delta_Y )^2)/(2*SIGMA^2));% 保存此时的信号扩散值H_i(i,j) = h;% 然后叠加测量噪声，生成当前分辨单元的测量值Measure_i(i,j) = h + sqrt(R)*randn(1);endendelse% 目标已经消失，恢复到原始的噪声观测for i=1:N_xfor j = 1:M_y% 分辨单元中仅有观测噪声Measure_i(i,j) = sqrt(R)*randn(1);endendendend%*****************************************% 在得到观测值后，对粒子更新%*****************************************% 1. 首先进行目标出现变量的转换c = zeros(2,3);for i = 1:PI_sc(i,1) = 0;for j = 2:PI_s+1% 进行转移概率矩阵计算c(i,j) = c(i,j-1) + PI_T(i,j-1);endend% 判断每个粒子的状态转换for s=1:N% 生成[0,1]上均匀分布的随机量un = rand(1);% 获取当前粒子的目标出现状态位i = X_PF(6,s)+1;m = 2;while (c(i,m) < un)m = m+1;end% 更新当前时刻的标志位X_PF(7,s) = X_PF(6,s);% 保存上一时刻的判断标志X_PF(6,s) = m-2;end% 2. 进行粒子更新for n = 1:N% 2.1 新生粒子的情况if ((X_PF(7,n) == 0) && (X_PF(6,n) == 1))X_PF_i = zeros(7,1);% 在测量值超过门限的区域中均匀地抽取粒子，首先找出超过门限的区域[PosRow,PosCol] = find(Measure_i>r_th);NPoints = length(PosRow);TarPos = ceil(rand(1)*NPoints);% 抽取粒子X_PF_i(1) = PosRow(TarPos);X_PF_i(2) = vmin + (vmax-vmin)*rand(1);X_PF_i(3) = PosCol(TarPos);X_PF_i(4) = vmin + (vmax-vmin)*rand(1);X_PF_i(5) = Imin + (Imax-Imin)*rand(1);X_PF_i(6) = 1;X_PF_i(7) = 0;% 保存的是上一时刻的E判决值X_PF(:,n) = X_PF_i;% 2.2 已存在粒子的情况elseif ((X_PF(7,n) == 1) && (X_PF(6,n) == 1))% 从转移概率处进行取值X_PF_mean = Phi*X_PF(1:5,n);% 抽取新的粒子X_PF_update = X_PF_mean + sqrt(Px_PF)*randn(5,1);% 完成粒子更新X_PF(1:5,n) = X_PF_update;end% 2.3 更新粒子的权值if (X_PF(6,n) == 1)% 计算似然比(考虑了目标扩散效应),首先确定X方向上的目标扩散区域% 横向分辨单元下限确定(下同)Tar_X_min = floor(X_PF(1,n)-p);if (Tar_X_min<= 0)Tar_X_min = 1;end% 纵向分辨单元上限确定(下同)Tar_X_max = ceil(X_PF(1,n)+p);if (Tar_X_max>= N_x)Tar_X_max = N_x;end% 接下来确定Y方向上的目标扩散区域Tar_Y_min = floor(X_PF(3,n)-p);if (Tar_Y_min<= 0)Tar_Y_min = 1;endTar_Y_max = ceil(X_PF(3,n)+p);if (Tar_Y_max>= M_y)Tar_Y_max = M_y;end% 求取似然概率值PL = 1;for i=Tar_X_min:Tar_X_maxfor j=Tar_Y_min:Tar_Y_max% 根据文献公式，求解似然比值h =Delta_X*Delta_Y*X_PF(5,n)/(2*pi*SIGMA^2)*exp(-((X_PF(1,n)-i*Delta_X)^2+(X_ PF(3,n)-j*Delta_Y)^2)/(2*SIGMA^2));Prob = exp(-0.5*h*(h-2*Measure_i(i,j))/R);PL = PL*Prob;endend% 最终得到权值w_i(n) = PL;else% 当粒子出现标识位为0时，不进行判断，直接赋1w_i(n) = 1;endend% 3. 进行权值的归一化处理w_i = w_i./sum(w_i);% 4. 经过上述处理，已经得到了PF-TBD滤波的值，接下来需要进行加权处理% 保存真实值if ((k >=T_ap) && (k <T_dp))% 只有处于目标出现时间段内，才进行状态保存，否则状态置为0 Xtru(:,k) = X;end% 保存经过粒子滤波后的值Xest_PF_UP = zeros(5,1);for id = 1:5Xest_PF_UP(id) = X_PF(id,:)*X_PF(6,:)'/sum(X_PF(6,:));endXest_PF(:,k) = Xest_PF_UP;% 保存当前时刻得到的目标出现概率估计值Prob_PF(k) = sum(X_PF(6,:))/N;% 5. 进行重采样的判断Neff = ceil(1/sum(w_i.^2)); if (Neff < N)% 有效样本点已经小于整个粒子数目，需要进行重采样c = zeros(1,N);c(1) = w_i(1);for s=2:Nc(s) = c(s-1) + w_i(s);end% 开始启动s = 1;% 抽取起始点u = zeros(1,N);un = rand(1)*1/N;for j=1:Nu(j) = un + (j-1)/N;while (u(j) > c(s))s = s+1;end% 设置样本点X_PF(:,j) = X_PF(:,s);% 设定权值w_i(j) = 1/N;endend% 6. 结束当前帧处理过程end% % 绘图% figure% % 绘图% axes1 = axes('FontSize',12, 'FontWeight','bold','Parent',gcf);% title(axes1,'\bf目标方位粒子滤波结果图');% xlabel(axes1,'\bfX方向');ylabel(axes1,'\bfY方向');box(axes1,'on');grid(axes1,'on');hold(axes1,'all'); %% plot1 = plot(Xtru(1,6:23),Xtru(3,6:23), 'LineWidth',2,'Parent',axes1);% plot2 = plot(Xest_PF(1,6:23),Xest_PF(3,6:23),'Color','k','%LineWidth',2,'LineStyle','-.','Parent',axes1);% 绘制出目标真实航迹与经过粒子滤波后的估计航迹figureplot(Xtru(1,6:23),Xtru(3,6:23),'r*'),holdon,plot(Xest_PF(1,6:23),Xest_PF(3,6:23),'ko');。