机动卡尔曼算法_扩展卡尔曼滤波的一种_kalmandynamic_matlab

T=2; alpha=0.8; % 加权衰减因子 window=1/(1-alpha); % 检测机动的有效窗口长度 dt=100; % dt=dt_x=dt_y=100 Th=25; % 机动检测门限 Ta=9.49; % 退出机动的检测门限 N=800/T; %　采样次数 M=50; %　模拟次数 %　真实轨迹数据 t=2:2:400; xo0=2000+0*t; yo0=10000-15*t; t=402:2:600; xo1=2000+0.075*((t-400).^2)/2; yo1=10000-15*400-(15*(t-400)-0.075*((t-400).^2)/2); t=602:2:610 ; xo2=xo1(100)+15*(t-600); yo2=yo1(100)+0*t; t=612:2:660; xo3=xo2(5)+(15*(t-610)-0.3*((t-610).^2)/2); yo3=yo2(5)-0.3*((t-610).^2)/2; t=662:2:800; xo4=xo3(25)+0*t; yo4=yo3(25)-15*(t-660); x=[xo0,xo1,xo2,xo3,xo4]; y=[yo0,yo1,yo2,yo3,yo4]; e_x1=zeros(1,N); e_x2=zeros(1,N); e_y1=zeros(1,N); e_y2=zeros(1,N); px=zeros(1,N); qy=zeros(1,N); u=zeros(1,N); u_a=zeros(1,N); for j=1:M no1=100*randn(1,N); % 随机白噪 no2=100*randn(1,N); for i=1:N; zx(i)=x(i)+no1(i); % 观测数据 zy(i)=y(i)+no2(i); z(:,i)=[zx(i);zy(i)]; end % X_estimate(2,:)=[zx(2),(zx(2)-zx(1))/T,zy(2),(zy(2)-zy(1))/T]; X_est=X_estimate(2,:); P_estimate=[dt^2,dt^2/T,0,0;dt^2/T,(dt^2)*2/(T^2),0,0;0,0,dt^2,dt^2/T;0,0,dt^2/T,(dt^2)*2/(T^2)]; x1(1)=zx(1); y1(1)=zy(1); x1(2)=X_estimate(2,1); y1(2)=X_estimate(2,3); u(1)=0; u(2)=0; u1=u(2); pp=0;% ０表示非机动，１表示机动 qq=0; rr=1; k=3; while k<=N if k<=20 z1=z(:,k); [X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1); X_estimate(k,:)=X_est; X_predict(k,:)=X_pre; P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)]; x1(k)=X_estimate(k,1); y1(k)=X_estimate(k,3); 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_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1); X_estimate(k,:)=X_est; X_predict(k,:)=X_pre; P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)]; x1(k)=X_estimate(k,1); y1(k)=X_estimate(k,3); 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=k-window-1; Xm_est=[X_estimate(k-1,:),0,0]; Xm_pre=[X_predict(k,:),0,0]; Pm_estimate=zeros(6,6); P=P(k-1,:); m=0; else %在机动模型中进行递推 Xm_est=Xm_estimate(k-1,:); end z1=z(:,k); [Xm_est,Pm_estimate,ua1,qq,m]=kalmandynamic(Xm_pre,Xm_est,Pm_estimate,z1,k,P,qq,m); Xm_estimate(k,:)=Xm_est; x1(k)=Xm_estimate(k,1); y1(k)=Xm_estimate(k,3); ua(k)=ua1; if ua1<Ta %　进入非机动模型，降维，标志 pp＝０ X_est=Xm_estimate(k,1:4); P_estimate=Pm_estimate(1:4,1:4); pp=0; rr=window+1; else end k=k+1; end end end for j=1:N px(1,j)=x1(1,j)+px(1,j); %　迭加每次估计的数据 qy(1,j)=y1(1,j)+qy(1,j); e_x1(j)=(x1(j)-x(j))+e_x1(j); e_y1(j)=(y1(j)-y(j))+e_y1(j); e_x2(j)=((x1(j)-x(j))^2)+e_x2(j); e_y2(j)=((y1(j)-y(j))^2)+e_y2(j); end end for k=1:N px(1,k)=px(1,k)/M; qy(1,k)=qy(1,k)/M; e_x(k)=e_x1(k)/M; ex(k)=sqrt(e_x2(k)/M-e_x(k)^2); e_y(k)=e_y1(k)/M; ey(k)=sqrt(e_y2(k)/M-e_y(k)^2); end figure(1); plot(x,y);axis([1500 5000 -2000 12000]); figure(2); plot(x,y,'b-',zx,zy,'k:',px,qy,'r'); legend('真实轨迹','观测轨迹','50次滤波轨迹'); figure(3); plot(x,y,'k',x1,y1,'r'); legend('真实轨迹','一次滤波轨迹'); figure(4); subplot(2,2,1),plot(e_x); title('X坐标 滤波误差均值曲线'); subplot(2,2,2),plot(e_y); title('Y坐标 滤波误差均值曲线'); subplot(2,2,3),plot(ex); title('X坐标 滤波误差标准差曲线'); subplot(2,2,4),plot(ey); title('Y坐标 滤波误差标准差曲线');