UKF\CKF、UKFSTF

clear all; close all; clc; R=[100,0;0,100]; q1=0.1; q2=0.02; T=1; beta=1; ro=0.95; Q=[q1*T^3/3,q1*T^2/2,0,0;q1*T^2/2,q1*T,0,0;0,0,q1*T^3/3,q1*T^2/T;0,0,q1*T^2/2,q1*T]*diag([0.1,0.01,0.1,0.01]); w=[0.2,-0.2,0.2]; N=60; for M=1:100 w1=sqrt(Q)*randn(4,N+1); v=sqrt(R)*randn(2,N+1); x(:,1)=[7000,300,1000,100]'; P=[100,0,0,0;0,10,0,0;0,0,100,0;0,0,0,10]; X(:,1)=x(:,1); for k=1:N if k<=20 oum=w(1); elseif k<=40&&k>20 oum=w(2); else oum=w(3); end F=[1,sin(oum*T)/oum,0,-(1-cos(oum*T))/oum; 0,cos(oum*T),0,-sin(oum*T); 0,(1-cos(oum*T))/oum,1,sin(oum*T)/oum; 0,sin(oum*T),0,cos(oum*T); ]; x(:,k+1)=F*x(:,k)+w1(:,k); z(:,k+1)=[2*x(1,k+1),2*x(3,k+1)]'+v(:,k); end oum=w(1); F=[1,sin(oum*T)/oum,0,-(1-cos(oum*T))/oum; 0,cos(oum*T),0,-sin(oum*T); 0,(1-cos(oum*T))/oum,1,sin(oum*T)/oum; 0,sin(oum*T),0,cos(oum*T); ]; x_ukf(:,1)=x(:,1); for k=1:N x_pre=zeros(4,1); z_pre=zeros(2,1); P_pre=zeros(4,4); P_zz=zeros(2,2); P_xz=zeros(4,2); y_pre=zeros(2,9); [kesi,oumiga]=ut1(x_ukf(:,k),P); for i=1:2*length(x(:,1))+1 kesi_pre(:,i)=F*kesi(:,i); y_pre(:,i)=[2*kesi_pre(1,i);2*kesi_pre(3,i)]; end for i=1:2*length(x(:,1))+1 x_pre=x_pre+oumiga(i)*kesi_pre(:,i); z_pre=z_pre+oumiga(i)*y_pre(:,i); end for i=1:2*length(x(:,1))+1 P_pre=P_pre+oumiga(i)*(kesi_pre(:,i)-x_pre)*(kesi_pre(:,i)-x_pre)'; P_zz=P_zz+oumiga(i)*(y_pre(:,i)-z_pre)*(y_pre(:,i)-z_pre)'; P_xz=P_xz+oumiga(i)*(kesi_pre(:,i)-x_pre)*(y_pre(:,i)-z_pre)'; end P_pre=P_pre+Q; P_zz=P_zz+R; K=P_xz/(P_zz); x_ukf(:,k+1)=x_pre+K*(z(:,k+1)-z_pre); P=P_pre-K*P_xz'; jfx_ukf=abs(x_ukf(1,k+1)-x(1,k+1)); jfxv_ukf=abs(x_ukf(2,k+1)-x(2,k+1)); jfy_ukf=abs(x_ukf(3,k+1)-x(3,k+1)); jfyv_ukf=abs(x_ukf(4,k+1)-x(4,k+1)); hjfx_ukf(:,k+1)=jfx_ukf; hjfxv_ukf(:,k+1)=jfxv_ukf; hjfy_ukf(:,k+1)=jfy_ukf; hjfyv_ukf(:,k+1)=jfyv_ukf; end zx_ukf(M,:)=hjfx_ukf; zxv_ukf(M,:)=hjfxv_ukf; zy_ukf(M,:)=hjfy_ukf; zyv_ukf(M,:)=hjfyv_ukf; n=length(x(:,1)); P=[100,0,0,0;0,10,0,0;0,0,100,0;0,0,0,10]; ckf_kesi1=sqrt(n)*[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]; ckf_kesi2=-sqrt(n)*[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]; ckf_kesi=[ckf_kesi1,ckf_kesi2] x_ckf(:,1)=x(:,1); for k=1:N x_pre=zeros(4,1); P_xz=zeros(4,2); P_zz=zeros(2,2); P_pre=zeros(4,4); y_pre=zeros(4,8); kesi=zeros(4,8); kesi_pre=zeros(4,8) z_pre=zeros(2,1); S=chol(P); for i=1:2*n kesi(:,i)=S*ckf_kesi(:,i)+x_ckf(:,k); kesi_pre(:,i)=F*kesi(:,i); x_pre=1/(2*n)*kesi_pre(:,i)+x_pre; end for i=1:2*n P_pre=1/(2*n)*kesi_pre(:,i)*kesi_pre(:,i)'+P_pre; end P_pre=P_pre-x_pre*x_pre'+Q; S_pre=chol(P_pre); for i=1:2*n y_pre(:,i)=S_pre*ckf_kesi(:,i)+x_pre; zz_pre(:,i)=[2*y_pre(1,i),2*y_pre(3,i)]'; z_pre=1/(2*n)*zz_pre(:,i)+z_pre; end for i=1:2*n P_zz=1/(2*n)* zz_pre(:,i)* zz_pre(:,i)'+P_zz; P_xz=1/(2*n)*kesi_pre(:,i)*zz_pre(:,i)'+P_xz end P_zz=P_zz-z_pre*z_pre'+R; P_xz= P_xz-x_pre*z_pre'; K=P_xz/P_zz; x_ckf(:,k+1)=x_pre+K*(z(:,k+1)-z_pre); P=P_pre-K*P_zz*K'; jfx_ckf=abs(x_ckf(1,k+1)-x(1,k+1)); jfxv_ckf=abs(x_ckf(2,k+1)-x(2,k+1)); jfy_ckf=abs(x_ckf(3,k+1)-x(3,k+1)); jfyv_ckf=abs(x_ckf(4,k+1)-x(4,k+1)); hjfx_ckf(:,k+1)=jfx_ckf; hjfxv_ckf(:,k+1)=jfxv_ckf; hjfy_ckf(:,k+1)=jfy_ckf; hjfyv_ckf(:,k+1)=jfyv_ckf; end zx_ckf(M,:)=hjfx_ckf; zxv_ckf(M,:)=hjfxv_ckf; zy_ckf(M,:)=hjfy_ckf; zyv_ckf(M,:)=hjfyv_ckf; x_stfckf(:,1)=x(:,1); P=[100,0,0,0;0,1,0,0;0,0,100,0;0,0,0,1]; for k=1:N x_pre=zeros(4,1); P_xz=zeros(4,2); P_zz=zeros(2,2); P_pre=zeros(4,4); y_pre=zeros(4,8); kesi=zeros(4,8); kesi_pre=zeros(4,8) z_pre=zeros(2,1); S=chol(P); for i=1:2*n kesi(:,i)=S*ckf_kesi(:,i)+x_stfckf(:,k); kesi_pre(:,i)=F*kesi(:,i); x_pre=1/(2*n)*kesi_pre(:,i)+x_pre; end for i=1:2*n P_pre=1/(2*n)*kesi_pre(:,i)*kesi_pre(:,i)'+P_pre; end P_pre=P_pre-x_pre*x_pre'+Q; a=z(:,k+1)-z_pre; H=[2,0,0,0;0,0,2,0]; if k==1 S0=0; elseif k>=2 S0=(ro*S0+a*a')/(1+ro); end [lmd,lmd0]=xiaojian(R,Q,F,H,beta,S0,P); P_pre=lmd*P_pre; S_pre=chol(P_pre); for i=1:2*n y_pre(:,i)=S_pre*ckf_kesi(:,i)+x_pre; zz_pre(:,i)=[2*y_pre(1,i),2*y_pre(3,i)]'; z_pre=1/(2*n)*zz_pre(:,i)+z_pre; end for i=1:2*n P_zz=1/(2*n)* zz_pre(:,i)* zz_pre(:,i)'+P_zz; P_xz=1/(2*n)*kesi_pre(:,i)*zz_pre(:,i)'+P_xz end P_zz=P_zz-z_pre*z_pre'+R; P_xz= P_xz-x_pre*z_pre'; K=P_xz/P_zz; x_stfckf(:,k+1)=x_pre+K*(z(:,k+1)-z_pre); P=P_pre-K*P_zz*K'; jfx_stfckf=abs(x_stfckf(1,k+1)-x(1,k+1)); jfxv_stfckf=abs(x_stfckf(2,k+1)-x(2,k+1)); jfy_stfckf=abs(x_stfckf(3,k+1)-x(3,k+1)); jfyv_stfckf=abs(x_stfckf(4,k+1)-x(4,k+1)); hjfx_stfckf(:,k+1)=jfx_stfckf; hjfxv_stfckf(:,k+1)=jfxv_stfckf; hjfy_stfckf(:,k+1)=jfy_stfckf; hjfyv_stfckf(:,k+1)=jfyv_stfckf; end zx_stfckf(M,:)=hjfx_stfckf; zxv_stfckf(M,:)=hjfxv_stfckf; zy_stfckf(M,:)=hjfy_stfckf; zyv_stfckf(M,:)=hjfyv_stfckf; end t=0:N [vx_ukf,vy_ukf,posx_ukf,posy_ukf,pos_ukf,v_ukf]=pjabs(zx_ukf,zy_ukf,zxv_ukf,zyv_ukf); [jpos_ukf,jv_ukf,jposx_ukf,jposy_ukf,jvx_ukf,jvy_ukf]=sz(vx_ukf,vy_ukf,posx_ukf,posy_ukf,pos_ukf,v_ukf); [vx_ckf,vy_ckf,posx_ckf,posy_ckf,pos_ckf,v_ckf]=pjabs(zx_ckf,zy_ckf,zxv_ckf,zyv_ckf); [jpos_ckf,jv_ckf,jposx_ckf,jposy_ckf,jvx_ckf,jvy_ckf]=sz(vx_ckf,vy_ckf,posx_ckf,posy_ckf,pos_ckf,v_ckf); [vx_stfckf,vy_stfckf,posx_stfckf,posy_stfckf,pos_stfckf,v_stfckf]=pjabs(zx_stfckf,zy_stfckf,zxv_stfckf,zyv_stfckf); [jpos_stfckf,jv_stfckf,jposx_stfckf,jposy_stfckf,jvx_stfckf,jvy_stfckf]=sz(vx_stfckf,vy_stfckf,posx_stfckf,posy_stfckf,pos_stfckf,v_stfckf); figure(2); plot(x(1,:),x(3,:),'k'); xlabel('X-Dis'); ylabel('Y-Dis'); legend('Target trajectory'); figure(3); plot(t,posx_ukf,'k-',t,posx_ckf,'k-+',t,posx_stfckf,'k:'); xlabel('Times') ylabel('X-Dis RMSE(m)') legend('UKF','CKF','STF-CKF') figure(4); plot(t,posy_ukf,'k-',t,posy_ckf,'k-+',t,posy_stfckf,'k:'); xlabel('Times') ylabel('Y-Dis RMSE(m)') legend('UKF','CKF','STF-CKF') figure(7); plot(t,pos_ukf,'k-',t,pos_ckf,'k-+',t,pos_stfckf,'k:'); xlabel('跟踪步数') ylabel('位移 RMSE(m)') legend('UKF','CKF','STF-CKF') figure(8); plot(t,vx_ukf,'k-',t,vx_ckf,'k-+',t,vx_stfckf,'k:'); xlabel('跟踪步数') ylabel('X方向速度 RMSE(m)') legend('UKF','CKF','STF-CKF') figure(9); plot(t,vy_ukf,'k-',t,vy_ckf,'k-+',t,vy_stfckf,'k:'); xlabel('跟踪步数') ylabel('Y方向速度 RMSE(m)') legend('UKF','CKF','STF-CKF') figure(10); plot(t,v_ukf,'k-',t,v_ckf,'k-+',t,v_stfckf,'k:'); xlabel('跟踪步数') ylabel('速度 RMSE(m)') legend('UKF','CKF','STF-CKF')