function?[X_PDA_updateP_PDA_update]=PDA（X_predictP_predictZ_PDAR）
gama=16;?
lamda=0.000004;???????????????????????????????????????????%單位面積虛假量測(cè)數(shù)
Pd=1;???????????????????????????????????????????????????%檢測(cè)概率，當(dāng)不取1時(shí)，后面的a計(jì)算出來(lái)都是0
Pg=0.9997;???????
H=[1?0?0?0;
????0?0?1?0];?????????????????????%量測(cè)矩陣

nc=size（Z_PDA2）-1;

S=H*P_predict*H‘+R;
K=P_predict*H‘/S;

j=1;
for?i=1:nc+1
????v（:i）=?Z_PDA（:i）-H*X_predict;
????d_squa（i）=v（:i）‘/S*v（:i）;
????if?d_squa（i）<=gama
???????gate_meas（:j）=Z_PDA（:i）;
???????j=j+1;
????end
end
if?j==1??%?no?measurement?falls?in?the?gate
???X_PDA_update=X_predict;%?using?the?predicted?state?as?the?renewed?state
???P_PDA_update=P_predict;
else
????nc=size（gate_meas2）-1;?%?the?number?of?measurements

????bb=lamda*sqrt（2*pi*det（S））*（1-Pd*Pg）/Pd;
????for?j=1:1:nc+1?????????????????????????????????????????%關(guān)聯(lián)概率的計(jì)算
?????????vgate（:j）=?gate_meas（:j）-H*X_predict;
?????????e（j）=exp（-0.5*vgate（:j）‘/S*vgate（:j））;?????
????end

????beta=e./（bb+sum（e））;
????beta0=bb/（bb+sum（e））;

??????%?更新值
??????PP=0;vv=0;
??????for?j=1:nc+1
??????????vv=vv+beta（j）.*vgate（:j）;
??????????PP=PP+beta（j）.*vgate（:j）*vgate（:j）‘;
??????end
??????X_PDA_update=X_predict+K*vv;

??????P_tilt=K*（PP-vv*vv‘）*K‘;
??????P_PDA_update=beta0*P_predict+（1-beta0）*（eye（4）-K*H）*P_predict+P_tilt;
end?