PDA算法Matlab程序（精编版）

1、一、测试程序% pda-fa算法实现% 何友雷达数据处理及应用p116 % 二维空间匀速直线运动，状态向量为x=x,vx,y,vy % x1=x0+vxt % y1=y0+vyt % 仿真：% 1、改变虚假量测数量nc：公式求取、手动设置% 2、改变量测噪声r=r 0; 0 r，即 r % 3、改变虚假量测位置q，偏离真实位置的程度% 4、关联概率计算clc; clear; close all; %* % 参数设置%* i=eye(4); t = 1; %采样间隔simtime = 100 ; %仿真步数a=1 t 0 0;0 1 0 0;0 0 1 t;0 0 0 1; %实际模型： cv

2、h=1 0 0 0;0 0 1 0; %测量模型q=0; %实际过程噪声g = t2/2 0; t 0; 0 t2/2; 0 t; %噪声加权矩阵r=200; r=r 0; 0 r; %量测噪声x0=200;0;10000;-15; %初始状态x(:,1)=x0; vk=sqrt(r)*randn;sqrt(r)*randn; zk(:,1)=h*x(:,1)+vk; gama=16; lamda=0.0004; %* % 量测生成%* for i=2:1:simtime x(:,i)=a*x(:,i-1); % 真实状态 vk=sqrt(r)*randn;sqrt(r)*randn; zk(

3、:,i)=h*x(:,i)+vk; %生成量测值end %* % pda初始化%* xk_pda=200;0;10100;-16; %初始状态、与实际值略有差别r11=r; r22=r; r12=0; r21=0; pkk_pda=r11 r11/t r12 r12/t; r11/t 2*r11/t2 r12/t 2*r12/t2; r21 r21/t r22 r22/t; r21/t 2*r21/t2 r22/t 2*r22/t2; %初始协方差xkk = xk_pda ; pkk = pkk_pda; x_pre = a*xkk; p_pre=a*pkk*a+g*q*g; p=r; for

