PDA算法Matlab程序（精編版）

1、一、測(cè)試程序% pda-fa算法實(shí)現(xiàn)% 何友雷達(dá)數(shù)據(jù)處理及應(yīng)用p116 % 二維空間勻速直線運(yùn)動(dòng)，狀態(tài)向量為x=x,vx,y,vy % x1=x0+vxt % y1=y0+vyt % 仿真：% 1、改變虛假量測(cè)數(shù)量nc：公式求取、手動(dòng)設(shè)置% 2、改變量測(cè)噪聲r(shí)=r 0; 0 r，即 r % 3、改變虛假量測(cè)位置q，偏離真實(shí)位置的程度% 4、關(guān)聯(lián)概率計(jì)算clc; clear; close all; %* % 參數(shù)設(shè)置%* i=eye(4); t = 1; %采樣間隔simtime = 100 ; %仿真步數(shù)a=1 t 0 0;0 1 0 0;0 0 1 t;0 0 0 1; %實(shí)際模型： cv

2、h=1 0 0 0;0 0 1 0; %測(cè)量模型q=0; %實(shí)際過(guò)程噪聲g = t2/2 0; t 0; 0 t2/2; 0 t; %噪聲加權(quán)矩陣r=200; r=r 0; 0 r; %量測(cè)噪聲x0=200;0;10000;-15; %初始狀態(tài)x(:,1)=x0; vk=sqrt(r)*randn;sqrt(r)*randn; zk(:,1)=h*x(:,1)+vk; gama=16; lamda=0.0004; %* % 量測(cè)生成%* for i=2:1:simtime x(:,i)=a*x(:,i-1); % 真實(shí)狀態(tài) vk=sqrt(r)*randn;sqrt(r)*randn; zk(

3、:,i)=h*x(:,i)+vk; %生成量測(cè)值end %* % pda初始化%* xk_pda=200;0;10100;-16; %初始狀態(tài)、與實(shí)際值略有差別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; %初始協(xié)方差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 %* % 產(chǎn)生雜波 %* % 量測(cè)確認(rèn)區(qū)域面積 sk=h*p_pre*h+ p; av=pi*gama*sqrt(det(sk); % 準(zhǔn)備生成雜波數(shù)目 nc=floor(10*av*lamda+1);%設(shè)置雜波數(shù)量 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個(gè)虛假量測(cè) 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關(guān)聯(lián) %* 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; % 預(yù)測(cè) x_pre=a*xkk; p_pre=a*pkk*a+g*q*g; %出各個(gè)狀態(tài)值 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); %真實(shí)值grid on; hold on plot(ex_pda(1,i),ey_pda(1,i),r-,linewidth,2); %濾波值plot(zk(1,i),zk(2,i),*

8、); %實(shí)際測(cè)量值plot(z_pda(1,i),z_pda(2,i),o); %組合測(cè)量值legend(真實(shí)值 ,濾波值 ,實(shí)際量測(cè) ,組合量測(cè) ); title(目標(biāo)運(yùn)動(dòng)軌跡); 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 函數(shù)function combine_z,combin

10、e_r = pda(z_matrix, pz_matrix, z_predict, pz_predict) % 概率數(shù)據(jù)關(guān)聯(lián)，雜波空間密度為泊松分布隨機(jī)變量% 輸入：% z_matrix ：波門內(nèi)的所有有效量測(cè)值% pz_matrix ：有效量測(cè)值的誤差方差陣% z_predict：預(yù)測(cè)量測(cè)值% pz_predict：預(yù)測(cè)量測(cè)值的誤差方差陣% 輸出：% combine_r為組合量測(cè)% combine_r：組合量測(cè)對(duì)應(yīng)的協(xié)方差% 中間變量：% beta 為正確關(guān)聯(lián)概率lamda=0.0004; pd=1; %檢測(cè)概率，當(dāng)不取1時(shí)，后面的 a計(jì)算出來(lái)都是0 pg=0.9997; %門限概率nm=s

11、ize(z_matrix); n=nm(2); % 量測(cè)數(shù)量m=nm(1); % 測(cè)量維數(shù)for i=1:1:n e(:,i)=z_matrix(:,i)-z_predict; s(:,:,i)=pz_predict+pz_matrix(:,:,i); %新息協(xié)方差 x 、r、q 互不相關(guān)條件下 % 何友 計(jì)算方法 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; % 楊萬(wàn)海 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 % 擴(kuò)充正確關(guān)聯(lián)概率，使得每一維量測(cè)都有對(duì)應(yīng)的關(guān)聯(lián)概率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濾波函數(shù)function x,p,k=kalman(x_forward,p_forward,z,a,g,q,h,r) % 卡爾曼濾波%2012.2.27 % 參數(shù)說(shuō)明% z-觀測(cè)數(shù)據(jù)矢量% a-系統(tǒng)模型狀態(tài)矩陣% g-系統(tǒng)模型噪聲系數(shù)矩陣% q-系統(tǒng)模型噪聲方差% h-量測(cè)系數(shù)矩陣% r-量測(cè)模型噪聲協(xié)方差% x_forward-前次估計(jì)狀態(tài)矢量% p_forward-前次估計(jì)狀態(tài)協(xié)方差矩陣% x-輸出估計(jì)狀態(tài)矢量% p-輸出估計(jì)狀態(tài)協(xié)方差矩陣% 預(yù)測(cè)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) 新息協(xié)方差% pxz = p_forward*h ; %狀態(tài)與量測(cè)