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;%實際模型：cvh=1 0 0 0;0 0

2、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:simtimex(:,i)=a*x(:,i-1);%真實狀態vk=sqrt(r)*randn;sqrt(r)*randn;zk(:,i)=h*x(:,i)+vk;%生成量測值end%*%

3、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 i=1:1:simtime%*%產生雜波%*%量測確認區域面

4、積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; for j=1:ncz_matrix(:,j) = xi(j);yi(j); end z_m

5、atrix(:,nc+1)=zk(:,i); pz_matrix = cat(3);for j=1:1:ncpz_matrix = cat(3,pz_matrix,q,0;0,q); endpz_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_predict) ; % pda z_pda(:,i) = combine_z ;%*%卡爾曼濾波%* p=c

6、ombine_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);%pkk(1,1);error2_pda(i)=ey_pda(i)-x(3,i);%pkk(2,2);error

7、3_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;figureplot(x(1,i),x(3,i),'-','linewidth',2);%真實值grid on; hold onplot(ex_pda(1,i),ey_pda(1,i),'r-','linewidth',2);%濾波值plot(zk(1,i),zk(2,i),'*');%實際測量值plot(z_p

8、da(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 ontitle(

9、'位置誤差 '); xlabel('t/s'); ylabel('error-x/m'); subplot(212)plot(abs(error3_pda(i),'linewidth',2); grid on xlabel('t/s'); ylabel('error-y/m');%速度誤差figuresubplot(211) plot(abs(error2_pda(i),'linewidth',2); grid ontitle('速度誤差 '); xlabel(

10、9;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,combine_r = pda(z_matrix, pz_matrix, z_predict, pz_predict)% 概率數據關聯，雜波空間密度為泊松分布隨機變量% 輸入：% z_matrix：波門內的所有有效量

11、測值% pz_matrix ：有效量測值的誤差方差陣% z_predict：預測量測值% pz_predict：預測量測值的誤差方差陣% 輸出：% combine_r為組合量測% combine_r：組合量測對應的協方差% 中間變量：% beta 為正確關聯概率lamda=0.0004;pd=1;%檢測概率，當不取1時，后面的 a計算出來都是0 pg=0.9997;%門限概率nm=size(z_matrix); n=nm(2);%量測數量m=nm(1); %測量維數for i=1:1:ne(:,i)=z_matrix(:,i)-z_predict;s(:,:,i)=pz_predict+pz_

12、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(:,i)'*inv(s(:,:,i)*e(:,i);bk(i)=lamda*(sqrt(2*pi)m*sqrt(det(s(:,:,i)*(1-pd); endfor i=1:1:nbeta_i(i)=a(i)/(bk(i)

13、+ sum(a); end% 擴充正確關聯概率，使得每一維量測都有對應的關聯概率beta = beta_i; for i=1:m-1beta=beta;beta_i; endm = beta.*z_matrix;combine_z=sum(m',1);combine_z=combine_z'combine_r=0;for i=1:ncombine_r = combine_r + (beta(:,i)*beta(:,i)').*pz_matrix(:,:,i); endbeta_i(n); end三、kalman濾波函數function x,p,k=kalman(x_fo

14、rward,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;% pxz = p_forward*h' ;% k = p_forward*h