4、 i=1:1:simtime %* % 产生杂波 %* % 量测确认区域面积 sk=h*p_pre*h+ p; av=pi*gama*sqrt(det(sk); % 准备生成杂波数目 nc=floor(10*av*lamda+1);%设置杂波数量 q=sqrt(av)/2; %q=sqrt(10*av)/2; a=x(1,i)-q; b=x(1,i)+q; c=x(3,i)-q; d=x(3,i)+q; % 生成代表杂波的nc个虚假量测 xi=a+(b-a)*rand(1,nc); yi=c+(d-c)*rand(1,nc); clear z_matrix; clear pz_matrix;

5、for j=1:nc z_matrix(:,j) = xi(j);yi(j); end z_matrix(:,nc+1)=zk(:,i); pz_matrix = cat(3); for j=1:1:nc pz_matrix = cat(3,pz_matrix,q,0;0,q); end pz_matrix = cat(3,pz_matrix,r); %* % pda关联 %* z_predict = h*x_pre; pz_predict = h*p_pre*h ; combine_z,combine_r=pda(z_matrix, pz_matrix, z_predict, pz_pred

6、ict) ; % pda z_pda(:,i) = combine_z ; %* % 卡尔曼滤波 %* p=combine_r; xk_pda,pk_pda,kk_pda=kalman(xkk,pkk,combine_z,a,g,q,h,p); xkk=xk_pda; pkk=pk_pda; % 预测 x_pre=a*xkk; p_pre=a*pkk*a+g*q*g; %出各个状态值 ex_pda(i)=xkk(1); evx_pda(i)=xkk(2); ey_pda(i)=xkk(3); evy_pda(i)=xkk(4); error1_pda(i)=ex_pda(i)-x(1,i);%

7、pkk(1,1); error2_pda(i)=ey_pda(i)-x(3,i);%pkk(2,2); error3_pda(i)=evx_pda(i)-x(2,i);%pkk(3,3); error4_pda(i)=evy_pda(i)-x(4,i);%pkk(4,4); end %* % 绘图%* i=1:simtime; figure plot(x(1,i),x(3,i),-,linewidth,2); %真实值grid on; hold on plot(ex_pda(1,i),ey_pda(1,i),r-,linewidth,2); %滤波值plot(zk(1,i),zk(2,i),*

8、); %实际测量值plot(z_pda(1,i),z_pda(2,i),o); %组合测量值legend(真实值 ,滤波值 ,实际量测 ,组合量测 ); title(目标运动轨迹); xlabel(x/m); ylabel(y/m); text(x(1,1)+1,x(3,1)+5,t=1); % 位置误差figure subplot(211) plot(abs(error1_pda(i),linewidth,2); grid on title(位置误差 ); xlabel(t/s); ylabel(error-x/m); subplot(212) plot(abs(error3_pda(i),

9、linewidth,2); grid on xlabel(t/s); ylabel(error-y/m); % 速度误差figure subplot(211) plot(abs(error2_pda(i),linewidth,2); grid on title(速度误差 ); xlabel(t/s); ylabel(error-vx/m/s); subplot(212) plot(abs(error4_pda(i),linewidth,2); grid on xlabel(t/s); ylabel(error-vy/m/s); 二、pda 函数function combine_z,combin

10、e_r = pda(z_matrix, pz_matrix, z_predict, pz_predict) % 概率数据关联，杂波空间密度为泊松分布随机变量% 输入：% z_matrix ：波门内的所有有效量测值% pz_matrix ：有效量测值的误差方差阵% z_predict：预测量测值% pz_predict：预测量测值的误差方差阵% 输出：% combine_r为组合量测% combine_r：组合量测对应的协方差% 中间变量：% beta 为正确关联概率lamda=0.0004; pd=1; %检测概率，当不取1时，后面的 a计算出来都是0 pg=0.9997; %门限概率nm=s

11、ize(z_matrix); n=nm(2); % 量测数量m=nm(1); % 测量维数for i=1:1:n e(:,i)=z_matrix(:,i)-z_predict; s(:,:,i)=pz_predict+pz_matrix(:,:,i); %新息协方差 x 、r、q 互不相关条件下 % 何友 计算方法 p115 式（ 7.36 ） % a(i)=exp(-1/2)*e(i)*inv(s(i)*e(i); % bk(i)=lamda*sqrt(2*pi)*det(s(i)*(1-pd*pg)/pd; % 杨万海 p86式（ 3-5-7 ） a(i)=pd*exp(-1/2)*(e(

12、:,i)*inv(s(:,:,i)*e(:,i); bk(i)=lamda*(sqrt(2*pi)m*sqrt(det(s(:,:,i)*(1-pd); end for i=1:1:n beta_i(i)=a(i)/(bk(i) + sum(a); end % 扩充正确关联概率，使得每一维量测都有对应的关联概率beta = beta_i; for i=1:m-1 beta=beta;beta_i; end m = beta.*z_matrix; combine_z=sum(m,1); combine_z=combine_z; combine_r=0; for i=1:n combine_r =

13、 combine_r + (beta(:,i)*beta(:,i).*pz_matrix(:,:,i); end beta_i(n); end 三、kalman滤波函数function x,p,k=kalman(x_forward,p_forward,z,a,g,q,h,r) % 卡尔曼滤波%2012.2.27 % 参数说明% z-观测数据矢量% a-系统模型状态矩阵% g-系统模型噪声系数矩阵% q-系统模型噪声方差% h-量测系数矩阵% r-量测模型噪声协方差% x_forward-前次估计状态矢量% p_forward-前次估计状态协方差矩阵% x-输出估计状态矢量% p-输出估计状态协方差矩阵% 预测x_pre=a*x_forward; p_pre=a*p_forward*a+g*q*g; % 增益矩阵k=p_pre*h*inv(h*p_pre*h+r); % pzz = h*p_forward*h+ r; %s(k+1/k+1) 新息协方差% pxz = p_forward*h ; %状态与量测