clc;clear
format?long;close?all;
for?l=1:6
T=2;x（:1）=[10?1?10?-15]‘;???????%初始狀態
F=[1?T?0?0;0?1?0?0;0?0?1?T;0?0?0?1];H=[1?0?0?0;0?0?1?0];R=[400?0;0?400];
for?k=1:99
????x（:k+1）=F*x（:k）;
????Z（:k）=H*x（:k+1）+randn*[20?20]‘;????
end
X（:1）=[Z（11）?（Z（11）-x（11））/T?Z（21）?（Z（21）-x（31））/T]‘;????%系統初始狀態
%初始協方差矩陣
P（:1:4）=[R（11）?R（11）/T?R（12）?R（12）/T;R（11）/T?2*R（11）/（T^2）?R（12）/T?2*R（12）/（T^2）;
????R（12）?R（12）/T?R（22）?R（22）/T;R（12）/T?2*R（12）/（T^2）?R（22）/T?2*R（22）/（T^2）];
X_temp=X（:1）;
ZZ（:1）=H*X（:1）;
P0=P（:1:4）;
tao=[0.5*T*T?0;T?0;0?0.5*T*T;0?T];??????????????%過程噪聲分布矩陣
Q=[16?0;0?9];????????????????????????????????%過程噪聲
for?k=1:9
????X（:k+1）=F*X_temp;??????????????????????????%一步狀態預測?
????ZZ（:k+1）=H*X（:k+1）;
????P（:（4*k+1）:（4*k+4））=F*P0*F‘+tao*Q*tao‘;????%一步預測協方差????
????S（:（（2*k-1）:（2*k）））=H*P（:（4*k+1）:（4*k+4））*H‘+R;??????????????%新息
????s=S（:（（2*k-1）:（2*k）））;
????K=P（:（4*k+1）:（4*k+4））*H‘*s^（-1）;???????????%增益
????X_temp=X（:k+1）+K*[Z（:k）-H*X（:k+1）];???%狀態更新
????P0=（eye（4）-K*H）*P（:（4*k+1）:（4*k+4））*（eye（4）+K*H）‘-K*R*K‘;??????%協方差更新
end
gama=16;lamda=0.00002;???????????????
PD=1;PG=0.9997;?????????????????????%檢測概率與門概率
for?k=10:99
????X（:k+1）=F*X_temp;??????????????????????????%一步狀態預測
????ZZ（:k+1）=H*X（:k+1）;
????P（:（4*k+1）:（4*k+4））=F*P0*F‘+tao*Q*tao‘;????%一步預測協方差????
????S（:（（2*k-1）:（2*k）））=H*P（:（4*k+1）:（4*k+4））*H‘+R;??????????????%新息
????s=S（:（（2*k-1）:（2*k）））;
????K=P（:（4*k+1）:（4*k+4））*H‘*s^（-1）;???????????%增益????
????Av=pi*gama*sqrt（det（s））;????????%二維正方形波門面積
????nc=round（10*Av*lamda+1）;????????%虛假量測總數
????j=0;
????q=sqrt（10*Av）/2;
????z_temp=Z（:k）;
????for?i=1:nc
????????a=Z（1k）-q;b=Z（1k）+q;
????????c=Z（2k）-q;d=Z（2k）+q;
????????xx（i）=a+（b-a）*rand;
????????y（i）=c+（d-c）*rand;
????????zz_temp=[xx（i）?y（i）]‘;??????%虛假量測
????????z_temp=[z_temp?zz_temp];????%虛假量測和真實量測在內的所有量測????
????end
????for?i=1:nc+1
????????v_temp（:i）=z_temp（:i）-ZZ（:k+1）;
????????%判斷落入波門內的量測
????????if?（v_temp（:i）‘*s^（-1）*v_temp（:i））<=gama
????????????j=j+1;
????????????z（:j）=z_temp（:i）;
????????????v（:j）=z（:j）-ZZ（:k+1）;
????????end
????end
????sum=0;b0=lamda*sqrt（det（2*pi*s））*（1-PD*PG）/PD;
????for?i=1:j
????????e（i）=exp（-0.5*v（:i）‘*s^（-1）*v（:i））;????????
????????sum=sum+e（i）;
????end
????beta0=b0/（b0+sum）;????sum1=[0;0];sum2=[0?0;0?0];
????for?i=1:j
????????beta（i）=e（i）/（b0+sum）;
????????sum1=sum1+beta（i）*v（:i）;???????????????%組合新息
????????sum2=sum2+beta（i）*v（:i）*v（:i）‘;
????end
????X_temp=X（:k+1）+K*sum1;???%狀態更新
????P0=beta0*P（:（4*k+1）:（4*k+4））+（1-beta0）*（eye（4）-K*H）*P（:（4*k+1）:（4*k+4））+K*（sum2-sum1*sum1‘）*K‘;??????%協方差更新
end
figure（l）;plot（X（1:）X（3:）‘--r‘）;grid?on;?hold?on;
plot（x（1:）x（3:）‘.k‘）;hold?on;
plot（Z（1:）Z（2:）‘xb‘）;xlabel（‘X‘）;ylabel（‘Y‘）;hold?on;
clear;
T=2;x（:1）=[10?1?10?-15]‘;???????%初始狀態
F=[1?T?0?0;0?1?0?0;0?0?1?T;0?0?0?1];H=[1?0?0?0;0?0?1?0];
R=[400?0;0?400];
for?k=1:99
????x（:k+1）=F*x（:k）;
????Z（:k）=H*x（:

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????5707??2014-05-06?07:54??NNSF.m

-----------?---------??----------?-----??----

?????????????????5707????????????????????1