clear?all;
close?all;
clc
T=10;
alpha=0.8;????????????%??加權衰減因子
window=round（1/（1-alpha））;?%??檢測機動的有效窗口長度
dt=100;???????????????%??dt=dt_x=dt_y=100
Ta=20;??????????????%??退出機動的檢測門限
Th=100;??????????????%??機動檢測門限
N=1200/T;??????????????%　采樣次數
M=50;?????????????????%　模擬次數

%　真實軌跡數據
t=T:T:400;
????xo0=2000+0*t;
????yo0=10000-15*t;
t=400+T:T:600;
????xo1=2000+0.075*（（t-400）.^2）/2;
????yo1=10000-15*400-（15*（t-400）-0.075*（（t-400）.^2）/2）;
t=600+T:T:610;
????xo2=xo1（length（xo1））+15*（t-600）;
????yo2=yo1（length（xo1））+0*t;???
t=610+T:T:660;
????xo3=xo2（length（xo2））+（15*（t-610）-0.3*（（t-610）.^2）/2）;
????yo3=yo2（length（xo2））-0.3*（（t-610）.^2）/2;
t=660+T:T:1200;
????xo4=xo3（length（xo3））+0*t;
????yo4=yo3（length（xo3））-15.*（t-660）;
???
x=[xo0xo1xo2xo3xo4];
y=[yo0yo1yo2yo3yo4];

e_x1=zeros（1N）;
e_x2=zeros（1N）;
e_y1=zeros（1N）;
e_y2=zeros（1N）;
px=zeros（1N）;
qy=zeros（1N）;
u=zeros（1N）;
u_a=zeros（1N）;

U?=?[10000500;50010000];
R=chol（U）;
for?j=1:M
????Q?=?randn（2N）;
????Noise?=?R*Q;
??for?i=1:N;
????zx（i）=x（i）+Noise（1i）;??????%??觀測數據
????zy（i）=y（i）+Noise（2i）;
????z（:i）=[zx（i）;zy（i）];
??end
??
%
X_estimate（2:）=[zx（2）（zx（2）-zx（1））/Tzy（2）（zy（2）-zy（1））/T];
X_est=X_estimate（2:）;
P_estimate=[dt^2dt^2/T00;dt^2/T（dt^2）*2/（T^2）00;00dt^2dt^2/T;00dt^2/T（dt^2）*2/（T^2）];
x1（1）=zx（1）;?y1（1）=zy（1）;?x1（2）=X_estimate（21）;?y1（2）=X_estimate（23）;
u（1）=0;?u（2）=0;?
u1=u（2）;

pp=1;%?０表示非機動，１表示機動
qq=1;
rr=1;
k=3;
while?k<=N????
????if?k<=20
????????z1=z（:k）;
????????[X_preX_estP_estimateu1]=kalmanstatic（X_estP_estimatez1ku1alpha）;
????????X_estimate（k:）=X_est;
????????X_predict（k:）=X_pre;
????????P（k:）=[P_estimate（11）P_estimate（12）P_estimate（22）P_estimate（33）P_estimate（34）P_estimate（44）];
????????x1（k）=X_estimate（k1）;
????????y1（k）=X_estimate（k3）;
????????u（k）=u1;
????????k=k+1;
????else
????????if?pp==0??%　進入非機動模型
????????????if?rr==window+1?%　由機動進入非機動模型，為防止回朔，至少遞推window＋1次
????????????????u1=0;
????????????else?
????????????end
????????????while?rr>0
????????????z1=z（:k）;
????????????[X_preX_estP_estimateu1]=kalmanstatic（X_estP_estimatez1ku1alpha）;
????????????X_estimate（k:）=X_est;
????????????X_predict（k:）=X_pre;
????????????P（k:）=[P_estimate（11）P_estimate（12）P_estimate（22）P_estimate（33）P_estimate（34）P_estimate（44）];
????????????x1（k）=X_estimate（k1）;
????????????y1（k）=X_estimate（k3）;
????????????u（k）=u1;
????????????rr=rr-1;
????????????end
????????????rr=1;
????????????if?u（k）>=Th
????????????????pp=1;qq=1;?%　“pp=1，qq=1”表示由非機動進入機動模型
????????????else
????????????end
????????????k=k+1;
????????else???%　機動模型
????????????if?qq==1???%由非機動進入機動模型，需要進行修正
????????????????k=ceil（k-window-1）;??
????????????????Xm_est=[X_estimate（k-1:）00];
????????????????Xm_pre=[X_predict（k:）00];
????????????????Pm_estimate=zeros（66）;
????????????????P=P（k-1:）;
????????????????m=0;
????????????else??????%在機動模型中進行遞推

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

?????文件???????5952??2018-05-07?20:02??vd1\vd1\CA.m

?????文件???????6001??2018-05-07?20:03??vd1\vd1\CV.m

?????文件???????2465??2018-05-07?17:25??vd1\vd1\kalmandynamic.m

?????文件????????852??2018-05-07?17:07??vd1\vd1\kalmanstatic.m

?????文件???????7153??2018-05-21?21:18??vd1\vd1\main.m

?????文件?????????38??2018-05-07?20:06??vd1\vd1\說明.txt

?????目錄??????????0??2018-05-21?21:08??vd1\vd1

?????目錄??????????0??2018-05-21?21:08??vd1

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

????????????????22461????????????????????8