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')
