%%%%本程序中的航跡圖是Beyond?the?kalman?filter:particle?filters?for?tracking
%%%%applications?中的?figure?6.2（圖像幾乎一致，只是仿真數據與書中給出的略有點變化
%%%%因為按書中數據根本得不到此圖，所以做了相應改動?）

clear
T=1;N=5000;M=30;?%%N表示粒子數，M表示采樣點個數
A=[1??0??T??0
???0??1??0??T
???0??0??1??0
???0??0??0??1];q1=1.6*10^（-3）;
B=[T^2/2???0???
????0????T^2/2
????T??????0
????0??????T];
H=[1??0?
???0??0];r1=pi/sqrt（12）;
???monte=zeros（1M）;
?for??h=1:100
???X=zeros（M4）;???Y=zeros（M4）;?C=zeros（M4）;%%X指的是目標真實的狀態Y指觀測平臺的運動狀態
????%%C是程序中的狀態
????X（1:）=[5000800-130*sin（2*pi/9）-130*cos（2*pi/9）?];??%%目標的初始狀態
????Y（1:）=[00235*sin（pi/6）-235*cos（pi/6）];%%平臺運動的初始位置
????C（1:）=X（1:）-Y（1:）;w（:1）=1/N*ones（N1）;c1=zeros（N+11）;zt=zeros（M4）;zt（1:）=X（1:）;
????W1=zeros（M4）;%%zt是濾波后得到的狀態
???w（:1）=random（‘Normal‘0.50.25N1）;z1=sum（w（:1））;p（1）=N;?m=0;
???for?j=1:M-1?????%%運行的時間
???????v=randn（13）;
???????X（j+1:）=X（j:）*A‘+q1*v（1:2）*B‘;?%%目標真實的運動狀態
???????if?12<=j||j<=16??
?????????Y（j+1:）=Y（j:）*A‘+[-3*0.5*T^2???15*0.5*T^2??-3*T???15*T];
???????else
?????????Y（j+1:）=Y（j:）*A‘;
???????end?????%%平臺的運動狀態
???????C（j+1:）=X（j+1:）-Y（j+1:）;%%程序中的運動狀態
???????%%C（j+1:）=C（j:）*A‘-[-3*0.5*T^2???15*0.5*T^2??-3*T???15*T]+q1*v（1:2）*B‘;
???????g=atan（C（j+11）/C（j+12））+r1*v（3）;?%%傳感器得到的量測值
???????n=0;
???????for???i=1:N
???????????z（1）=0;%%x的第一列表示的是粒子在時刻1的狀態，第二列表示的是粒子在時刻2的狀態，
?????%%共i行j列N行100列，N個粒子100個跟蹤時刻
???????????????if?j==1?
??????????????????L=ones（N1）*C（1:）;
??????????????????L（i:）=normrnd（（L（i:）*A‘-（Y（j+1:）-Y（j:）*A‘））diag（q1*B*B‘）‘）;
??????????????????w（ij）=w（i1）/z1;
???????????????elseif??12<=j||j<=16
??????????????????L（i:）=normrnd（（L（i:）*A‘-[-3*0.5*T^2???15*0.5*T^2??-3*T???15*T]）diag（q1*B*B‘）‘）;
???????????????else
??????????????????L（i:）=normrnd（（L（i:）*A‘）diag（q1*B*B‘）‘）;
??????????????end
???????%%%%?*normpdf（x（i:j+1）x（i:j）*A‘-（Y（j+1:）-Y（j:）*A‘）diag（q1*B*B‘）‘）
???????????w（ij+1）=w（ij）*normpdf（gatan（L（i1）/L（i2））r1）;%%w存貯的是粒子的權重，i表示粒子j表示時刻
???????end
???????z=sum（w（:j+1））;
???????for??k=1:N
???????????w（kj+1）=w（kj+1）/z;?
???????????c1（1）=0;
???????????c1（k+1）=c1（k）+w（kj+1）^2;?%%%c是累計粒子權重的平方的變量
???????????m=m+1;
???????end
???????p（j+1）=1/（c1（N+1））;?%%p是判斷需不需要進行重采樣的變量
???????if??p（j+1）<=N/3
???????????[Lw（:j+1）]=resample（Lw（:j+1））;
???????????n=n+1
???????end
???????zt（j+1:）=w（:j+1）‘*L+Y（j+1:）;%?W1（j+1:）=（zt（j+1:）-X（j+1:））.^2;%%zt是濾波后得到的狀態?W1是誤差的平方;
???end
????monte（j+1）=monte（j+1）+（zt（j+11）-X（j+11））^2+（zt（j+12）-X（j+12））^2;
?end
?monte=（monte./100）.^（1/2）;
?w（1:10M）
?p
?l=n
??t=1:1:M;
subplot（211）;
plot（X（:1）X（:2）‘r‘Y（:1）Y（:2）‘b‘）;title（‘position?‘）;
subplot（212）;
%plot（zt（:1）zt（:2）‘b‘X（:1）X（:2）‘r‘）;title（‘position?‘）;
%subplot（223）;
plot（tmonte‘k-‘）;title（‘x?position??error?versus?time?step‘）;
%subplot（224）;
%plot（tW1（:2）‘r‘）;title（‘y?position??error?versus?time?step‘）;

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

?????文件???????3194??2008-07-03?21:18??bot703.m

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

?????????????????3412????????????????????2